[发明专利]一种简易快速的卡尔曼滤波方法

权利要求书

1.一种简易快速的卡尔曼滤波方法，其特征在于，包括如下步骤：

步骤(1)：设置过程噪声方差Q和观测噪声方差R：针对不同种类的目标设置对应的过程噪声方差Q和观测噪声方差R；

步骤(2)：计算增益K：根据卡尔曼滤波的递推计算公式，利用转移矩阵Φ，观测矩阵H，以及过程噪声方差Q、观测噪声方差R计算得到增益K；

步骤(3)：修正丢点情况下的增益K：分析滤波器稳定后发生丢点时不同的丢点数以及Q/R的值对增益的影响并得到滤波器实时增益与滤波器稳定增益的比值，利用该比值对增益K进行修正；

步骤(4)：记录不同种类的目标所对应的增益K，构建对应的滤波器：在数据率T固定的情况下分别记录滤波稳定前的所有增益K以及滤波稳定后的增益K以及不同丢点数情况下经过修正的增益K，构建针对不同种类对象的滤波器。

2.根据权利要求1所述的方法，其特征在于，所述步骤(2)中计算增益K用到的目标状态运动方程表示为：

X_k+1＝ΦX_k+W_k

其中，X_k、X_k+1为状态向量，Φ为状态向量的转移矩阵，w_k为均值为零，方差为Q的高斯白噪声；

观测方程表示为：

z_k＝HX_k+v_k

其中，H为观测矩阵，v_k为均值为零、方差为R的高斯白噪声。

3.根据权利要求2所述的方法，其特征在于，所述步骤(2)中的卡尔曼滤波的递推计算公式如下：

预测状态X_k+1|k＝ΦX_k|k+w_k

预测协方差P_k+1|k＝ΦP_k|kΦ^T+Q

新息协方差S＝HP_k+1|kH^T+R

卡尔曼增益K＝P_k+1|kH^TS^-1

估计值状态X_k+1|k+1＝X_k+1|k+K(z_k-HX_k+1|k)

估计协方差P_k+1|k+1＝(1-KH)P_k+1|k。