一种新型模糊卡尔曼滤波数据融合算法

肖力铭，屈济坤，齐海生，岳振军

(中国人民解放军陆军工程大学，江苏 南京 210007)

为了获得精确的机动目标航迹估计，需要通过卡尔曼滤波算法去除航迹数据中的误差。当机动目标的运动过程和噪声都确定，且系统噪声和量测噪声都是零均值白噪声时，经典的卡尔曼滤波方法能够得到较好的结果[1]。实际场景中，尤其在恶劣条件下，可能会存在较大的模型误差，噪声也不会保持确定的统计特性，导致卡尔曼滤波器发散。文献[2]和文献[3]提出通过引入模糊推理系统动态调整滤波器模型中的误差力度，实现对滤波器发散的控制。但这些方法假设噪声协方差矩阵之间存在简单的线性关系，即假设当前的噪声协方差矩阵是前一时刻噪声协方差矩阵的某一常数倍，通过模糊推理来调整倍数值，这一做法默认假设引起误差的各个因素是均匀的，对测量误差来说这一假设具有一定的合理性，但对于状态误差来说，显然就不符合事实了。

基于此，本文进一步改进了基于模糊推理的卡尔曼滤波算法，将当前状态噪声协方差矩阵看成是前一时刻噪声协方差矩阵的随机扰动，通过引入模糊控制器，依据不同的误差来源采用不同推理规则和协方差矩阵调整策略，在线调整系统噪声协方差值的扰动程度，使之更好地贴合实际情况，从而使复杂条件下的数据融合效果得到较大程度的改善，仿真实验验证了算法的有效性。

1 卡尔曼滤波算法

20世纪60年代，卡尔曼(R．E．Kalman)发表了一篇重要的论文《A New Approach to Linear Filtering and Prediction Problems》，提出了一种新的线性滤波和预测理论。这种方法的实用价值在1963年美国“阿波罗计划”中得到证实，现在广泛应用于通信、导航、制导与控制、动态系统辨识、电力系统和经济等领域[4-8]。

设系统的态方程和量测方程分别为：

X(k)=AX(k-1)+BU(k)+ω(k)

(1)

Z(k)=HX(k)+v(k)

(2)

其中，X(k)、Z(k)、U(k)分别表示k时刻的系统状态、测量值和系统控制量，A、B和H分别表示系统参数和测量系统参数，ω(k)和v(k)分别是系统的过程噪声和测量噪声。

卡尔曼滤波算法通过以下五个方程描述。

系统状态的预测方程为

X(k|k-1)=AX(k-1|k-1)+Bu(k)

(3)

该状态下的协方差预测方程为

P(k|k-1)=AP(k|k-1)A′+Q

(4)

系统滤波估计方程：

X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))

(5)

卡尔曼滤波增益方程：

Kg(k)=P(k|k-1)H′/HP(k|k-1)H′+R

(6)

滤波协方差更新方程：

P(k|k)=(I-Kg(k)H)P(k|k-1)

(7)

2 新型模糊卡尔曼滤波算法

2.1 算法描述

在经典卡尔曼滤波算法中，ω(k)和v(k)均被看作理想的零均值高斯白噪声，且其协方差阵Qk和Rk已知[9-10]。然而，在复杂环境中，这一假设并不能满足，因而需要实时调整Q，R值，以有效减小误差。

对运动状态引起的噪声协方差矩阵改变，可假设k时刻的系统噪声协方差估计值为：

(8)

另一方面，定义传感器获得的偏差值的观测噪声协方差估计值为：

(9)

其中，λi(k)为时变特性引起的量测噪声协方差调整量。

当经典卡尔曼滤波算法的条件满足时，Pi(k)与Cqi(k)、Si(k)与Cri(k)近似相等，复杂条件下，其相等性被破坏，即探测系统中的噪声和测量噪声统计特性发生变化，需要调整Pi(k)和Si(k)的值，使之更好地反应实际情况。

为了刻画Cqi(k)和Pi(k)的差别，定义函数：

其中，F是某一矩阵范数(本文取为2-范数)，MDQi(k)值与1差别过大时，就必须通过调整Qi(k)的值改变Pi(k)值的大小，使得MDQi(k)近似等于1。

2.2 协方差矩阵调整规则

1)Q矩阵调整规则和算法

Q矩阵反映系统状态误差情况，可以假设各分量具有均匀性，对式(8)做如下处理：

如果MDQi(k)接近于1，那么τi(k)=1；

如果MDQi(k)远大于1，那么τi(k)>1；

如果MDQi(k)远小于1，那么τi(k)<1。

2)R矩阵调整规则和算法

R矩阵反映量测误差情况，因而对式(9)采用如下规则：

如果MDRi(k)接近于1，那么λi(k)=0；

如果MDRi(k)远大于1，那么λi(k)>0；

如果MDRi(k)远小于1，那么λi(k)<0。

这个算法的核心思想是，如果理论协方差矩阵的范数远大于实际计算值，则让理论协方差矩阵的元素大者变小、小者变大，进而减小理论协方差矩阵的范数，使之与实际吻合，反之亦然。

通过仿真实验可以得到以下结论：

当MDQi(k)≫1时，1<τi(k)≤20；

当MDQi(k)≪1时，0.05≤τi(k)<1；

当0<|λi(k)|≤10时，滤波能够较好地收敛在目标航迹上，否则，滤波会比较发散，导致追踪效果不够稳定。

3 实验验证

其中，u1(k)和u2(k)是在时间间隔T内，径向速度和方位角速度的变化情况。由此得到状态方程：

X(k+1)=A(k+1,k)X(k)+ω(k)

(10)

记v1(k)和v2(k)为观测噪声，得到测量方程：

即

Z(k)=CX(k)+v(k)

(11)

状态方程激励信号的协方差阵为

E[ω(k)ωT(j)]=Q(k)δkj

由式(10)可知，ω(k)=[0u1(k) 0u2(k)]T，故

由式(10)，得到状态变量X(k)的初始估计值和真实值分别为

假设激励信号u和噪声源v是独立的，则均方误差矩阵为

分别对卡尔曼滤波算法和本文方法进行实验仿真，得到结果如图1-5所示。

由图1和图2可以看出，当取样点达到一定时，卡尔曼滤波算法的追踪效果将出现明显的偏差，而本文提出的模糊卡尔曼滤波算法则有效地避免了此类情况的发生。由图3和图4可以看出，卡尔曼滤波方法在航迹追踪时，随着时间的推移，积累的绝对误差将不断增加，最终将会丧失航迹追踪的功能。而本文提出的模糊卡尔曼滤波算法，随着噪声协方差的模糊调整量不断变化，能够较好地完成航迹的吻合跟踪，从而达到预期的效果。图5通过将两种方法的航迹追踪效果进行对比印证了本文所述方法的有效性。

4 结束语

本文通过对误差特性的分析，提出基于不同的误差来源采用不同推理规则和协方差矩阵调整策略的模糊卡尔曼滤波算法。实例表明，在克服运动目标长时跟踪导致的累积误差和恶劣条件导致的误差波动方面起到了一定的作用，并且算法稳定性较好，计算量也没有显著增加。