卡爾曼濾波算法研究

毛秀華，吳 健

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

?

卡爾曼濾波算法研究

毛秀華，吳 健

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

對卡爾曼濾波的起源和發展進行了簡述，然后對標準卡爾曼濾波的定義和模型進行了回顧，重點對近似二階擴展卡爾曼濾波、擴維無跡卡爾曼濾波和自適應卡爾曼濾波等3種最新改進型的卡爾曼濾波算法進行了詳細闡述，最后對這3種新改進型的卡爾曼濾波算法的優缺點進行了對比分析，對各自的適用領域和場景進行了說明。

卡爾曼濾波；近似二階擴展卡爾曼濾波；無跡卡爾曼濾波；自適應卡爾曼濾波

0 引 言

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

最小均方誤差被用來作為卡爾曼濾波器估計的準則，在完成對信號狀態變量的估計時，信號與噪聲的狀態空間模型是卡爾曼濾波算法首先使用的一個模型，在進行狀態變量的估計時，利用的是當前時刻的估計值和當前時刻的觀測值。

卡爾曼濾波算法在各行業中的應用已經有30多年的歷史了，實踐證明卡爾曼濾波器在解決相關問題時，它是最優的解決方案，有時甚至是效率最高最可行的解決方案。卡爾曼濾波算法現在應用得十分廣泛，在電子、控制、導航、無線傳感器網絡等多個領域均有應用，在雷達跟蹤、無源定位跟蹤、導彈追蹤等軍事方面，卡爾曼濾波算法均有不錯的應用并取得了不俗的成績。

文章首先對標準卡爾曼濾波的定義、數學模型進行了簡單介紹，然后重點對3種最新改進型的卡爾曼濾波算法進行了闡述，對近似二階擴展卡爾曼濾波、擴維無跡卡爾曼濾波和自適應卡爾曼濾波這3種算法的優缺點進行了分析和說明，對各自的應用場景進行了闡述。

1 標準卡爾曼濾波算法

為了說明卡爾曼濾波算法的基本原理，首先假設物理系統的狀態更新過程為一個離散時間的隨機過程。被控制對象的輸入對物理系統的狀態會產生影響，噪聲對物理系統的觀測過程產生影響，假定物理系統的狀態是非直接可觀測的。在以上假設前提下，得到系統的狀體方程和觀測方程[2]：

(1)

式中:Xk為狀態向量;Lk為觀測向量;Φk,k-1為狀態轉移矩陣;Uk-1為控制向量,一般不考慮;Γk,k-1、Bk為系數矩陣;Ωk-1為系統動態噪聲向量;Δk為觀測噪聲向量，其隨機模型為:

(2)

(3)

(4)

卡爾曼濾波遞推公式為:

(5)

(6)

(7)

(8)

(9)

2 幾種最新改進型的卡爾曼濾波算法

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

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

(10)

(11)

得到最優估計和估計誤差方差陣的遞推方程分別為：

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中,假設非線性函數y=f(X)在狀態X的最優估計(預測)值處線性化,即:

(14)

y的均值、方差和協方差的近似估計:

(15)

對均值進行二階近似,而對方差和協方差進行一階近似,即可得:

(16)

式中：Λ為梯度函數。

考慮如下帶加性噪聲的非線性離散系統:

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)，可得如下近似二階卡爾曼濾波遞推公式。

預測階段：

(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) 擴維無跡卡爾曼濾波算法

利用無跡變換和卡爾曼濾波算法的線性濾波框架建立起來的一種改進型卡爾曼濾波算法叫無跡卡爾曼濾波(UKF)。UKF的狀態向量概率密度函數是通過非線性狀態方程來估算得到，無跡卡爾曼濾波存在的不足是當系統的噪聲較大時，無跡卡爾曼濾波并不能得到精確的卡爾曼濾波結果。

為了克服無跡卡爾曼濾波的這一缺點，改進型無跡卡爾曼濾波在初始狀態中添加過程和測量噪聲，這樣采樣點就包含了這些噪聲，從而保證了噪聲能夠在非線性系統中傳輸和估計，從而使得無跡卡爾曼濾波的結果能夠更好地接近真實值，在系統噪聲和觀測噪聲較大的條件下改進型無跡卡爾曼濾波應用更加有效[4]。無跡卡爾曼濾波算法如下：

設非線性系統模型為:

(26)

式中：X(k)為系統k時刻的n維狀態向量；L(k)為系統k時刻的測量向量；f(·),h(·)為非線性變換；過程噪聲V(k)和測量噪聲W(k)是零均值。

初始狀態為:

(27)

(28)

狀態變量為:

(29)

狀態方差為:

(30)

此時的采樣點集變為{Xai(k|k),i=0,1,…,2N,N=(n+q+m)}。

時間更新過程變為:

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

xx(k-1)

(31)

(32)

(33)

(34)

(35)

測量更新過程變為：

(36)

(37)

(38)

(39)

(40)

在本算法中因為狀態按照擴展維數來計算的，Sigma采樣點的個數為2(n+q+m) +1,維數為N×(2N+1)；當不擴維時,Sigma采樣點的個數為2n+1,維數為n×(2n+1)[5]。

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

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

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

(41)

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

多模型自適應卡爾曼濾波和基于信息的卡爾曼濾波是自適應卡爾曼濾波的2個最主要的研究方向[8]。基于信息的卡爾曼濾波的研究方法主要分為2類:Q(k)和R(k)是通過信息估計得到的,進而完成對增益矩陣J(k)的控制，J(k)的自適應估計是通過信息來實現。

(42)

(43)

(44)

多模型自適應卡爾曼濾波算法估計了噪聲的方差矩陣和噪聲的均值，該方法適用于觀測噪聲為有色噪聲的高動態系統。多模型自適應卡爾曼濾波的最大缺點是計算復雜，計算量偏大，不利于實時處理。基于信息的自適應卡爾曼濾克服了第1種方法的不足，計算量較小，但是計算過程中涉及到矩陣求逆，操作也不是十分方便，但是這種方法適用于噪聲統計特性不明確或噪聲發生變換的場景[9]。

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

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

近似二階卡爾曼濾波算法是一種典型的遞推非線性濾波算法，參閱相關文獻可知，近似二階卡爾曼濾波算法在計算量稍微大于擴展卡爾曼濾波算法的基礎上，近似二階卡爾曼濾波算法的濾波效果明顯強于擴展卡爾曼濾波算法，而且此時的近似二階卡爾曼濾波算法的計算量明顯低于無跡卡爾曼濾波算法。綜上所述，近似二階卡爾曼濾波算法適用于濾波精度和實時性要求較高的場合。

近似二階卡爾曼濾波算法也并非絕對完美，它也存在有許多的不足，近似二階卡爾曼濾波算法要利用雅克比矩陣來完成對系統的線性化處理,但是在實際應用中非線性函數的雅克比矩陣確實非常難得到，在無法得到雅克比矩陣的情況下，系統誤差就會很大，而且還會導致濾波器的不穩定。近似二階卡爾曼濾波算法適用于對估計精度和計算量都有所要求的非線性系統濾波器設計。

(2) 擴維無跡卡爾曼濾波算法

UKF是通過無跡變換而來，是通過線性KF框架得到的。無跡卡爾曼濾波是通過非線性狀態方程去估計狀態向量的概率密度函數，相比擴展卡爾曼濾波，無跡卡爾曼濾波的精度提高了許多，而且無跡卡爾曼濾波對參數不敏感，無跡卡爾曼濾波的魯棒性非常好，無跡卡爾曼濾波非常適用于非線性系統。

無跡卡爾曼濾波算法也有其不足，那就是當系統中噪聲較大時，濾波結果并不是十分的精確，而改進無跡卡爾曼濾波算法對系統噪聲和過程噪聲進行了采集，將采集到的噪聲和采樣點進行集中，從而使得濾波精度和靈敏度都得到了提高。擴維無跡卡爾曼濾波是無損變換和標準卡爾曼濾波的有機結合體，它適用于導航、目標跟蹤、信號處理和神經網絡學習等多種場合。

(3) 自適應卡爾曼濾波

由自適應卡爾曼濾波公式可以看到，濾波方程遞推公式的計算過程是一個連續的預測和修正過程。在計算過程中，可以計算速度參數，校正由干擾引起的突變，與實際變形趨勢更加吻合。當得到新的觀測數據時，可以計算出實時處理觀測結果的新的濾波值，它將參數估計和預測有機地結合起來，因此，自適應卡爾曼濾波特別適合于變形監測的動態數據處理。

自適應卡爾曼濾波也有缺點，它是基于過去所有觀測值的條件期望，這些觀測值的權值被處理成一致的，因此隨著時間的推移，當采集到的數據越來越多，從新的數據中獲得的信息量將相對下降，算法逐漸失去了修正的能力，參數估計可能偏離真實值較遠而不能更新[10]。自適應卡爾曼濾波將系統辨識與濾波估計有機地結合到一起，它對動態系統的濾波非常有效。

4 結束語

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

[1] 黃小平，王巖．卡爾曼濾波原理及應用[M]．北京:電子工業出版社，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