0 引言

扩展卡尔曼滤波(Extended Kalman Filter，EKF)是解决非线性系统状态估计问题的最经典滤波技术^[1-3]。然而，EKF滤波存在一些缺点，例如系统动力学模型和测量模型需可微；状态估计对偏差比较敏感甚至会滤波发散^[4]。另外，由于EKF的测量更新不能使滤波结果达到最大似然值，研究者们提出了扩展卡尔曼滤波的迭代形式(Iterated Extended Kalman Filter，IEKF)^[5-7]。在状态预测之后，IEKF测量更新会进行迭代，从而得到优于EKF的估计结果，且在广泛的应用中已经得到验证^[8-9]。为了减少状态预测对最终估计的影响，提出了修正迭代扩展卡尔曼滤波(Modified Iterated Extended Kalman Filter，MIEKF)算法^[10-11]，该方法在迭代过程中使用测量更新的状态估计代替状态预测，然后利用与IEKF中相同的高斯-牛顿方法求解。然而，进一步研究发现，在第一次迭代之后，测量噪声信息已被包含到估计的状态中，因此系统状态和估计的状态将不与测量噪声之间保持相互独立。综上，为了应用于更高精度的估计场合中，有必要研究新的滤波技术来解决上述问题。

受CHANG L B等人^[12-13]和文献[14]中思想的启发，本文提出一种基于状态增广的修正迭代扩展卡尔曼滤波(State augmentation-based Modified Iterated Extended Kalman Filtering，SMIEKF)算法，即在迭代时将测量噪声直接增广到系统状态中，与经典的EKF和MIEKF相比，该滤波算法在收敛速度和估计精度方面均更优。

1 非线性滤波问题

一般的非线性离散时间滤波问题可描述为：

其中，x_k代表状态向量；w_k代表t_k时刻的随机输入，且w_k的均值为0，协方差为Q_k。

测量模型可表示为：

其中，y_k代表t_k时刻的测量值；v_k代表t_k时刻的测量噪声，且v_k的均值为0，协方差为R_k。

2 基于状态增广的修正迭代扩展卡尔曼滤波

2.1 修正迭代扩展卡尔曼滤波

在非线性测量情况下，当局部线性化能无条件满足时，与EKF相比，IEKF具有优越的性能。但是，当初始估计误差较大时，IEKF的性能将会降低甚至低于EKF的滤波精度。另外，在IEKF的测量更新迭代过程，直接将EKF的状态预测作为迭代步骤的初始值，而该值对最终估计的状态具有直接且巨大的影响。当系统状态能完全被测量数据观测时，最终状态估计比状态预测值更接近真实值^[15]。如果将最终状态估计用于非线性迭代更新的初始值并利用高斯-牛顿方法解决该问题，则该算法称为修正迭代扩展卡尔曼滤波(MIEKF)，且MIEKF修正的测量更新等式为^[10-11]：

显然，当进行一步迭代后测量噪声信息被包含到估计的状态中，从而导致系统状态和测量噪声之间不再满足正交性，式(3)便不再成立，因此，仍然采用式(3)使得滤波算法的精度大大降低。基于此，本文提出了基于状态增广的修正迭代扩展卡尔曼滤波来解决此问题。

2.2 基于状态增广的修正迭代扩展卡尔曼滤波算法

本文提出基于状态增广的修正迭代扩展卡尔曼滤波(SMIEKF)的状态估计算法，SMIEKF算法的伪码运行过程如下：

由于测量噪声已被包含到系统状态中，使得新测量噪声相对于增广的系统状态为零，而零向量与所有向量均正交。因此，保证了状态和测量噪声之间的正交性。

3 算法仿真

在本节中，通过使用从雷达地面站记录的距离测量数据再现弹道目标的轨迹来验证所提SMIEKF滤波方法的性能，目的是估计弹道目标在重新进入具有恒定但未知的阻力系数大气层时的轨迹。在系统动态模型中不考虑作用在目标上的重力加速度，原因在于这种改进的模型对于高初始速度是有效的，这使得空气动力加速度较重力加速度占主导地位。另外，从雷达跟踪站记录距离测量数据。由于其在动态和测量模型中具有显著的非线性，因此该示例在滤波算法研究工作中被广泛引用。

3.1 动力学与测量模型

上述问题的动力学模型为：

其中v_k指具有零均值的测量噪声向量，且它的概率密度函数为p(v_k)；a是雷达的高度；b是雷达和弹道目标间的水平距离。测距信息从频率为1 Hz的雷达跟踪站获得。仿真参数和初始条件见表1和表2^[16]。

3.2 仿真结果分析

在该仿真中，使用四阶龙格-库塔方法对连续时间动态方程离散化，且将时间步长设置为1 s。两种迭代滤波算法均执行三次迭代，共进行100次蒙特卡罗模拟仿真，每次运行过程都使用不同的噪声样本，以便计算出滤波误差的中值估计的绝对值，从而对三种滤波算法的性能进行公平地比较。蒙特卡罗仿真的结果如图1~图3所示。

仿真结果表明，与标准的EKF和MIEKF相比，SMIEKF具有最好的性能；且MIEKF比标准EKF性能更优。原因在于：在MIEKF测量更新过程中，系统状态与测量噪声相互独立，但在执行第一次迭代后，这将不再成立。相比之下，在所提出的SMIEKF算法中，将测量噪声增广到状态变量中，则对应于增广后状态的新测量噪声为零，且零向量在统计学上与任何向量均正交，使得增广后的状态与测量噪声仍保持相互独立，因此SMIEKF可得到比MIEKF更精确的预测测量误差协方差，从而在三种滤波算法中呈现出最优的状态估计性能。

4 结论

本文基于状态增广方法提出了一种性能增强的SMIEKF滤波算法，该方法主要是在测量更新的迭代过程中，将测量噪声增广到系统状态中，使得新的系统状态独立于迭代过程中相应的测量噪声，从而得到更好的滤波结果。仿真结果也表明，与MIEKF和传统的EKF相比，所提出的滤波方法具有更优的性能。

参考文献

[1] LERRO D，BAR-SHALOM Y.Tracking with debiased consistent converted measurements versus EKF[J].IEEE Transactions on Aerospace and Electronic Systems，1993，29(3)：1015-1022.

[2] 黄树清，胡方强，包亚萍，等.抑制多径的BD2/GPS双模自适应扩展卡尔曼滤波算法[J].电子技术应用，2017，43(2)：77-80.

[3] 李敏涛.基于RAEKF的GPS/INS紧组合导航方法研究[J].电子技术应用，2019，45(2)：33-36.

[4] ATHANS M，WISHER R P，BERTOLINI A.Suboptimal state estimation for continuous-time nonlinear systems from discrete noise measurements[J].IEEE Transactions on Automatic Control，1968，13(5)：504-515.

[5] BELL，B M，CATHEY F W.The iterated Kalman filter update as a Gauss-Newton method[J].IEEE Transactions on Automatic Control，1993，38(2)：294-297

[6] GELB A.Applied optimal estimation[D].The Massachusetts Institute of Technology Press：Cambridge，MA，USA，1974.

[7] BAR-SHALOM Y，LI X R，KIRUBARAJAN T.Estimation with application to tracking and navigation：theory，algorithm，and software[M].New York：Wiley，2001.

[8] KARLGAARD C D.Nonlinear regression Huber-Kalman filtering and fixed-interval smoothing[J].Journal of Guidance，Control and Dynamics，2015，38(2)：322-330.

[9] LI W，LIU M H，DUAN D.Improved robust Huber-based divided difference filtering[J].Proceedings of the Institution of Mechanic Engineering，Part G：Journal of Aerospace Engineering，2014，228(11)：2123-2129.

[10] YANG Z，ZHONG D，GUO F，et al.Gauss-Newton iteration based algorithm for passive location by a single observer[J].Journal of Systems Engineering and Electronics，2007，29(12)：2006-2009.

[11] ZHANG J，JI H.Modified iterated extended Kalman filter based multi-observer fusion tracking for IRST[J].Journal of Systems Engineering and Electronics，2010，32(3)：504-507.

[12] CHANG L B，HU B Q，CHANG G B.Multiple outliers suppression derivative-free filter based on unscented transformation[J].Journal of Guidance，Control and Dynamics，2012，35：1902-1906.

[13] CHANG L B，HU B Q，CHANG G B.Marginalised iterated unscented Kalman filter[J].IET Control Theory and Applications，2012，6(6)：847-854.

[14] LIU M，ZHAN X，LI W.State augmentation-based iterated divided difference filtering[J].Proceedings of the Institution of Mechanic Engineering，Part G：Journal of Aerospace Engineering，2015，229(13)：2537-2544.

[15] LEFEBVRE T.，BRUYNINCKX H，SCHUTTER DE J.Kalman filters for non-linear systems：a comparison of performance[J].International Journal of Control，2004，77：639-653.

[16] KARLGAARD C D，SCHAUB H.Comparison of several nonlinear filters for a benchmark tracking problem[C].AIAA Paper，2006：2006-6243.

作者信息:

刘美红1，高山凤1，李 伟2, 谢 彬3

(1.山西大学 自动化系，山西 太原030006；2.太原理工大学 自动化系，山西 太原030024；

3.三菱重工海尔(青岛)空调机有限公司，山东 青岛266200)