改進的自適應卡爾曼濾波算法

劉桂辛

（河北前進機械廠 河北 石家莊　050035）

改進的自適應卡爾曼濾波算法

劉桂辛

（河北前進機械廠 河北 石家莊050035）

為了消弱由測量噪聲的變化對導航估計的影響，本文提出了一種自適應濾波法。該算法利用閾值自動選擇開窗窗口的長度調節自適應因子，以此調整擴展卡爾曼濾波法（EKF）與無跡卡爾曼濾波法（UKF）中的濾波增益，進而合理利用測量信息，由此分別形成AEKF與AUKF算法。將兩種方法分別應用于全球導航系統（GPS）和航位推算（DR）緊組合導航系統中，仿真結果證明了與UKF相比，可以有效地避免濾波發散。

卡爾曼濾波；GPS/DR組合導航；自適應濾波；AEKF；AUKF

在室外作業時，利用導航系統可以實現監控中心對運載車的工作狀態進行干預的目的。現今，國內外設計者青睞于將GPS與DR定位方法結合，能互補長短，有效解決定位的連續性問題。

Kalman濾波法［1］可用于設計運載體的高精度組合導航系統。在實際工程利用中，系統模型大部分是非線性，并且由于環境的改變，模型的觀測噪聲可能會發生很大變化，使噪聲的統計特征不再準確，影響定位精度甚至使濾波發散。EKF［2-3］和UKF［4］算法是解決非線性系統估計的有效方法，但在模型準確和加性噪聲的統計特征明確時其估計結果才可靠、精準，不能完全滿足實際工程的需要。

為了解決這一問題，國內外專家提出了自適應濾波算法。在最為常用的方法中，相比較而言，最小均方（LMS）自適應算法［5］收斂速度慢，遞歸最小二乘（RLS）算法［6-7］魯棒性較差。文中提出的算法利用閾值自動選擇開窗窗口的長度調節自適應因子，以此調整EKF與UKF算法中的濾波增益，進而合理利用測量信息，由此分別形成AEKF與AUKF算法。這種方法的優點是判定效率比較高，能快速收斂，適合實時濾波，并且在測量噪聲的統計特性不明確的情況下有效避免濾波發散，使估計值平穩、精確。

1　GPS/DR組合導航系統模型與建模

根據車載導航系統對導航的精度和連續性的要求，并且結合以上耦合方式的特點，本文選擇研究GPS/DR組合導航系統的緊組合方式。GPS/DR緊組合系統的結構框圖如圖1所示。

圖1　GPS/DR緊組合結構框圖Fig．1　GPS/DR tight combination structure

2　改進的自適應卡爾曼濾波算法

在自適應擴展卡爾曼濾波（AEKF）算法中，定義新息Ck：

這里應用新息的相關矩陣作為當環境噪聲發生改變時，測量噪聲的統計特征不明確的情況下的新的量測值。當Rk減小時，減小。反之，當Rk增大時，增大。最終這種關系被應用在AEKF算法中。

在自適應無跡卡爾曼濾波（AUKF）算法中，定義新息Ck為：

式中:M為自適應窗口。

定義d為測量殘余價值量，由Ck與相關矩陣的逆矩陣得到d。計算公式：

d值用來調節自適應窗口M的長度。判定如下：

式中：μmin和μmax為判定門限。一般由經驗給出，μmin=0，μmax=1；η為窗口M的收斂速率，取η＜1的任意有理小數。

顯然自適應窗口長度最大為k，最小為1。即當d＞1時，環境噪聲的統計特性不明確，測量值不能正確得到狀態的估計值，應取最小的新息矩陣，M=1，最小；當d＜0時環境噪聲的統計特性明確，測量值能正確得到狀態的估計值，當取最大的新息矩陣，M=k，最大；當0＜d＜1時，這個創新的序列M= k×ηd-μmin，取適當值，從而使取值適當。

在此，可將此自適應算法與RLS算法中的遺忘因子λ的意義聯系起來。最終，增益矩陣Kk由自適應因子bk調節

本算法所得到的自適應因子bk的作用是：隨著噪聲大小的改變，使測量噪聲的統計特征Rk的變化對增益Kk的影響減到最小，從而使估計值達到最佳。更能敏感地反應當前時刻觀測量的動力學模型的誤差現狀。

通過以上算式，介紹了自適應過程，其先通過調整窗口長度，再調節自適應因子。這種方法的優點是判定效率比較高，能快速收斂，適合實時濾波，并且在測量噪聲的統計特性不明確的情況下有效避免濾波發散，使估計值平穩、精確。其缺點是當實時濾波環境下測量噪聲的改變對濾波增益產生較大的影響，當減小這種影響是以犧牲精度或是收斂速率為代價的，即輸出的狀態誤差較大或是濾波算法所用的時間較長。AEKF算法分析結構圖如圖2所示。

圖2　AEKF算法分析結構圖Fig．2　Analysis structure graph of AEKF algorithm

3　MATLAB仿真分析

根據地面車輛的運動情況，設定運載車的初始位置坐標為（0，0），初始速度為（10，10），初始加速度為（0，0）。初始化X0=［0；10；0；0；10；0］，P0=diag（［100，1，0．01，100，1，0．01］），過程噪聲均方誤差為，測量噪聲均方誤差為σe=12 m，σn=12 m，σω=0．001 rad/s，σs=0．7 m。濾波周期T=1 s。

取濾波采樣時間N=300 s。假定在初始1～100 s時，測量噪聲Vk的大小為隨機高斯白噪聲。隨著時間的推移，由于環境噪聲的變化，在101～200 s時，測量噪聲Vk為1．5Vk，在201～300 s時，測量噪聲Vk為0．5Vk。此時，在此條件下，用MATLAB軟件分別用UKF、EKF、AEKF、AUKF算法進行仿真。

由于處理的是隨機過程，每次得到的程序運行結果是不一定的，但是不會有很大差距。因此本實驗取了5次運行結果的平均值進行比較，結果如表1所示。

由實驗數據可得，雖然，算法對AEKF和AUKF的運行時間影響各異。但是，無論是快是慢，差距都不大。在短時間（300 s）的自適應調整時，并沒有超出實時性的有效范圍。這有力證明了AUKF和AEKF算法適合于GPS/DR車載組合導航系統在車輛定位和導航中的應用，滿足實時性的要求。

表1　4種算法基于時間的比較Tab.1　Four methods of time comparison

取采樣時間N=3 000 s。假定在初始1～1 000 s時，測量噪聲Vk的大小為隨機高斯白噪聲。隨著時間的推移，由于環境的變化，在1 001～2 000 s時，測量噪聲Vk變化為1．5Vk，在2 001～3 000 s時，測量噪聲Vk變化為0．5Vk。

UKF算法經運行，已經濾波發散，直接結果是程序不能運行。分析其原因是濾波均方誤差Pk失去正定性，而導致的濾波發散。而EKF、AEKF和AUKF算法仍能進行估計。可見，使用自適應調節改善UKF算法的濾波性能。在環境噪聲改變的情況下，可以長時間進行估計時，而不使濾波發散。最后得到3種算法經過仿真的東、北向位置和速度參數誤差的比較圖如圖3～6所示。

圖3　AEKF、EKF和AUKF算法的東向位置誤差比較Fig．3　East position estimation error comparison of AEKF、AUKF and EKF

圖4　AEKF、EKF和AUKF算法的北向位置誤差比較Fig．4　North position estimation error comparison of AEKF、AUKF and EKF

由圖3和圖4的3種算法的東、北向位置誤差比較仿真直觀圖所示，在噪聲較大（1 001～2 000 s）時，AEKF算法比EKF算法表現出的誤差較小。而AUKF算法雖然沒有發散，但是誤差較大。噪聲較小（2 001～3 000 s）時，AUKF算法在北向的位置誤差隨著時間的增長而逐漸偏移。而AEKF和EKF得到的誤差結果不相上下。這說明雖然自適應算法增長了UKF算法濾波發散的時間，但是隨著時間的增長，其對精度的提高沒有AEKF算法效果好。

由圖5和圖6的3種算法的東、北向速度誤差比較仿真直觀圖所示，在噪聲較大（1 001～2 000 s）時，AEKF比EKF算法出現的較大的誤差次數較少。而AUKF算法雖然沒有發散，但是誤差較大。在噪聲較小（2 001～3 000 s）時，AUKF算法表現出的誤差較大，而AEKF和EKF算法表現出的誤差不相上下。

圖5　AEKF、EKF和AUKF算法的東向速度誤差比較Fig．5　East velocity estimation error comparison of AEKF、AUKF and EKF

圖6　AEKF、EKF和AUKF算法的北向速度誤差比較Fig．6　North velocity estimation error comparison of AEKF、AUKF and EKF

綜合以上結果，采樣時間（3 000 s）與采樣短時間（300 s）的濾波結果進行比較可得。相同的是，在噪聲較大的情況下，都表明AEKF和AUKF算法具有較好的濾波效果。而UKF和EKF算法在噪聲較小的時候體現了較好的濾波效果。不同的是，經自適應改進的AUKF算法有效的解決了UKF算法在長時間估計計算時出現的濾波發散問題。而AEKF算法比EKF算法在導航的總體參數方面也提高了精度和濾波的平穩度。

這些由數據結果客觀表明。得到的AEKF、AUKF和EKF算法均方誤差和平均誤差數據如表2和表3所示。

表2　AEKF、AUKF和EKF算法平均誤差數據Tab.2　Average error value comparison of AEKF、AUKF and EKF

由表2中數據可知，因為UKF算法已經發散，而AUKF算法有明顯的自適應性，仍能進行濾波，可以說是最主要的優點。但與EKF算法相比，其只在東向速度的平均誤差方面有所改進，對其余參數的估計精度均有所下將。而AEKF算法所得數據與EKF算法相比，除東向位置平均誤差外，均有很大提高。說明經自適應改進的算法，在噪聲環境變化的情況下，提高了估計精度。然而采樣時間長（3 000 s）時，自適應算法的收斂速度也減慢，這是在工程應用中應該考慮的因素。

由表3中的數據可知，經對比AEKF和EKF算法的東、北向位置平均誤差所得，AEKF算法減小了均方誤差，這說明AEKF算法改善了對估計位置的濾波的性能。但是在對東、北向位置的均方誤差均沒有改善。這驗證了減小濾波波動要以犧牲精度為代價的觀點。而AUKF算法相對于EKF和AEKF算法而言，其均方誤差的輸出北向位置均方誤差外，其余參數都有所增大，這說明AUKF算法在長時間（3 000 s）時，雖然能解決濾波發散問題，但缺乏對濾波的性能方面的完善。

表3　AEKF、AUKF和EKF算法均方誤差數據Tab.3　Mean square error value comparison of AEKF、AUKF and EKF

4　結　論

本文著重研究了GPS/DR車載組合導航系統中環境噪聲大小的改變對濾波估計的影響，并針對這一問題加以自適應改進。

通過EKF和UKF算法的自適應改進，利用新息的相關矩陣和估計矩陣的跡對自適應因子進行求值，從而調節增益矩陣。由AEKF和AUKF算法在誤差的平均值以及均方誤差兩方面的仿真實驗比較顯示，在環境噪聲大小變化時，文中得到的AUKF算法能有效消除UKF算法濾波發散問題，使估計誤差保持在可靠范圍之內。并且AEKF算法相對EKF算法的定位精度也提高了。

通過判定門限的經驗取值和收斂因子，定義了一種新開窗窗口的長度的自適應調整。并由MATLAB仿真實驗得到收斂因子的最佳收斂速率。文中得到的AEKF和AUKF算法，在短時間的估計計算中，濾波的收斂速率并沒有減慢，能有效滿足組合導航系統的實時性要求。

該自適應濾波算法結構簡單，容易實現，并且都可以與EKF與UKF算法結合。在實際的工程應用中，面對收斂速率和濾波的穩定性這兩個相抵觸的影響因素時，應該根據具體的情況加以選擇。當實時性要求較高時，可以以最小收斂速率為標準進行改進。當穩定性較高時，則以得到最小的均方誤差為標準。

由于研究涉及的領域較廣、算法較多、理論較深，在對基于多傳感器信息融合的GPS/DR車載組合導航系統的研究當GPS信號失鎖時，可以使用聯合卡爾曼濾波組合方式。對兩種濾波方式進行加權處理，以使更優的利用二者信息。

［1］Haitao Zhang，Jian Rong，Xiaochun Zhong．The performance comparison and algorithm analysis of first order EKF，Second Order EKF and smoother for GPS/DR navigation［C］//IEEE InternationalConferenceonCommunicationTechnology Proceedings，2008:432-437．

［2］Jing Yu，Xiyuan Chen．Application of Extended Kalman Filter in Ultre-Tight GPS/INS Integration Based onGPS Software Receiver［M］．IEEE 2010．

［3］Sebastian Kluge，Konrad Reif，Martin Brokate．Stochastic Stability of the Extended Kalman Filter With Intermittent Observations［J］．IEEE Transactions on automatic control，2010，55（2）:514-518．

［4］Yao Li，Xiaosu Xu．The application of EKF and UKF to the SINS/GPS integrated navigation systems［M］．IEEE National Natural Science Foundation of China，2010．

［5］YanLing Hao，Zhen Guo，Feng Sun，Wei Gao．Adaptive Extended Kalman Filtering for SINS/GPS Integrated Navigation Systems［J］．IEEE International Joint Conference on Computational Sciences and Optimization，2009:192-194．

［6］J Arenas-Garcia，A R Figueiras-Vidal，A H Sayed．Steady stateperformance of convex combinations of adaptive filters ［C］//Proceedings of IEEE International Conference Acoustics，Speech，and Signal Processing，2005．

［7］XING Guo-Quan，Zhao Yuan．Application Analysis of RLS Adaptive Filter in Signal Noise Removing and Simulation［M］．IEEE，2010

The improved adaptive Kalman filter algorithm

LIU Gui-xin
（Hebei Qianjin Machinery Factory,Shijiazhuang 050035,China）

In order to weaken the variational measurement noise influence of the navigation estimation，this paper propose a kind of adaptive filtering method．The algorithm using threshold automatic selection the windowing of length，which regulates adaptive factor，to adjust the gain of Extended Kalman filter（EKF）and Unscented Kalman filter（UKF）algorithm，and rationaliy utilized the measurement information，thus respectively formed the AEKF and AUKF algorithm．When the two kinds of methods were used in the global navigation system （GPS）and dead rocking(DR)tightly integrated navigation system，the simulation result demonstrates that，compared to UKF，the method can effectively avoid filtering divergence．

Kalman filter；GPS and DR integrated navigation system；adaptive filter；AEKF；AUKF

TN713

A

1674－6236（2016）02-0048-04

2015-03-09稿件編號：201503125

劉桂辛（1987—），女，河北保定人，碩士研究生，助理工程師。研究方向：多傳感器信息處理。