0 引言

    擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)是解決非線性系統(tǒng)狀態(tài)估計(jì)問題的最經(jīng)典濾波技術(shù)^[1-3]。然而，EKF濾波存在一些缺點(diǎn)，例如系統(tǒng)動(dòng)力學(xué)模型和測量模型需可微；狀態(tài)估計(jì)對(duì)偏差比較敏感甚至?xí)V波發(fā)散^[4]。另外，由于EKF的測量更新不能使濾波結(jié)果達(dá)到最大似然值，研究者們提出了擴(kuò)展卡爾曼濾波的迭代形式(Iterated Extended Kalman Filter，IEKF)^[5-7]。在狀態(tài)預(yù)測之后，IEKF測量更新會(huì)進(jìn)行迭代，從而得到優(yōu)于EKF的估計(jì)結(jié)果，且在廣泛的應(yīng)用中已經(jīng)得到驗(yàn)證^[8-9]。為了減少狀態(tài)預(yù)測對(duì)最終估計(jì)的影響，提出了修正迭代擴(kuò)展卡爾曼濾波(Modified Iterated Extended Kalman Filter，MIEKF)算法^[10-11]，該方法在迭代過程中使用測量更新的狀態(tài)估計(jì)代替狀態(tài)預(yù)測，然后利用與IEKF中相同的高斯-牛頓方法求解。然而，進(jìn)一步研究發(fā)現(xiàn)，在第一次迭代之后，測量噪聲信息已被包含到估計(jì)的狀態(tài)中，因此系統(tǒng)狀態(tài)和估計(jì)的狀態(tài)將不與測量噪聲之間保持相互獨(dú)立。綜上，為了應(yīng)用于更高精度的估計(jì)場合中，有必要研究新的濾波技術(shù)來解決上述問題。

    受CHANG L B等人^[12-13]和文獻(xiàn)[14]中思想的啟發(fā)，本文提出一種基于狀態(tài)增廣的修正迭代擴(kuò)展卡爾曼濾波(State augmentation-based Modified Iterated Extended Kalman Filtering，SMIEKF)算法，即在迭代時(shí)將測量噪聲直接增廣到系統(tǒng)狀態(tài)中，與經(jīng)典的EKF和MIEKF相比，該濾波算法在收斂速度和估計(jì)精度方面均更優(yōu)。

1 非線性濾波問題

    一般的非線性離散時(shí)間濾波問題可描述為：

其中，x_k代表狀態(tài)向量；w_k代表t_k時(shí)刻的隨機(jī)輸入，且w_k的均值為0，協(xié)方差為Q_k。

    測量模型可表示為：

其中，y_k代表t_k時(shí)刻的測量值；v_k代表t_k時(shí)刻的測量噪聲，且v_k的均值為0，協(xié)方差為R_k。

2 基于狀態(tài)增廣的修正迭代擴(kuò)展卡爾曼濾波

2.1 修正迭代擴(kuò)展卡爾曼濾波

    在非線性測量情況下，當(dāng)局部線性化能無條件滿足時(shí)，與EKF相比，IEKF具有優(yōu)越的性能。但是，當(dāng)初始估計(jì)誤差較大時(shí)，IEKF的性能將會(huì)降低甚至低于EKF的濾波精度。另外，在IEKF的測量更新迭代過程，直接將EKF的狀態(tài)預(yù)測作為迭代步驟的初始值，而該值對(duì)最終估計(jì)的狀態(tài)具有直接且巨大的影響。當(dāng)系統(tǒng)狀態(tài)能完全被測量數(shù)據(jù)觀測時(shí)，最終狀態(tài)估計(jì)比狀態(tài)預(yù)測值更接近真實(shí)值^[15]。如果將最終狀態(tài)估計(jì)用于非線性迭代更新的初始值并利用高斯-牛頓方法解決該問題，則該算法稱為修正迭代擴(kuò)展卡爾曼濾波(MIEKF)，且MIEKF修正的測量更新等式為^[10-11]：

    顯然，當(dāng)進(jìn)行一步迭代后測量噪聲信息被包含到估計(jì)的狀態(tài)中，從而導(dǎo)致系統(tǒng)狀態(tài)和測量噪聲之間不再滿足正交性，式(3)便不再成立，因此，仍然采用式(3)使得濾波算法的精度大大降低?；诖耍疚奶岢隽嘶跔顟B(tài)增廣的修正迭代擴(kuò)展卡爾曼濾波來解決此問題。

2.2 基于狀態(tài)增廣的修正迭代擴(kuò)展卡爾曼濾波算法

    本文提出基于狀態(tài)增廣的修正迭代擴(kuò)展卡爾曼濾波(SMIEKF)的狀態(tài)估計(jì)算法，SMIEKF算法的偽碼運(yùn)行過程如下：

    由于測量噪聲已被包含到系統(tǒng)狀態(tài)中，使得新測量噪聲相對(duì)于增廣的系統(tǒng)狀態(tài)為零，而零向量與所有向量均正交。因此，保證了狀態(tài)和測量噪聲之間的正交性。

3 算法仿真

    在本節(jié)中，通過使用從雷達(dá)地面站記錄的距離測量數(shù)據(jù)再現(xiàn)彈道目標(biāo)的軌跡來驗(yàn)證所提SMIEKF濾波方法的性能，目的是估計(jì)彈道目標(biāo)在重新進(jìn)入具有恒定但未知的阻力系數(shù)大氣層時(shí)的軌跡。在系統(tǒng)動(dòng)態(tài)模型中不考慮作用在目標(biāo)上的重力加速度，原因在于這種改進(jìn)的模型對(duì)于高初始速度是有效的，這使得空氣動(dòng)力加速度較重力加速度占主導(dǎo)地位。另外，從雷達(dá)跟蹤站記錄距離測量數(shù)據(jù)。由于其在動(dòng)態(tài)和測量模型中具有顯著的非線性，因此該示例在濾波算法研究工作中被廣泛引用。

3.1 動(dòng)力學(xué)與測量模型

    上述問題的動(dòng)力學(xué)模型為：

其中v_k指具有零均值的測量噪聲向量，且它的概率密度函數(shù)為p(v_k)；a是雷達(dá)的高度；b是雷達(dá)和彈道目標(biāo)間的水平距離。測距信息從頻率為1 Hz的雷達(dá)跟蹤站獲得。仿真參數(shù)和初始條件見表1和表2^[16]。

3.2 仿真結(jié)果分析

    在該仿真中，使用四階龍格-庫塔方法對(duì)連續(xù)時(shí)間動(dòng)態(tài)方程離散化，且將時(shí)間步長設(shè)置為1 s。兩種迭代濾波算法均執(zhí)行三次迭代，共進(jìn)行100次蒙特卡羅模擬仿真，每次運(yùn)行過程都使用不同的噪聲樣本，以便計(jì)算出濾波誤差的中值估計(jì)的絕對(duì)值，從而對(duì)三種濾波算法的性能進(jìn)行公平地比較。蒙特卡羅仿真的結(jié)果如圖1~圖3所示。

    仿真結(jié)果表明，與標(biāo)準(zhǔn)的EKF和MIEKF相比，SMIEKF具有最好的性能；且MIEKF比標(biāo)準(zhǔn)EKF性能更優(yōu)。原因在于：在MIEKF測量更新過程中，系統(tǒng)狀態(tài)與測量噪聲相互獨(dú)立，但在執(zhí)行第一次迭代后，這將不再成立。相比之下，在所提出的SMIEKF算法中，將測量噪聲增廣到狀態(tài)變量中，則對(duì)應(yīng)于增廣后狀態(tài)的新測量噪聲為零，且零向量在統(tǒng)計(jì)學(xué)上與任何向量均正交，使得增廣后的狀態(tài)與測量噪聲仍保持相互獨(dú)立，因此SMIEKF可得到比MIEKF更精確的預(yù)測測量誤差協(xié)方差，從而在三種濾波算法中呈現(xiàn)出最優(yōu)的狀態(tài)估計(jì)性能。

4 結(jié)論

    本文基于狀態(tài)增廣方法提出了一種性能增強(qiáng)的SMIEKF濾波算法，該方法主要是在測量更新的迭代過程中，將測量噪聲增廣到系統(tǒng)狀態(tài)中，使得新的系統(tǒng)狀態(tài)獨(dú)立于迭代過程中相應(yīng)的測量噪聲，從而得到更好的濾波結(jié)果。仿真結(jié)果也表明，與MIEKF和傳統(tǒng)的EKF相比，所提出的濾波方法具有更優(yōu)的性能。

參考文獻(xiàn)

[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] 黃樹清，胡方強(qiáng)，包亞萍，等.抑制多徑的BD2/GPS雙模自適應(yīng)擴(kuò)展卡爾曼濾波算法[J].電子技術(shù)應(yīng)用，2017，43(2)：77-80.

[3] 李敏濤.基于RAEKF的GPS/INS緊組合導(dǎo)航方法研究[J].電子技術(shù)應(yīng)用，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.山西大學(xué) 自動(dòng)化系，山西 太原030006；2.太原理工大學(xué) 自動(dòng)化系，山西 太原030024；

3.三菱重工海爾(青島)空調(diào)機(jī)有限公司，山東 青島266200)