卡爾曼濾波算法研究

毛秀華，吳 健

(中國(guó)電子科技集團(tuán)公司第五十一研究所，上海 201802)

?

卡爾曼濾波算法研究

毛秀華，吳 健

(中國(guó)電子科技集團(tuán)公司第五十一研究所，上海 201802)

對(duì)卡爾曼濾波的起源和發(fā)展進(jìn)行了簡(jiǎn)述，然后對(duì)標(biāo)準(zhǔn)卡爾曼濾波的定義和模型進(jìn)行了回顧，重點(diǎn)對(duì)近似二階擴(kuò)展卡爾曼濾波、擴(kuò)維無(wú)跡卡爾曼濾波和自適應(yīng)卡爾曼濾波等3種最新改進(jìn)型的卡爾曼濾波算法進(jìn)行了詳細(xì)闡述，最后對(duì)這3種新改進(jìn)型的卡爾曼濾波算法的優(yōu)缺點(diǎn)進(jìn)行了對(duì)比分析，對(duì)各自的適用領(lǐng)域和場(chǎng)景進(jìn)行了說(shuō)明。

卡爾曼濾波；近似二階擴(kuò)展卡爾曼濾波；無(wú)跡卡爾曼濾波；自適應(yīng)卡爾曼濾波

0 引 言

卡爾曼濾波器最早要追尋到1960年卡爾曼先生發(fā)表的關(guān)于利用遞歸算法來(lái)求解離散線性濾波器問(wèn)題的學(xué)術(shù)論文——《A New Approach to Linear Filtering and Prediction Problems》(線性濾波與預(yù)測(cè)問(wèn)題的新方法)，在卡爾曼先生的這篇學(xué)術(shù)論文中首次提出了針對(duì)維納濾波器缺點(diǎn)的全新解決方案，這種方案就是時(shí)至今日被人們所熟知的卡爾曼濾波方法。卡爾曼濾波方法功能非常強(qiáng)大，應(yīng)用也十分廣泛，利用卡爾曼濾波方法不僅可以預(yù)測(cè)信號(hào)當(dāng)前、過(guò)去的狀態(tài)，還可以預(yù)測(cè)信號(hào)的下一步即將來(lái)的狀態(tài)，這種預(yù)測(cè)和估計(jì)可以在不知道系統(tǒng)確切模型的條件下完成[1]。

最小均方誤差被用來(lái)作為卡爾曼濾波器估計(jì)的準(zhǔn)則，在完成對(duì)信號(hào)狀態(tài)變量的估計(jì)時(shí)，信號(hào)與噪聲的狀態(tài)空間模型是卡爾曼濾波算法首先使用的一個(gè)模型，在進(jìn)行狀態(tài)變量的估計(jì)時(shí)，利用的是當(dāng)前時(shí)刻的估計(jì)值和當(dāng)前時(shí)刻的觀測(cè)值。

卡爾曼濾波算法在各行業(yè)中的應(yīng)用已經(jīng)有30多年的歷史了，實(shí)踐證明卡爾曼濾波器在解決相關(guān)問(wèn)題時(shí)，它是最優(yōu)的解決方案，有時(shí)甚至是效率最高最可行的解決方案。卡爾曼濾波算法現(xiàn)在應(yīng)用得十分廣泛，在電子、控制、導(dǎo)航、無(wú)線傳感器網(wǎng)絡(luò)等多個(gè)領(lǐng)域均有應(yīng)用，在雷達(dá)跟蹤、無(wú)源定位跟蹤、導(dǎo)彈追蹤等軍事方面，卡爾曼濾波算法均有不錯(cuò)的應(yīng)用并取得了不俗的成績(jī)。

文章首先對(duì)標(biāo)準(zhǔn)卡爾曼濾波的定義、數(shù)學(xué)模型進(jìn)行了簡(jiǎn)單介紹，然后重點(diǎn)對(duì)3種最新改進(jìn)型的卡爾曼濾波算法進(jìn)行了闡述，對(duì)近似二階擴(kuò)展卡爾曼濾波、擴(kuò)維無(wú)跡卡爾曼濾波和自適應(yīng)卡爾曼濾波這3種算法的優(yōu)缺點(diǎn)進(jìn)行了分析和說(shuō)明，對(duì)各自的應(yīng)用場(chǎng)景進(jìn)行了闡述。

1 標(biāo)準(zhǔn)卡爾曼濾波算法

為了說(shuō)明卡爾曼濾波算法的基本原理，首先假設(shè)物理系統(tǒng)的狀態(tài)更新過(guò)程為一個(gè)離散時(shí)間的隨機(jī)過(guò)程。被控制對(duì)象的輸入對(duì)物理系統(tǒng)的狀態(tài)會(huì)產(chǎn)生影響，噪聲對(duì)物理系統(tǒng)的觀測(cè)過(guò)程產(chǎn)生影響，假定物理系統(tǒng)的狀態(tài)是非直接可觀測(cè)的。在以上假設(shè)前提下，得到系統(tǒng)的狀體方程和觀測(cè)方程[2]：

(1)

式中:Xk為狀態(tài)向量;Lk為觀測(cè)向量;Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣;Uk-1為控制向量,一般不考慮;Γk,k-1、Bk為系數(shù)矩陣;Ωk-1為系統(tǒng)動(dòng)態(tài)噪聲向量;Δk為觀測(cè)噪聲向量，其隨機(jī)模型為:

(2)

(3)

(4)

卡爾曼濾波遞推公式為:

(5)

(6)

(7)

(8)

(9)

2 幾種最新改進(jìn)型的卡爾曼濾波算法

(1) 近似二階擴(kuò)展卡爾曼濾波算法

從前面的介紹和闡述可知，標(biāo)準(zhǔn)卡爾曼濾波算法主要針對(duì)的是線性系統(tǒng)，而在實(shí)際的工程運(yùn)用中需要解決的問(wèn)題多數(shù)是非線性系統(tǒng)，這時(shí)候標(biāo)準(zhǔn)卡爾曼濾波算法就失去了效能，在這種背景下20世紀(jì)70年代擴(kuò)展卡爾曼濾波算法(EKF)被提出來(lái)了，擴(kuò)展卡爾曼濾波算法針對(duì)的是非線性系統(tǒng)，目前在非線性系統(tǒng)估計(jì)方面已經(jīng)有了十分廣泛的應(yīng)用。近似二階擴(kuò)展卡爾曼濾波方法(AS-EKF)采用的框架是線性最小方差框架，采用均值的二階近似變換來(lái)獲取非線性系統(tǒng)的遞推濾波框架。該濾波基于線性最小方差遞推框架，狀態(tài)X的最小方差估計(jì)為[3]：

(10)

(11)

得到最優(yōu)估計(jì)和估計(jì)誤差方差陣的遞推方程分別為：

E(X(k+1)/L1k+1)=E(X(k+1)/L(k+1),L1k)

(12)

var(X(k+1)/L1k+1)=var(X(k+1)/L(k+1),L1k)

(13)

在EKF中,假設(shè)非線性函數(shù)y=f(X)在狀態(tài)X的最優(yōu)估計(jì)(預(yù)測(cè))值處線性化,即:

(14)

y的均值、方差和協(xié)方差的近似估計(jì):

(15)

對(duì)均值進(jìn)行二階近似,而對(duì)方差和協(xié)方差進(jìn)行一階近似,即可得:

(16)

式中：Λ為梯度函數(shù)。

考慮如下帶加性噪聲的非線性離散系統(tǒng):

X(k+1)=f(X(k),k)+Γ(X(k),k)V(k)

(17)

L(k+1)=h(X(k+1),k+1)+W(k+1)

(18)

將式(16)所使用的近似二階方法代入式(12)和(13)，可得如下近似二階卡爾曼濾波遞推公式。

預(yù)測(cè)階段：

(19)

(20)

其中:

(21)

(22)

更新階段：

K(k)=

(23)

(24)

P(k+1)= (I-K(k+1)Hk+1)P(k+1|k)·

(I-K(k+1)Hk+1)T+

K(k+1)R(k+1)KT(k+1)

(25)

(2) 擴(kuò)維無(wú)跡卡爾曼濾波算法

利用無(wú)跡變換和卡爾曼濾波算法的線性濾波框架建立起來(lái)的一種改進(jìn)型卡爾曼濾波算法叫無(wú)跡卡爾曼濾波(UKF)。UKF的狀態(tài)向量概率密度函數(shù)是通過(guò)非線性狀態(tài)方程來(lái)估算得到，無(wú)跡卡爾曼濾波存在的不足是當(dāng)系統(tǒng)的噪聲較大時(shí)，無(wú)跡卡爾曼濾波并不能得到精確的卡爾曼濾波結(jié)果。

為了克服無(wú)跡卡爾曼濾波的這一缺點(diǎn)，改進(jìn)型無(wú)跡卡爾曼濾波在初始狀態(tài)中添加過(guò)程和測(cè)量噪聲，這樣采樣點(diǎn)就包含了這些噪聲，從而保證了噪聲能夠在非線性系統(tǒng)中傳輸和估計(jì)，從而使得無(wú)跡卡爾曼濾波的結(jié)果能夠更好地接近真實(shí)值，在系統(tǒng)噪聲和觀測(cè)噪聲較大的條件下改進(jìn)型無(wú)跡卡爾曼濾波應(yīng)用更加有效[4]。無(wú)跡卡爾曼濾波算法如下：

設(shè)非線性系統(tǒng)模型為:

(26)

式中：X(k)為系統(tǒng)k時(shí)刻的n維狀態(tài)向量；L(k)為系統(tǒng)k時(shí)刻的測(cè)量向量；f(·),h(·)為非線性變換；過(guò)程噪聲V(k)和測(cè)量噪聲W(k)是零均值。

初始狀態(tài)為:

(27)

(28)

狀態(tài)變量為:

(29)

狀態(tài)方差為:

(30)

此時(shí)的采樣點(diǎn)集變?yōu)閧Xai(k|k),i=0,1,…,2N,N=(n+q+m)}。

時(shí)間更新過(guò)程變?yōu)?

xx(k/k-1)=f(xx(k-1/k-1),k-1)+

xx(k-1)

(31)

(32)

(33)

(34)

(35)

測(cè)量更新過(guò)程變?yōu)椋?/p>

(36)

(37)

(38)

(39)

(40)

在本算法中因?yàn)闋顟B(tài)按照擴(kuò)展維數(shù)來(lái)計(jì)算的，Sigma采樣點(diǎn)的個(gè)數(shù)為2(n+q+m) +1,維數(shù)為N×(2N+1)；當(dāng)不擴(kuò)維時(shí),Sigma采樣點(diǎn)的個(gè)數(shù)為2n+1,維數(shù)為n×(2n+1)[5]。

(3) 自適應(yīng)卡爾曼濾波算法

從前面的介紹可見，傳統(tǒng)卡爾曼濾波算法的限制條件較多，對(duì)系統(tǒng)模型有一定的依賴性，當(dāng)系統(tǒng)模型存在誤差時(shí)，直接影響到系統(tǒng)的濾波效果。針對(duì)上述問(wèn)題，很多學(xué)者提出了不同的改進(jìn)方案，其中自適應(yīng)卡爾曼濾波算法是最有效的解決方案之一[6]。

(a) 多模型自適應(yīng)卡爾曼濾波算法

(41)

(b) 基于信息的自適應(yīng)卡爾曼濾波算法

多模型自適應(yīng)卡爾曼濾波和基于信息的卡爾曼濾波是自適應(yīng)卡爾曼濾波的2個(gè)最主要的研究方向[8]。基于信息的卡爾曼濾波的研究方法主要分為2類:Q(k)和R(k)是通過(guò)信息估計(jì)得到的,進(jìn)而完成對(duì)增益矩陣J(k)的控制，J(k)的自適應(yīng)估計(jì)是通過(guò)信息來(lái)實(shí)現(xiàn)。

(42)

(43)

(44)

多模型自適應(yīng)卡爾曼濾波算法估計(jì)了噪聲的方差矩陣和噪聲的均值，該方法適用于觀測(cè)噪聲為有色噪聲的高動(dòng)態(tài)系統(tǒng)。多模型自適應(yīng)卡爾曼濾波的最大缺點(diǎn)是計(jì)算復(fù)雜，計(jì)算量偏大，不利于實(shí)時(shí)處理。基于信息的自適應(yīng)卡爾曼濾克服了第1種方法的不足，計(jì)算量較小，但是計(jì)算過(guò)程中涉及到矩陣求逆，操作也不是十分方便，但是這種方法適用于噪聲統(tǒng)計(jì)特性不明確或噪聲發(fā)生變換的場(chǎng)景[9]。

3 幾種卡爾曼濾波算法的比較與分析

(1) 近似二階卡爾曼濾波算法

近似二階卡爾曼濾波算法是一種典型的遞推非線性濾波算法，參閱相關(guān)文獻(xiàn)可知，近似二階卡爾曼濾波算法在計(jì)算量稍微大于擴(kuò)展卡爾曼濾波算法的基礎(chǔ)上，近似二階卡爾曼濾波算法的濾波效果明顯強(qiáng)于擴(kuò)展卡爾曼濾波算法，而且此時(shí)的近似二階卡爾曼濾波算法的計(jì)算量明顯低于無(wú)跡卡爾曼濾波算法。綜上所述，近似二階卡爾曼濾波算法適用于濾波精度和實(shí)時(shí)性要求較高的場(chǎng)合。

近似二階卡爾曼濾波算法也并非絕對(duì)完美，它也存在有許多的不足，近似二階卡爾曼濾波算法要利用雅克比矩陣來(lái)完成對(duì)系統(tǒng)的線性化處理,但是在實(shí)際應(yīng)用中非線性函數(shù)的雅克比矩陣確實(shí)非常難得到，在無(wú)法得到雅克比矩陣的情況下，系統(tǒng)誤差就會(huì)很大，而且還會(huì)導(dǎo)致濾波器的不穩(wěn)定。近似二階卡爾曼濾波算法適用于對(duì)估計(jì)精度和計(jì)算量都有所要求的非線性系統(tǒng)濾波器設(shè)計(jì)。

(2) 擴(kuò)維無(wú)跡卡爾曼濾波算法

UKF是通過(guò)無(wú)跡變換而來(lái)，是通過(guò)線性KF框架得到的。無(wú)跡卡爾曼濾波是通過(guò)非線性狀態(tài)方程去估計(jì)狀態(tài)向量的概率密度函數(shù)，相比擴(kuò)展卡爾曼濾波，無(wú)跡卡爾曼濾波的精度提高了許多，而且無(wú)跡卡爾曼濾波對(duì)參數(shù)不敏感，無(wú)跡卡爾曼濾波的魯棒性非常好，無(wú)跡卡爾曼濾波非常適用于非線性系統(tǒng)。

無(wú)跡卡爾曼濾波算法也有其不足，那就是當(dāng)系統(tǒng)中噪聲較大時(shí)，濾波結(jié)果并不是十分的精確，而改進(jìn)無(wú)跡卡爾曼濾波算法對(duì)系統(tǒng)噪聲和過(guò)程噪聲進(jìn)行了采集，將采集到的噪聲和采樣點(diǎn)進(jìn)行集中，從而使得濾波精度和靈敏度都得到了提高。擴(kuò)維無(wú)跡卡爾曼濾波是無(wú)損變換和標(biāo)準(zhǔn)卡爾曼濾波的有機(jī)結(jié)合體，它適用于導(dǎo)航、目標(biāo)跟蹤、信號(hào)處理和神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)等多種場(chǎng)合。

(3) 自適應(yīng)卡爾曼濾波

由自適應(yīng)卡爾曼濾波公式可以看到，濾波方程遞推公式的計(jì)算過(guò)程是一個(gè)連續(xù)的預(yù)測(cè)和修正過(guò)程。在計(jì)算過(guò)程中，可以計(jì)算速度參數(shù)，校正由干擾引起的突變，與實(shí)際變形趨勢(shì)更加吻合。當(dāng)?shù)玫叫碌挠^測(cè)數(shù)據(jù)時(shí)，可以計(jì)算出實(shí)時(shí)處理觀測(cè)結(jié)果的新的濾波值，它將參數(shù)估計(jì)和預(yù)測(cè)有機(jī)地結(jié)合起來(lái)，因此，自適應(yīng)卡爾曼濾波特別適合于變形監(jiān)測(cè)的動(dòng)態(tài)數(shù)據(jù)處理。

自適應(yīng)卡爾曼濾波也有缺點(diǎn)，它是基于過(guò)去所有觀測(cè)值的條件期望，這些觀測(cè)值的權(quán)值被處理成一致的，因此隨著時(shí)間的推移，當(dāng)采集到的數(shù)據(jù)越來(lái)越多，從新的數(shù)據(jù)中獲得的信息量將相對(duì)下降，算法逐漸失去了修正的能力，參數(shù)估計(jì)可能偏離真實(shí)值較遠(yuǎn)而不能更新[10]。自適應(yīng)卡爾曼濾波將系統(tǒng)辨識(shí)與濾波估計(jì)有機(jī)地結(jié)合到一起，它對(duì)動(dòng)態(tài)系統(tǒng)的濾波非常有效。

4 結(jié)束語(yǔ)

卡爾曼濾波算法作為研究熱點(diǎn)已經(jīng)持續(xù)多年，對(duì)卡爾曼濾波算法的改進(jìn)也非常多，除本文介紹的幾種方法以外，還有基于小波變換的卡爾曼濾波算法、基于自適應(yīng)采樣平方根的無(wú)跡卡爾曼濾波算法、雙無(wú)跡卡爾曼濾波算法等。這些改進(jìn)型卡爾曼濾波算法適用場(chǎng)景各不相同，適用條件也千變?nèi)f化，一種完美、通用的卡爾曼濾波算法是不存在的，很多的卡爾曼濾波算法都是在彼此基礎(chǔ)上改進(jìn)而來(lái)，大家是相輔相成，相互支撐的，具體選用那種算法，需要由實(shí)際應(yīng)用場(chǎng)景來(lái)決定。

[1] 黃小平，王巖．卡爾曼濾波原理及應(yīng)用[M]．北京:電子工業(yè)出版社，2015．

[2] GAO S S，HU G G，ZHONG Y M.Windowing and random weighting-based adaptive unscented Kalman filter[J].International Journal of Adaptive Control and Signal Processing，2014，29(2):201-223．

[3] GUSTAFSSON F,HENDEBY G.Some relations between extended and unscented Kalman filters[J]．IEEE Transactions on Signal Process,2012,60(2):545- 555.

[4] MEDEIROS C B,WANDERLEY M M.Multiple-model linear Kalman filter framework for unpredictable signals[J].IEEE Sensors Journal,2014,14(4):979-991.

[5] CHANG G.Kalman filter with both adaptivity and robustness[J].Process Control,2014,24(3):81-87.

[6] CHANG L,HU B,LI A,QIN F.Transformed unscented Kalman filter[J]．IEEE Transactions on Automatic Control,2013,58(1):252-257.

[7] HORWOOD J T,POORE A B.Adaptive Gaussian sum filtersfor space surveillance[J]．IEEE Transactions on Automatic Control,2011,56(8):1777-1790.

[8] HUR H,AHN H S.Discrete-time H∞ filtering for mobile robotlocalization using wireless sensor network[J]．IEEE Sensors Journal,2013,13(1):245-252.

[9] KARLGAARD C D.Nonlinear regression Huber-Kalman filtering andfixed-interval smoothing[J]．Journal of Guidance,Control and Dynamics,2014,38(2):322- 330.

[10]MBALAWATA I S,SRKKS,VIHOLA M,HAARIO H.Adaptive Metropolis algorithm using variational Bayesian adaptive Kalman filter[J].Computer Statist and Data Analysis,2015,83(1):101-115.

ResearchintoKalmanFilteringAlgorithm

MAO Xiu-hua,WU Jian
(51st Research Institute of CETC,Shanghai 201802,China)

This paper expatiates the origin and development of Kalman filtering briefly,then reviews the definition and model of the standard Kalman filtering,focuses on expatiating three new improved Kalman fitering algorithms in detail:approximate two order extended Kalman filtering,augmented unscented Kalman filtering,adaptive Kalman filtering,finally compares and analyzes the advantages and disadvantages of three new improved Kalman filtering algorithms,performs illumination to respective application fields and scene.

Kalman filtering；approximate two order extended Kalman filtering；unscented Kalman filtering；adaptive Kalman filtering

2017-03-30

TN713

：A

：CN32-1413(2017)03-0064-05

10.16426/j.cnki.jcdzdk.2017.03.015