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

高 偉，葉 攀，許偉通

（哈爾濱工程大學(xué)自動(dòng)化學(xué)院，哈爾濱 150001）

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

高 偉，葉 攀，許偉通

（哈爾濱工程大學(xué)自動(dòng)化學(xué)院，哈爾濱 150001）

SINS／GPS組合導(dǎo)航系統(tǒng)的融合算法主要是卡爾曼濾波，卡爾曼濾波實(shí)現(xiàn)最優(yōu)估計(jì)的前提是系統(tǒng)的模型和隨機(jī)噪聲信息必須準(zhǔn)確已知；實(shí)際情況下，大部分系統(tǒng)的模型和隨機(jī)噪聲信息不完全可知，這可能會(huì)導(dǎo)致濾波器估計(jì)精度下降；針對(duì)這一問(wèn)題，根據(jù)求解遺傳因子的方法不同對(duì)傳統(tǒng)的自適應(yīng)衰減卡爾曼濾波進(jìn)行改進(jìn)，提出一種改進(jìn)的自適應(yīng)衰減卡爾曼濾波；改進(jìn)后的算法分別適用于系統(tǒng)噪聲統(tǒng)計(jì)模型不準(zhǔn)確可知和量測(cè)噪聲統(tǒng)計(jì)模型不準(zhǔn)確可知兩種情況，分別對(duì)應(yīng)于兩種濾波算法，并且二者具有統(tǒng)一的濾波框架；仿真結(jié)果表明，改進(jìn)的自適應(yīng)衰減卡爾曼濾波比卡爾曼濾波精度較高，有效解決了因?yàn)樵肼暷Ｐ筒粶?zhǔn)確導(dǎo)致的精度下降問(wèn)題。

組合導(dǎo)航系統(tǒng)；自適應(yīng)衰減卡爾曼濾波；遺忘因子；噪聲模型

0 引言

捷捷聯(lián)慣性導(dǎo)航系統(tǒng) （strapdown inertial navigation system，SINS）作為一種自主式導(dǎo)航系統(tǒng)，已經(jīng)被廣泛應(yīng)用于艦船導(dǎo)航、飛機(jī)制導(dǎo)、水下導(dǎo)航等領(lǐng)域。GPS／SINS組合導(dǎo)航系統(tǒng)引入GPS的位置、速度信息為觀測(cè)量，克服了單一SINS的誤差隨時(shí)間積累的缺點(diǎn)。

GPS／SINS組合導(dǎo)航系統(tǒng)進(jìn)行數(shù)據(jù)融合時(shí)使用的是卡爾曼濾波（Kalman Filter，KF），卡爾曼濾波器是一種最優(yōu)估計(jì)，但是前提是系統(tǒng)的模型和隨機(jī)噪聲信息必須準(zhǔn)確已知。實(shí)際情況下，大部分系統(tǒng)的模型和隨機(jī)噪聲信息不完全可知，這可能會(huì)導(dǎo)致濾波器估計(jì)不準(zhǔn)確甚至發(fā)散。為此，有學(xué)者提出自適應(yīng)衰減卡爾曼濾波器（AdaptiveFadingKalmanFilter，AFKF）［1］，來(lái)解決系統(tǒng)系統(tǒng)噪聲不完全可知的問(wèn)題，但這些方法只針對(duì)系統(tǒng)噪聲不完全可知的情況，沒(méi)有考慮量測(cè)噪聲不完全可知的情況，并且?guī)в屑s束條件［2 3］。

針對(duì)上述問(wèn)題，本章提出基于新息協(xié)方差的改進(jìn)AFKF，來(lái)補(bǔ)償未知的噪聲信息造成的影響，改進(jìn)AFKF分別適用于系統(tǒng)噪聲不完全可知的情況和量測(cè)噪聲不完全可知的情況，并且二者具有統(tǒng)一的濾波框架。

1 自適應(yīng)衰減卡爾曼濾波算法

考慮如下線性離散時(shí)間隨機(jī)系統(tǒng)：

式中，xk是n×1狀態(tài)矢量，zk是m×1量測(cè)矢量，Φk是狀態(tài)轉(zhuǎn)移矩陣，Hk是量測(cè)矩陣。系統(tǒng)噪聲和量測(cè)噪聲的序列wk、vk都是高斯白噪聲，Qk、Rk為對(duì)應(yīng)的方差陣。

為了補(bǔ)償不完整噪聲信息的影響，將遺忘因子λk引入誤差協(xié)方差方程，形式如下：

定義1：

對(duì)于式（1）表示的線性隨機(jī)系統(tǒng)，系統(tǒng)噪聲統(tǒng)計(jì)模型不完整的情況，其離散時(shí)間自適應(yīng)衰減卡爾曼濾波器（AFKF）［4］為：

式中，ηk是濾波器的新息，Ck是計(jì)算的新息協(xié)方差，ˉCk是估計(jì)的新息協(xié)方差，M是窗口尺寸，Pk（－）是計(jì)算的KF的誤差協(xié)方差。

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

式（9）成立的前提條件是HkT列滿(mǎn)秩，并且只針對(duì)系統(tǒng)噪聲不完全可知的情況，為了避免這些限制，對(duì)求解遺忘因子的方法進(jìn)行改進(jìn)，提出改進(jìn)的自適應(yīng)衰減卡爾曼濾波。

當(dāng)前時(shí)刻的新息協(xié)方差的估計(jì)值：

新息主要由誤差決定，新息協(xié)方差可以很好地反應(yīng)當(dāng)前時(shí)刻誤差的影響，因此未知的系統(tǒng)噪聲信息會(huì)使誤差協(xié)方差和新息協(xié)方差都增加。在式（4）中量測(cè)方差Rk保持不變，這表明αk表征的新息協(xié)方差的增加是由λk表征的誤差協(xié)方差的增加導(dǎo)致的。Pk（－）的增加量可以補(bǔ)償系統(tǒng)噪聲信息不完整產(chǎn)生的影響，如果增加的誤差協(xié)方差大于Rk，可以認(rèn)為αk近似等于λk。那么，稱(chēng)這種AFKF為“調(diào)諧Pk的 AFKF”。

對(duì)于量測(cè)方程不完整的情況，同上述情況一樣，未知的量測(cè)方程信息會(huì)使誤差協(xié)方差和新息協(xié)方差都增加。這種情況下可以認(rèn)為αk表征的新息協(xié)方差的增加是由量測(cè)噪聲協(xié)方差Rk的增加導(dǎo)致的，誤差協(xié)方差Pk（－）保持不變。Rk的增加導(dǎo)致Kk的減少，Kk的減少量可以補(bǔ)償未知的量測(cè)方程信息產(chǎn)生的影響。假設(shè)取λk＝1，則誤差協(xié)方差保持不變ˉPk（－）＝Pk（－）。設(shè)計(jì)濾波器的增益變?yōu)樵瓉?lái)的，濾波增益的減小意味著更少的利用量測(cè)信息，濾波增益的減少使量測(cè)噪聲協(xié)方差Rk變?yōu)棣羕Rk，如式（5）所示。稱(chēng)這種AFKF為“調(diào)諧Rk的AFKF”。第二種方法得到的濾波增益為：

定義2：

對(duì)于式（1）表示的線性隨機(jī)系統(tǒng)，系統(tǒng)噪聲不完全可知的情況，即調(diào)諧Pk的AFKF，其離散時(shí)間自適應(yīng)衰減卡爾曼濾波器（AFKF）與定義1比較，需要把式（5）、（6）換成式（12）、（13）：

定義3：

對(duì)于式（1）表示的線性隨機(jī)系統(tǒng)，量測(cè)噪聲不完全可知的情況，即調(diào)諧Rk的AFKF，其離散時(shí)間自適應(yīng)衰減卡爾曼濾波器（AFKF）與定義1比較，需要把式（5）、（6）換成式（14）、（15）：

圖1表示的是改進(jìn)的AFKF的原理，該AFKF為系統(tǒng)噪聲信息不完整和量測(cè)噪聲信息不完整兩種情況提供了統(tǒng)一的濾波框架，利用新息求解的遺忘因子能夠自動(dòng)適應(yīng)并補(bǔ)償未知信息造成的影響。同時(shí)，該算法計(jì)算量較小，遺忘因子的計(jì)算較簡(jiǎn)單，能夠應(yīng)用于比較復(fù)雜的線性隨機(jī)系統(tǒng)。

圖1 AFKF原理框架圖

3 濾波模型

利用AFKF對(duì)系統(tǒng)的參數(shù)進(jìn)行估計(jì)時(shí)，首先要建立狀態(tài)方程和量測(cè)方程，具體過(guò)程如下：

將加速計(jì)零偏和陀螺漂移擴(kuò)展至狀態(tài)量中，狀態(tài)方程基本形式為：

式中，狀態(tài)量為：

狀態(tài)矩陣為：

選取慣導(dǎo)解算的位置與GPS提供的位置的差值作為觀測(cè)量，量測(cè)方程為：

式中，H＝［02×2diag（1，1） 02×3］；LI、λI為慣導(dǎo)的解算值；LG，λG為GPS測(cè)量值。

4 仿真分析

仿真初始條件設(shè)置如下：

1）陀螺和加速度計(jì)的輸出信號(hào)由捷聯(lián)慣性導(dǎo)航系統(tǒng)模擬器產(chǎn)生，陀螺常值漂移為0.01°／h，加速度計(jì)常值零偏為10－4g；系統(tǒng)噪聲和測(cè)量噪聲均設(shè)為高斯白噪聲。

2）卡爾曼濾波的初始條件：X（0）各分量都取0；

圖2 誤差對(duì)比圖

3）載體以Vx＝2 m／s，Vy＝2 m／s的速度勻速直行；導(dǎo)航開(kāi)始階段初始姿態(tài)角誤差：0.05°，0.05°，0.1°；初始經(jīng)度λ＝126.670 5°，初始緯度L＝45.779 6°。

設(shè)測(cè)量噪聲完全可知，系統(tǒng)噪聲不完全可知，則基于調(diào)諧Pk的AFKF的SINS／GPS組合導(dǎo)航系統(tǒng)仿真結(jié)果如圖2所示。

從圖2可以看出，與KF相比基于調(diào)諧Pk的ATKF的SINS／GPS組合導(dǎo)航系統(tǒng)導(dǎo)航參數(shù)收斂較快，收斂精度較高，尤其是位置誤差達(dá)到了很高的精度。

在相同仿真條件下，設(shè)系統(tǒng)噪聲完全可知，測(cè)量噪聲不完全可知，則基于調(diào)諧Rk的AFKF的SINS／GPS組合導(dǎo)航系統(tǒng)仿真結(jié)果如圖3所示。

圖3 誤差對(duì)比圖

從圖3可以看出，與KF相比基于調(diào)諧Rk的AFKF的SINS／GPS組合導(dǎo)航系統(tǒng)導(dǎo)航參數(shù)收斂較快，但位置誤差的收斂精度相比于基于調(diào)諧Pk的AFKF較低，這是由于位置噪聲的統(tǒng)計(jì)信息不準(zhǔn)確可知導(dǎo)致的，但相比于KF收斂精度較高。

5 結(jié)論

為了解決SINS／GPS組合導(dǎo)航系統(tǒng)中噪聲統(tǒng)計(jì)信息不完整造成的影響，提出改進(jìn)的自適應(yīng)衰減卡爾曼濾波。改進(jìn)的算法簡(jiǎn)化了遺忘因子的求解，可以分別適用于系統(tǒng)噪聲不完全可知和量測(cè)噪聲不完全可知兩種情況，并且二者具有統(tǒng)一的濾波框架。對(duì)算法的仿真結(jié)果表明，改進(jìn)的算法可以很好的補(bǔ)償噪聲信息不完全可知造成的影響，對(duì)提高SINS／GPS組合導(dǎo)航系統(tǒng)的導(dǎo)航精度有實(shí)際的意義。

［1］Kim KH，Lee J G，Chan G P.Adaptive Two－Stage Extended Kalman Filter for a Fault－Tolerant INS－GPS Loosely Coupled System［J］.IEEE Transactions on Aerospace＆Electronic Systems，2009，45（1）：125-137.

［2］Hu Y M，Qin Y Y.Notice of Retraction Adaptive two－stage Kalman filter in the presence of random bias［A］.2010 3rd IEEE International Conference on Computer Science and Information Technology（ICCSIT）［C］.IEEE，2010.

［3］Kim K H，Lee J G，Chan G P.Adaptive two－stage Kalman filter in the presence of unknown random bias［J］.International Journal of AdaptiveControl＆Signal Processing，2006，20（7）：305 -319.

［4］Hide C，Moore T，Smith M.Adaptive Kalman filtering algorithms for integrating GPS and low cost INS［A］.Position Location and Navigation Symposium 2004［C］.2004：227-233.

Improved Adaptive Fading Kalman Filter Algorithm

Gao Wei，Ye Pan，Xu Weitong

（Automation College，Harbin Engineering University，Harbin 150001，China）

The fusion algorithm of SINS／GPSintegrated navigation system is mainly based on Kalman filter.Kalman filter is the optimal estimation on the conditions that system model and random noise information are accurately known.In practice，most system model and random noise information are not completely known，which may lead to filter estimation accuracy decline.Aiming at this problem，this paper improves the traditional adaptive fading Kalman filter according to the method of solving forgetting factor，and proposes an improved adaptive fading Kalman filter.The improved algorithm respectively applies in the cases that system noise statistical model cannot be accurately known and measurement noise statistical models cannot be accurately known，respectively corresponding to the two filter algorithms.What’s more，they have a unified filter framework.The simulation results show that the improved adaptive fading Kalman filter is more accurate than Kalman filter and it can effectively solves the accuracy decline problem caused by the inaccurate noise model.

integrated navigation system；adaptive fading Kalman filter；forgetting factor；noise model

1671-4598（2016）08-0190-03

10.16526／j.cnki.11－4762／tp.2016.08.051

：U666.1

：A

2016-01-16；

：2016-02-11。

國(guó)家自然科學(xué)基金（51379042）；中央高校基本科研業(yè)務(wù)費(fèi)專(zhuān)項(xiàng)資金（heucfq1404）。

高 偉（1977-），男，黑龍江哈爾濱人，博士，教授，主要從事捷聯(lián)導(dǎo)航技術(shù)、光學(xué)陀螺技術(shù)、海洋運(yùn)載器綜合導(dǎo)航技術(shù)方向的研究。