衰減記憶平方根容積卡爾曼濾波及其應(yīng)用

楊旺明

（三峽大學(xué)　宜昌　443002）

1　引言

目標(biāo)跟蹤技術(shù)是信號處理領(lǐng)域的一個重要研究方向，有著極為廣泛的應(yīng)用價值。隨著現(xiàn)代信號處理技術(shù)的發(fā)展，目標(biāo)的機(jī)動能力大幅提高，光電跟蹤系統(tǒng)中原有的采用標(biāo)準(zhǔn)卡爾曼濾波對目標(biāo)進(jìn)行跟蹤和預(yù)測的方法，顯然已越來越難以滿足目標(biāo)跟蹤精度和預(yù)測精度的要求［1］。

光電跟蹤系統(tǒng)中的目標(biāo)跟蹤屬于非線性估計問題，解決該問題常用的非線性方法有粒子濾波（PF）算法、擴(kuò)展卡爾曼濾波（EKF）算法和無跡卡爾曼濾波（UKF）算法等［2～3］。PF算法由于計算較為復(fù)雜，實(shí)時性較差，很少應(yīng)用于要求快速反應(yīng)的實(shí)際工程中；EKF算法是傳統(tǒng)非線性狀態(tài)估計的代表，但只適用于弱非線性系統(tǒng)，對于強(qiáng)非線性系統(tǒng)，存在著濾波精度不高，穩(wěn)定性差，以及在高維情況下計算較為復(fù)雜等缺點(diǎn)［4］；UKF算法利用一組sigma點(diǎn)代表狀態(tài)變量的分布，并以此求解非線性系統(tǒng)高斯隨機(jī)變量的均值和方差，對于任何的非線性系統(tǒng)精度都能達(dá)到二階以上［5］，其精度高于EKF。文獻(xiàn)［6］中提出的CKF算法是一種基于容積數(shù)值積分準(zhǔn)則的遞推型貝葉斯濾波器，該算法具有優(yōu)越的非線性逼近性能、數(shù)值精度以及濾波穩(wěn)定性，又不必預(yù)先設(shè)定參量，易于實(shí)現(xiàn)，因而得到廣泛應(yīng)用［7～9］。在實(shí)際的濾波過程中，由于計算機(jī)字長有限，因而存在著計算誤差，這些誤差積累起來，會降低濾波精度，甚至造成計算發(fā)散；在目標(biāo)跟蹤中，不可能完全獲悉目標(biāo)的運(yùn)動狀態(tài)，因此不能真實(shí)反映物理過程，使得觀測值與模型不匹配，會造成濾波發(fā)散。為了提高CKF算法的精度和穩(wěn)定性，文獻(xiàn)［4］提出了SCKF算法，以估計誤差的平方根矩陣進(jìn)行迭代計算，解決了CKF算法中由于計算誤差引起的計算發(fā)散問題，但并未解決由模型誤差引起的濾波發(fā)散問題。

因此，本文應(yīng)用衰減記憶濾波算法對SCKF算法進(jìn)行改進(jìn)，提出了MASCKF算法，以克服由模型誤差引起的濾波發(fā)散，提高濾波精度和穩(wěn)定性。最后，仿真驗(yàn)證了該算法的有效性。

2　MASCKF算法

2.1　容積積分原理

離散非線性動態(tài)系統(tǒng)的狀態(tài)方程為

式中，xk為系統(tǒng)狀態(tài)估計向量；zk為量測值；過程噪聲 wk-1和量測噪聲 vk相互獨(dú)立，且 wk-1~N(0，Qk-1)，vk~N(0，Rk)。

SCKF算法將非線性濾波歸結(jié)為非線性函數(shù)與高斯概率密度乘積的積分求解問題，即

式中，I(f)為所求積分；x為狀態(tài)估計向量；f(x)為求積非線性函數(shù)；Rn為積分域。

對于式（14）的積分問題，SCKF算法采用Spherical-Radial求容積規(guī)則，采用一組2n個等權(quán)值的容積點(diǎn)來實(shí)現(xiàn)非線性逼近，即

式中，n為狀態(tài)變量維數(shù)。

使用三階容積原則獲得的基本容積點(diǎn)和對應(yīng)權(quán)值為

SCKF具體算法可參考文獻(xiàn)［6］。

2.2 MASCKF算法

顯然，SCKF對觀測數(shù)據(jù)的記憶是無限增長的，即計算k時刻的最優(yōu)估計 x?k|k時要用到k時刻以前所有的觀測數(shù)據(jù)。因此，當(dāng)濾波模型不準(zhǔn)確時，隨著k的增大，濾波值中的舊數(shù)據(jù)比重太大，新數(shù)據(jù)由于比重太小而不能有效抑制誤差對狀態(tài)估計的影響，易引起濾波發(fā)散。因此，逐漸減少舊數(shù)據(jù)的權(quán)重，相對地增加新數(shù)據(jù)的權(quán)重，是克服由模型誤差引起的濾波發(fā)散的有效方法［10］。根據(jù)這一觀點(diǎn)設(shè)計的濾波方法被稱為衰減記憶濾波。本文在SCKF算法的基礎(chǔ)上引入衰減記憶濾波思想，設(shè)計了MASCKF算法。

為了克服N時刻的濾波發(fā)散，就應(yīng)該相對突出增益WN，而相對減小N時刻以前的增益矩陣Wk，且Wk與 Rk，Qk-1為負(fù)相關(guān)關(guān)系。所以，為了達(dá)到上述目的，可使遠(yuǎn)離N時刻的Rk，Qk-1逐漸變大。本文采用指數(shù)加權(quán)法，將…，R處理為下列矩陣：

式中，Ci(i=0，1，2，…)是適當(dāng)選取的正整數(shù)。

同樣地，將N時刻以前的系統(tǒng)噪聲的作用逐漸衰減，則有

因此，在式（1）過程噪聲wk-1和量測噪聲vk的分布變?yōu)?/p>

1）時間更新

采用Cholesky方法分解協(xié)方差：

計算容積點(diǎn)：

式中 i=1，2，3，…，M。

計算通過狀態(tài)方程傳播的容積點(diǎn)：

估計預(yù)測誤差方差矩陣的平方根：在實(shí)際應(yīng)用中，當(dāng)系統(tǒng)模型描述不準(zhǔn)確及噪聲統(tǒng)計特性不確定時，新數(shù)據(jù)對預(yù)測值的修正作用下降，舊數(shù)據(jù)的修正作用相對上升，濾波誤差的均值趨于無窮大產(chǎn)生發(fā)散現(xiàn)象。綜上所述，從濾波發(fā)散的原因入手，取Ci＞0時，衰減因子提高了新數(shù)據(jù)的權(quán)值。與常規(guī)SCKF算法相比，衰減記憶容積卡爾曼濾波算法在式（13～14）中增加了一個衰減因子，加重當(dāng)前時刻數(shù)據(jù)在狀態(tài)估計中的作用，從而避免濾波器發(fā)散。

3　光電跟蹤中的目標(biāo)狀態(tài)估計

由于光電跟蹤的目標(biāo)量測信息是在極坐標(biāo)系中得到的，如果將其轉(zhuǎn)化為直坐標(biāo)系的位置信息，量測信息將在數(shù)學(xué)轉(zhuǎn)換中失去物理量測之間的不相關(guān)性，使濾波效果變差［11］。因此，本文直接在極坐標(biāo)系中進(jìn)行濾波預(yù)測。為了檢驗(yàn)所設(shè)計算法在目標(biāo)模型不準(zhǔn)確條件下的對濾波精度的改善效果，目標(biāo)模型直接采用三階常加速（CA）模型。

以跟蹤目標(biāo)在俯仰平面上的角度濾波預(yù)測為例，選取目標(biāo)俯仰角位移、角速度和角加速度作為狀態(tài)變量，即

4　仿真實(shí)驗(yàn)及結(jié)果分析

4.1　仿真條件設(shè)定

實(shí)際光電跟蹤系統(tǒng)的目標(biāo)俯仰角濾波中，濾波輸入信號為目標(biāo)相對于跟蹤平臺的俯仰角度值，即跟蹤平臺的高低指向和量測所得的脫靶量信號值之和。本文為了驗(yàn)證MASCKF算法在目標(biāo)模型不準(zhǔn)確的條件下對濾波性能的改善，特將正弦信號θ(t)=sin(0.628t)rad作為所跟蹤的目標(biāo)在水平方位上的角位移，則θ(t)即為跟蹤系統(tǒng)方位角跟蹤的等效正弦輸入信號。量測系統(tǒng)的量測間隔為T=0.01s，量測噪聲方差為Rk=1.8m rad，系統(tǒng)噪聲方差為Qk=25m rad s2。濾波狀態(tài)初值選為 x0=[0，0，0]T，誤差協(xié)方差的初值為：P0|0=diag[1000，1000，1000]。選用CKF算法和SCKF算法作為參照對比實(shí)驗(yàn)。

4.2　仿真試驗(yàn)結(jié)果分析

在上述條件設(shè)置下，以θ(t)為等效正弦輸入信號，分別用MASCKF、CKF和SCKF方法進(jìn)行100次蒙特卡羅方法仿真，三種濾波算法預(yù)測的位移、速度和加速度的均方根誤差如圖1、2、3所示。

由圖1、2、3可知，由于正弦運(yùn)動是變加速運(yùn)動，目標(biāo)模型不可能精確匹配實(shí)際情況，所以三種濾波算法預(yù)測出的位移、速度和加速度都還存在一些誤差；CKF由于受到計算發(fā)散和由模型誤差引起的濾波發(fā)散的影響，其均方根誤差值一直較大，且處于劇烈變化中；與CKF相比，SCKF由于采用了平方根方法削弱了計算誤差，減小了濾波誤差，其結(jié)果精確和穩(wěn)定；而MASCKF算法同時采用了平方根方法和衰減記憶濾波，有效抑制了計算誤差和濾波發(fā)散，其位移、速度和角速度的均方根誤差明顯降低，且趨于穩(wěn)定狀態(tài)。

圖1　角位移均方根誤差

圖2　角速度均方根誤差

圖3　角加速度均方根誤差

5　結(jié)語

CKF算法由于其推理嚴(yán)謹(jǐn)、易于實(shí)現(xiàn)而得到廣泛應(yīng)用。但CKF屬于卡爾曼濾波框架的濾波算法，因而同樣面臨著由計算誤差帶來的計算發(fā)散和由系統(tǒng)的模型誤差引起的濾波發(fā)散。采用平方根方法改善CKF的SCKF算法只能有效抑制計算發(fā)散，因此，本文在SCKF算法基礎(chǔ)上應(yīng)用衰減記憶濾波算法對其進(jìn)行改進(jìn)，提出了MASCKF算法，以克服由模型誤差引起的濾波發(fā)散，提高濾波精度。并將此方法應(yīng)用到光電跟蹤系統(tǒng)的目標(biāo)跟蹤濾波預(yù)測中，通過仿真對比表明，在目標(biāo)運(yùn)動模型不準(zhǔn)確的條件下，MASCKF的濾波精度和穩(wěn)定性優(yōu)于SCKF和CKF。

［1］李文軍，陳濤.基于卡爾曼濾波器的等效復(fù)合控制技術(shù)研究［J］.光學(xué)精密工程，2006，14（2）：279-284.LIWenjun，CHEN Tao.Equivalentcombined control technique based on Kalman filter.Optics and Precision Engineering，2006，14（2）：279-284.

［2］Sun F C，Hu X S，Zou Y，et al.Adaptive unscented Kalman filtering for state of charge estimation ofa lithium-ion batteryfor electricvehicles［J］.Energy，36 （5） ：3531-3540.

［3］Yang X J，Xing K Y，F(xiàn)eng X L.Maneuvering Target Tracking in Dense Clutter Based on Particle Filtering［J］.2011，24（2）：171-180.

［4］付廣仁，曹正文，米林山，等.基于最小二乘和EKF的偽衛(wèi)星定位算法［J］.計算機(jī)工程與應(yīng)用，2012，48（34）：148-151.FU Guangren，CAO Zhengwen，MILinshan，et al.Pseudolite positioning algorithm based on least squares and EKF.Computer Engineering and Applications，2012，48（34）：148-151.

［5］Bruno O S，Leonardo A B，Paulo I，et al.Flight path reconstruction-a comparison of nonlinear Kalman filter and smoother algorithms［J］.Aerospace Science and Technology，2011，15（1）：60-71.

［6］Ienkaran A，Simon H.Cubature Kalman filter［J］.IEEE Trans.on Automatic Control，2009，54（6）：1254-1279.

［7］穆靜，蔡遠(yuǎn)利.迭代容積卡爾曼濾波算法及其應(yīng)用［J］.系統(tǒng)工程與電子技術(shù)，2011，33（7）：1454-1457.MU Jing，CAIYuanli.Iterated cubature Kalman filter and its application.Systems Engineering and Electronics，2011，33（7）：1454-1457.

［8］鹿傳國，馮新喜，張迪.基于改進(jìn)容積卡爾曼濾波的純方位目標(biāo)跟蹤［J］.系統(tǒng)工程與電子技術(shù)，2012，34（1）：28-33.LU Chuanguo，F(xiàn)ENG Xinxi，ZHANG Di.Pure bearing tracking based on improved cubature Kalman filter.Systems Engineering and Electronics，2012，34（1）：28-33.

［9］Martin H，Karl J，Jiri J，etal.Dynamicmodeling of neuronal responses in fMRI using cubature Kalman filtering［J］.NeuroImage，2011，56（4）：2109-2128.

［10］朱萍，茅旭初.基于衰減記憶UKF的高精度GPS定位估計［J］.計算機(jī)仿真，2011（2）：35-38.ZHU Ping，MAO Xuchu.A High Precision GPSPosition Estimation.Computer Simulation，2011（2）：35-38.