基于弦線法的去導(dǎo)迭代擴(kuò)展卡爾曼濾波器

李 科，常國(guó)賓，李勝全，金際航

(海軍海洋測(cè)繪研究所，天津 300061)

0 引言

在線性高斯濾波中，卡爾曼濾波(Kalman Filter，KF)可以得到狀態(tài)量的極大驗(yàn)后估計(jì)(Maximum a Posteriori，MAP)。但對(duì)于非線性觀測(cè)方程，需要對(duì)方程進(jìn)行線性化近似才可以應(yīng)用KF算法，這種近似包括擴(kuò)展卡爾曼濾波［1］(Extended Kalman Filter，EKF)中的一階Taylor級(jí)數(shù)近似和無(wú)敏卡爾曼濾波［2］(Unscented Kalman Filter，UKF)中的統(tǒng)計(jì)線性化近似［3］。近似方法一般得到的估計(jì)值不是MAP估計(jì)。之前的應(yīng)用表明［1，4-9］，在觀測(cè)信息值得信任時(shí)，迭代擴(kuò)展卡爾曼濾波(Iterated Extended Kalman Filter，IEKF)可以使估計(jì)值逐漸向MAP估計(jì)收斂。Bell［10］通過(guò)構(gòu)造偽觀測(cè)量，把MAP估計(jì)問(wèn)題轉(zhuǎn)化為極大似然估計(jì)(ML)，指出IEKF中的觀測(cè)迭代是一種高斯-牛頓迭代過(guò)程。然而高斯-牛頓算法是一種解析算法，當(dāng)觀測(cè)方程的表達(dá)式比較復(fù)雜，或者其函數(shù)不能用初等函數(shù)表達(dá)時(shí)，求解Jacobian矩陣比較困難［11］，一定程度上限制了IEKF的應(yīng)用。

1 IEKF是高斯－牛頓迭代過(guò)程

高斯-牛頓迭代用于求解非線性最小二乘問(wèn)題，是牛頓迭代算法的一種近似應(yīng)用。Bell從高斯-牛頓迭代的角度推導(dǎo)了IEKF的觀測(cè)迭代公式［10］。

假設(shè)各隨機(jī)量都為高斯分布，KF的觀測(cè)更新可以表示為一個(gè)求極大驗(yàn)后估計(jì)的問(wèn)題。驗(yàn)后概率可表示為

即求狀態(tài)量的極大驗(yàn)后估計(jì)可轉(zhuǎn)化為所構(gòu)造的偽觀測(cè)量的極大似然估計(jì)。令STS=C-1，f(x)=S(Zg(x))，

由上式可知，卡爾曼濾波中的觀測(cè)更新過(guò)程為一個(gè)典型的非線性最小二乘估計(jì)問(wèn)題。應(yīng)用高斯-牛頓迭代算法［12］可以得到

式中，H為觀測(cè)方程的Jacobian矩陣。將式(6)代入式(5)并整理，得

至此可以發(fā)現(xiàn)，傳統(tǒng)的迭代擴(kuò)展卡爾曼濾波算法的觀測(cè)迭代過(guò)程為一個(gè)高斯-牛頓迭代過(guò)程。

2 去導(dǎo)迭代擴(kuò)展卡爾曼濾波器

牛頓迭代法需要求非線性函數(shù)的導(dǎo)數(shù)，這限制了IEKF的應(yīng)用。用兩點(diǎn)間的割線斜率矩陣取代牛頓迭代法中的Jacobian矩陣，可以得到弦線迭代法。設(shè)xn，xn-1是非線性函數(shù)h(x)=0的兩個(gè)近似解，過(guò)點(diǎn)(xn，h(xn))和點(diǎn)(xn-1，h(xn-1))做一條直線，則弦線方程為

當(dāng)x為一維時(shí)，H由式(8)唯一確定，即H=(h(xi)-h(xi-1))/(xi-xi-1);當(dāng)x為多維時(shí)，僅由式(8)不能唯一確定H，此時(shí)用多維弦線法，即Broyden法［6］求H，得

令L(x)=0的解為xi+1，則

與牛頓法一樣，當(dāng)h(x)在根的某個(gè)鄰域內(nèi)有二階的連續(xù)導(dǎo)數(shù)，且一階導(dǎo)數(shù)不為0時(shí)，弦線法具有局部收斂性，可以證明弦線法具有超越性的收斂速度。

把弦線法的思想引入迭代擴(kuò)展卡爾曼濾波，可以得到一種去導(dǎo)的迭代擴(kuò)展卡爾曼濾波算法，其觀測(cè)迭代公式為

由于每次迭代需要已知之前的兩次迭代值作為迭代更新的近似解，而在初次迭代時(shí)只能得到預(yù)測(cè)值這一個(gè)近似解，為此，采用有限差分法通過(guò)狀態(tài)估計(jì)預(yù)測(cè)值和方差矩陣預(yù)測(cè)值構(gòu)造迭代初始值的方法，則

其中，α為衰減因子，此值根據(jù)觀測(cè)方程的非線性程度、觀測(cè)噪聲和系統(tǒng)噪聲的強(qiáng)度進(jìn)行選擇，取0＜α≤1。

3 UNGM實(shí)例仿真

通過(guò)一個(gè)具有代表性的例子來(lái)考察本文提出的DF-IEKF以及EKF、IEKF和UKF等濾波算法的濾波性能。單變量非平穩(wěn)增長(zhǎng)模型(Univariate Nonstationary Growth Model，UNGM)在經(jīng)濟(jì)領(lǐng)域應(yīng)用很普遍，由于其高度的非線性以及狀態(tài)量分布的雙模態(tài)特性，使得其經(jīng)常被作為驗(yàn)證非線性濾波算法的基準(zhǔn)模型。其離散時(shí)間動(dòng)態(tài)系統(tǒng)方程為

其中:系統(tǒng)噪聲wk-1～N(0，1);觀測(cè)噪聲vk～N(0，1);仿真時(shí)刻K=500;仿真時(shí)用于產(chǎn)生仿真數(shù)據(jù)的初始真值x0=0.1;Monte Carlo仿真次數(shù)M取為50。

各種濾波算法的性能分別用兩種指標(biāo)進(jìn)行表示。一種是每次Monte Carlo仿真的時(shí)間平均均方誤差(T_MSE)，另一種指標(biāo)是在每個(gè)時(shí)間點(diǎn)處的Monte Carlo集總平均均方誤差(S_MSE)，其定義為

m和k分別為Monte Carlo仿真次數(shù)參數(shù)和時(shí)間參數(shù)。圖1和圖2分別給出了各種濾波算法得出的上述兩種指標(biāo)曲線圖(為便于顯示，圖2只給出k為200～250之間的估計(jì)誤差。

圖1 各濾波算法時(shí)間平均均方誤差曲線Fig.1 MSE of each filtering algorithm time

圖2 各濾波算法集平均均方誤差曲線圖Fig.2 MSE of each filtering algorithm set

表1給出了各種濾波算法在時(shí)間域和集總域都求平均后的均方誤差，其定義為

表1 各濾波算法的MSETable 1 MSE of each filtering algorithm

圖1、圖2和表1都說(shuō)明，UKF、IEKF和DF-IEKF的性能都明顯優(yōu)于EKF，IEKF和DF-IEKF而略優(yōu)于UKF，而IEKF和DF-IEKF的性能沒(méi)有明顯的區(qū)別，這也驗(yàn)證了本文的結(jié)論，即本文提出的新方法在不損失濾波精度的前提下，應(yīng)用更方便。

4 結(jié)論

本文從IEKF的高斯-牛頓推導(dǎo)過(guò)程入手，揭示了其存在的缺陷和觀測(cè)迭代中引入弦線法的可行性，得到了一種去導(dǎo)迭代擴(kuò)展卡爾曼濾波算法(Derivative Free Iterated Extended Kalman Filter，DF-IEKF)，并論述了新算法的應(yīng)用細(xì)節(jié)，最后把新算法應(yīng)用于一個(gè)非線性實(shí)例。仿真實(shí)驗(yàn)的結(jié)果表明了新算法在應(yīng)用方面的優(yōu)勢(shì)，并驗(yàn)證了相對(duì)于傳統(tǒng)迭代擴(kuò)展卡爾曼濾波，新算法并沒(méi)有損失濾波精度。

［1］CANDY J V.Bayesian signal processing［M］.New Jersey:John Wiley＆Sons，2006.

［2］JULIER SJ，UHLMANN JK.A new extension of the Kalman filter to nonlinear systems［C］//Proc.SPIE-Int.Soc.Opt.Eng.Orlando.1997，3068:182-193.

［3］LEFEBVRE T，BRUYNINCKX，H，DE SCHUTTER J.Comment on“a new method for the nonlinear transformation of means and covariances in filters and estimators”［J］.IEEE Transactions on Automatic Control，2002，47(8):1406-1409.

［4］ZHAN R，WAN J.Iterated unscented Kalman filter for passive target tracking［J］.IEEE Transactions on Aerospace and Electronic Systems，2007，43(3):1155-1163.

［5］劉大鵬，馬曉川，朱昀，等.基于混合迭代UKF的捷聯(lián)慣導(dǎo)對(duì)準(zhǔn)算法設(shè)計(jì)［J］.系統(tǒng)仿真學(xué)報(bào)，2010，22(10):2404-2406.

［6］張俊根，姬紅兵.IMM迭代擴(kuò)展卡爾曼粒子濾波跟蹤算法［J］.電子與信息學(xué)報(bào)，2010，32(5):1116-1120.

［7］程水英，毛云祥.迭代無(wú)味卡爾曼濾波器［J］.數(shù)據(jù)采集與處理，2009，24(s):43-48.

［8］李茜，賈旭東，馬寅峰，等.彈箭定姿系統(tǒng)的改進(jìn)卡爾曼濾波算法研究［J］.電光與控制，2011，18(8):47-50.

［9］李明鎖，井亮，鄒杰，等.結(jié)合擴(kuò)展卡爾曼濾波的Cam-Shift移動(dòng)目標(biāo)跟蹤算法［J］.電光與控制，2011，18(4):1-5.

［10］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.

［11］MERWE R.Sigma-point Kalman fiters for probabilistic inference in dynamic state-space models［D］.Portland，USA:Oregon Health＆Sci.Univ.，2004.

［12］DENNIS J E，SCHNABEL R B.Numerical methods for unconstrained optimization and nonlinear equations［M］.New Jersey:Prentice-Hall，Englewood Cliffs，1983.