CN111865268A - 一种简易快速的卡尔曼滤波方法 - Google Patents

Abstract

本发明公开一种简易快速的卡尔曼滤波方法，针对不同种类的目标设置对应的状态转移过程噪声方差Q和传感器观测噪声方差R，根据卡尔曼滤波的递推公式，利用转移矩阵，观测矩阵，过程噪声方差，观测噪声方差计算得到增益，通过分析滤波器稳定后发生丢点时不同的丢点数以及Q/R的值对增益的影响得到相应的比值，利用该比值对增益进行修正，最后在不同数据率的情况下分别记录滤波稳定前的所有增益以及滤波稳定后的增益以及不同丢点数情况下经过修正的增益，构建针对不同种类对象的滤波器。解决了传统方法在多目标高数据率跟踪中由于计算机的运算速度限制而影响系统的实时性和由于丢点后卡尔曼增益的变化会降低滤波精度以及可能造成跟踪目标丢批的问题。

Description

一种简易快速的卡尔曼滤波方法

技术领域

本发明属于目标状态估计领域，具体涉及一种简易快速的卡尔曼滤波方法。

背景技术

卡尔曼滤波(Kalman filtering，KF)理论目前广泛应用于航空航天、导航定位、目标跟踪、控制等各种领域，它是一种线性最优最小方差估计器。在雷达的目标跟踪数据处理中，普遍地采用了卡尔曼滤波方法。

该方法中，某个时刻的滤波值相当于该时刻的预测值与测量值的加权和，其权值由卡尔曼增益决定。计算卡尔曼增益，需要先计算预测协方差、新息协方差以及估计协方差，这些数据的计算量占了整个滤波计算量的70％左右。

在多目标跟踪中，由于计算机的运算速度有限，使用标准卡尔曼滤波方法会影响系统的实时性，尤其是在多目标高数据率的情况下。为了减少计算量，经常采用简化常增益卡尔曼滤波方法。典型的方法有α-β、α-β-γ滤波，分段常增益滤波等。其中，α-β、α-β-γ滤波的整个过程增益都为常值，性能较差；分段常增益滤波将滤波过程分为几个阶段，每个阶段采用不同的增益值，到达稳态后，增益就保持不变，其中增益值的设置由离线的仿真计算来获得。分段常增益滤波对标准卡尔曼滤波进行了较好的近似，性能接近标准卡尔曼滤波。这些简化方法共同点是滤波器达到稳定后采用常增益。此时不再实时计算协方差矩阵，计算量大为减小，易于工程实现。

专利方面，申请号为CN201810359381.0的中国发明专利申请一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法；申请号为CN201710190791.2的中国发明专利申请一种用于目标跟踪的混合平方根容积卡尔曼滤波方法；申请号为CN201510284296.9的中国发明专利申请一种改进的去偏坐标转换卡尔曼滤波方法；申请号为CN201410134666.6的中国发明专利申请一种自适应高阶容积卡尔曼滤波方法；申请号为CN201710436120.X的中国发明专利申请一种目标跟踪中带遗忘因子的自适应迭代容积卡尔曼滤波方法。

尽管上述方法使卡尔曼滤波的效果得到提升，但在实际应用时，并不能在每个采样时刻都能获得目标观测值，这会对滤波增益产生影响，不仅降低滤波精度，还可能会造成丢批，而这种影响在常增益算法设计时往往被忽略。

发明内容

本发明的目的在于提供一种简易快速的卡尔曼滤波方法，解决了传统卡尔曼滤波方法在多目标高数据率跟踪中由于计算机的运算速度限制而影响系统的实时性和由于丢点后卡尔曼增益的变化会降低滤波精度以及可能造成跟踪目标丢批的问题。

实现本发明目的的技术解决方案为：一种简易快速的卡尔曼滤波方法，包括如下步骤：

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

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

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

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

本发明与现有技术相比，其显著优点在于：

(1)本发明的方法同时记录了卡尔曼滤波稳定前及稳定后的增益K，减少实时运算量的同时不降低滤波性能，比较适用于实时系统。

(2)本发明有效的解决了滤波器稳定后发生丢点时滤波精度降低以及可能造成丢批的问题；通过分析不同的丢点数以及Q/R的值对增益的影响，得到相应的比值，并利用该比值对增益K进行修正。

附图说明

图1为距离通道的卡尔曼增益变化仿真图。

图2为角度通道的卡尔曼增益变化仿真图。

图3为距离通道丢点数对距离增益的影响仿真图。

图4为距离通道丢点数对距离速度增益的影响仿真图。

图5为角度通道丢点数对角度增益的影响仿真图。

图6为角度通道丢点数对角度速度增益的影响仿真图。

具体实施方式

下面结合附图对本发明作进一步详细描述。

本发明提供了一种简易快速的卡尔曼滤波方法。当转移矩阵Φ，观测矩阵H，以及过程噪声方差Q、观测噪声方差R为常数矩阵，数据率T保持不变时，卡尔曼滤波会渐近稳定，利用该特点，针对不同种类的目标，设置对应的Q和R，在速率T固定的情况下分别记录滤波稳定前的所有增益K以及滤波稳定后的增益值K，分析滤波器稳定后发生丢点时丢点数以及Q/R的值对增益的影响修正丢点对增益造成的影响，构建针对不同种类对象的滤波器，具体步骤如下：

第一步，设置过程噪声方差Q和观测噪声方差R。

当转移矩阵Φ，观测矩阵H，以及过程噪声方差Q、观测噪声方差R为常数矩阵，数据率T保持不变时，卡尔曼滤波会渐近稳定。因此，针对不同种类的目标需要设置对应的过程噪声方差Q和观测噪声方差R。

第二步，计算增益K。

目标状态运动方程一般可表示为

X_k+1＝ΦX_k+W_k (1)

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

观测方程可表示为

z_k＝HX_k+v_k (2)

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

标准卡尔曼滤波的递推计算公式如下：

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

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

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

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

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

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

已知转移矩阵Φ，观测矩阵H，过程噪声方差Q和观测噪声方差R，可以计算得到对应的增益K。

从方程(7)可以看出，当增益K变大时，估计值便更倾向于测量值，当K变小时，估计值便更倾向于预测值。而从方程(6)可以看出，增益K与过程噪声方差Q成正比，与测量噪声方差R成反比。

在物理意义上，运动方程相当于是一种先验的模型，而测量值相当于一个后验的信息，需要利用后验的信息去修正先验。卡尔曼增益便体现了这种修正的程度，当运动方程很准确时，估计值主要取决于运动方程的预测值，反之当测量很精确时，估计值便主要取决于测量值。

第三步，修正丢点情况下的增益K。

当到达稳定状态时，卡尔曼增益K及预测协方差P_k|k-1、估计协方差P_k|k都近似不变。可认为

此时，预测协方差

与估计协方差

也并不相等。实际上，

这是因为利用运动方程来预测时，协方差是增加的，而得到测量值更新时，协方差减小。当增加量等于减小量时，滤波器达到稳定。两者的差值为：

从以上的分析可发现，当未得到观测不能进行更新(即丢点时)，协方差将一直增加，从而打破平衡状态，增益也会发生变化。

以CV模型为例，来分析卡尔曼增益的变化规律。CV模型采用极坐标形式，方位、仰角、距离可解耦成三个通道单独滤波，用于卡尔曼滤波时，滤波器是一致渐近稳定的。

在CV模型中，状态向量为

x分别为目标方位r、方位a、仰角e。

为了先说明增益是如何变化的，假设数据率为0.1，采样数为200步，其中100～110步未得到测量，只能做预测。图1为距离通道的卡尔曼增益变化曲线，图2为角度通道的卡尔曼增益变化曲线。从图中可以看出，滤波开始，增益迅速降低达到收敛状态，之后保持平稳，在100～110丢点后，增益会比稳定增益有较大的抬高。

实际上，在CV模型中，在不丢点的情况下，增益只和过程噪声方差Q、测量噪声方差R以及数据率T相关。即增益K有如下关系：

因此，可分析在固定数据率下，增益达到稳定后丢点数对增益的影响，定义实际增益与稳定增益之比为Ratio：

图3和图4分别为数据率T＝0.1s时，滤波器达到稳定状态后不同

比值下丢点数对距离和距离速度增益的影响。图5和图6分别为数据率T＝0.1s时，滤波器达到稳定状态后不同

比值下丢点数对角度和角度速度增益的影响。

从以上四个图中可以看出，对于距离、角度增益，随着丢点数的增加，实际增益值与稳定值的比值会一直增加，最后趋于稳定。而距离速度、角度速度增益随着丢点数增加，实际增益值与稳定值之比会先增加再减小。

通过实验可以发现，丢点对增益的影响是比较明显的。大多数简化卡尔曼算法中，滤波器达到稳定后就采用常增益，这种处理在丢点时存在着较大的误差，必须根据丢点的情况进行适当的修正，如可根据Q/R以及丢点数插值得到比值。

以上实验是针对数据率为0.1s得到的，当采样间隔变大时，少数的丢点会对增益产生更大的影响。这种影响是具有一般性的，其他模型也存在。

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

Claims (3)

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。

Images