改進(jìn)的自適應(yīng)卡爾曼濾波算法

劉桂辛

（河北前進(jìn)機(jī)械廠 河北 石家莊　050035）

改進(jìn)的自適應(yīng)卡爾曼濾波算法

劉桂辛

（河北前進(jìn)機(jī)械廠 河北 石家莊050035）

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

卡爾曼濾波；GPS/DR組合導(dǎo)航；自適應(yīng)濾波；AEKF；AUKF

在室外作業(yè)時，利用導(dǎo)航系統(tǒng)可以實(shí)現(xiàn)監(jiān)控中心對運(yùn)載車的工作狀態(tài)進(jìn)行干預(yù)的目的。現(xiàn)今，國內(nèi)外設(shè)計者青睞于將GPS與DR定位方法結(jié)合，能互補(bǔ)長短，有效解決定位的連續(xù)性問題。

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

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

1　GPS/DR組合導(dǎo)航系統(tǒng)模型與建模

根據(jù)車載導(dǎo)航系統(tǒng)對導(dǎo)航的精度和連續(xù)性的要求，并且結(jié)合以上耦合方式的特點(diǎn)，本文選擇研究GPS/DR組合導(dǎo)航系統(tǒng)的緊組合方式。GPS/DR緊組合系統(tǒng)的結(jié)構(gòu)框圖如圖1所示。

圖1　GPS/DR緊組合結(jié)構(gòu)框圖Fig．1　GPS/DR tight combination structure

2　改進(jìn)的自適應(yīng)卡爾曼濾波算法

在自適應(yīng)擴(kuò)展卡爾曼濾波（AEKF）算法中，定義新息Ck：

這里應(yīng)用新息的相關(guān)矩陣作為當(dāng)環(huán)境噪聲發(fā)生改變時，測量噪聲的統(tǒng)計特征不明確的情況下的新的量測值。當(dāng)Rk減小時，減小。反之，當(dāng)Rk增大時，增大。最終這種關(guān)系被應(yīng)用在AEKF算法中。

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

式中:M為自適應(yīng)窗口。

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

d值用來調(diào)節(jié)自適應(yīng)窗口M的長度。判定如下：

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

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

在此，可將此自適應(yīng)算法與RLS算法中的遺忘因子λ的意義聯(lián)系起來。最終，增益矩陣Kk由自適應(yīng)因子bk調(diào)節(jié)

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

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

圖2　AEKF算法分析結(jié)構(gòu)圖Fig．2　Analysis structure graph of AEKF algorithm

3　MATLAB仿真分析

根據(jù)地面車輛的運(yùn)動情況，設(shè)定運(yùn)載車的初始位置坐標(biāo)為（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的大小為隨機(jī)高斯白噪聲。隨著時間的推移，由于環(huán)境噪聲的變化，在101～200 s時，測量噪聲Vk為1．5Vk，在201～300 s時，測量噪聲Vk為0．5Vk。此時，在此條件下，用MATLAB軟件分別用UKF、EKF、AEKF、AUKF算法進(jìn)行仿真。

由于處理的是隨機(jī)過程，每次得到的程序運(yùn)行結(jié)果是不一定的，但是不會有很大差距。因此本實(shí)驗(yàn)取了5次運(yùn)行結(jié)果的平均值進(jìn)行比較，結(jié)果如表1所示。

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

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

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

UKF算法經(jīng)運(yùn)行，已經(jīng)濾波發(fā)散，直接結(jié)果是程序不能運(yùn)行。分析其原因是濾波均方誤差Pk失去正定性，而導(dǎo)致的濾波發(fā)散。而EKF、AEKF和AUKF算法仍能進(jìn)行估計。可見，使用自適應(yīng)調(diào)節(jié)改善UKF算法的濾波性能。在環(huán)境噪聲改變的情況下，可以長時間進(jìn)行估計時，而不使濾波發(fā)散。最后得到3種算法經(jīng)過仿真的東、北向位置和速度參數(shù)誤差的比較圖如圖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算法表現(xiàn)出的誤差較小。而AUKF算法雖然沒有發(fā)散，但是誤差較大。噪聲較小（2 001～3 000 s）時，AUKF算法在北向的位置誤差隨著時間的增長而逐漸偏移。而AEKF和EKF得到的誤差結(jié)果不相上下。這說明雖然自適應(yīng)算法增長了UKF算法濾波發(fā)散的時間，但是隨著時間的增長，其對精度的提高沒有AEKF算法效果好。

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

圖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

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

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

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

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

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

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

4　結(jié)　論

本文著重研究了GPS/DR車載組合導(dǎo)航系統(tǒng)中環(huán)境噪聲大小的改變對濾波估計的影響，并針對這一問題加以自適應(yīng)改進(jìn)。

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

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

該自適應(yīng)濾波算法結(jié)構(gòu)簡單，容易實(shí)現(xiàn)，并且都可以與EKF與UKF算法結(jié)合。在實(shí)際的工程應(yīng)用中，面對收斂速率和濾波的穩(wěn)定性這兩個相抵觸的影響因素時，應(yīng)該根據(jù)具體的情況加以選擇。當(dāng)實(shí)時性要求較高時，可以以最小收斂速率為標(biāo)準(zhǔn)進(jìn)行改進(jìn)。當(dāng)穩(wěn)定性較高時，則以得到最小的均方誤差為標(biāo)準(zhǔn)。

由于研究涉及的領(lǐng)域較廣、算法較多、理論較深，在對基于多傳感器信息融合的GPS/DR車載組合導(dǎo)航系統(tǒng)的研究當(dāng)GPS信號失鎖時，可以使用聯(lián)合卡爾曼濾波組合方式。對兩種濾波方式進(jìn)行加權(quán)處理，以使更優(yōu)的利用二者信息。

［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，F(xiàn)eng 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—），女，河北保定人，碩士研究生，助理工程師。研究方向：多傳感器信息處理。