基于變權新息協方差的自適應卡爾曼濾波器

朱文超， 何 飛

(1.中國電子科技集團公司第三十八研究所， 安徽 合肥 230041； 2.中國科學技術大學， 安徽 合肥 230027；3.中國科學院合肥智能機械研究所， 安徽 合肥 230031)

卡爾曼(Kalman)濾波器具有實時性高、數據存儲量小、估計精度高等特點，已廣泛運用于數字信號處理、故障聯合診斷、目標跟蹤等領域[1]。然而，傳統(tǒng)Kalman濾波器依賴于系統(tǒng)模型的準確性，當狀態(tài)參數發(fā)生擾動時，通常無法精確跟蹤系統(tǒng)突變狀態(tài)，嚴重時會發(fā)生發(fā)散現象[2]。針對這一問題，文獻[3-4]針對SINS/DVL組合導航系統(tǒng)，利用Sage-Husa估計原理確定量測新息協方差的取值準則，并實時解算漸消因子，調整預測協方差。文獻[5-6]將強跟蹤思想融入自適應容積Kalman濾波器，提升其魯棒特性，解決因船舶及飛行器運動模型偏差導致濾波精確度下降的問題。文獻[7-8] 在強跟蹤濾波器中引入了限定記憶理論，形成區(qū)間Kalman濾波器，通過漸消因子修正濾波增益，增強跟蹤突變狀態(tài)的能力。文獻[9]利用交互多模型算法與衰減記憶Kalman濾波，實時更新衰減因子，調整濾波增益，并成功運用于預警機運動跟蹤領域。文獻[10] 基于模糊控制理論模型，動態(tài)調整弱化因子與漸消因子，膨脹量測新息，降低估計誤差，并成功應用于GPS定位解算領域中。

然而，上述研究均未對狀態(tài)突變程度進行分層，致使狀態(tài)收斂速度慢。為解決該問題，本文在濾波發(fā)散判據的基礎上，梳理儲備系數與量測新息協方差的關系，對狀態(tài)突變程度進行劃分，針對不同幅度的狀態(tài)突變，利用變權新息協方差求解漸消因子，提升了估計精度，能實時跟蹤系統(tǒng)突變狀態(tài)。

1 Kalman濾波

1.1 線性離散時間系統(tǒng)

k歷元下，線性離散時間系統(tǒng)狀態(tài)方程和量測方程可表示為

Xk=Φk，k-1Xk-1+Γk，k-1Uk-1+Ψk，k-1Wk-1

(1)

Zk=HkXk+Vk

(2)

式中：Xk是m維狀態(tài)向量；Φk，k-1是m×m維k-1歷元過渡至k歷元的狀態(tài)轉移矩陣；Uk-1是p維輸入控制量；Γk，k-1是m×p維控制輸入系數矩陣；Wk-1是q維系統(tǒng)過程噪聲序列；Ψk，k-1是m×q維過程噪聲系數矩陣；Zk是j維量測向量；Hk是j×m維量測系數矩陣；Vk是j維系統(tǒng)量測噪聲序列。

1.2 傳統(tǒng)Kalman濾波魯棒性分析

假設狀態(tài)擾動(控制參數突變、噪聲統(tǒng)計特性變化等)于k歷元加載至穩(wěn)態(tài)系統(tǒng)，則真實狀態(tài)將發(fā)生突變，突變信息首先表現在量測新息，作用于狀態(tài)模型中的系數矩陣，如擾動方程(3)中ΔΦk，k-1、ΔΓk，k-1、ΔΨk，k-1。若仍依靠舊模型進行狀態(tài)估計，則必然產生較大的偏差。然而，穩(wěn)態(tài)系統(tǒng)的濾波增益為定值，無法實時增權量測新息，估計量將逐漸偏離真實值，最終發(fā)散。

Xk=(Φk，k-1+ΔΦk，k-1)Xk-1+(Γk，k-1+ΔΓk，k-1)Uk-1+

(Ψk，k-1+ΔΨk，k-1)Wk-1

(3)

精確跟蹤系統(tǒng)突變狀態(tài)，避免濾波發(fā)散的方法有2種：1)研究擾動參數特性，構建精確的數學模型，獲取準確預測估計；2)實時激活濾波增益，在狀態(tài)擾動環(huán)境下，增權量測新息。

然而，擾動種類繁多，特性復雜，難以準確建立數學模型。本文在分析狀態(tài)突變程度的基礎上，綜合考慮跟蹤精度及算法時耗，利用變權量測新息協方差解算漸消因子，激活濾波增益，增權量測新息，實時精確跟蹤系統(tǒng)突變狀態(tài)。

2 自適應Kalman濾波

2.1 自適應Kalman遞推公式

在傳統(tǒng)Kalman濾波的基礎上，引入漸消因子λk優(yōu)化預測估計協方差，結合線性離散時間系統(tǒng)模型，獲取自適應Kalman濾波遞推公式。

1)狀態(tài)預測。

狀態(tài)預測估計

(4)

預測估計協方差

(5)

2)觀測更新。

量測新息

(6)

濾波增益

(7)

最優(yōu)估計

(8)

后驗協方差

(9)

式中：Qk-1、Rk分別為過程噪聲Wk-1與量測噪聲Vk的協方差矩陣。

2.2 漸消因子解算

線性最優(yōu)Kalman濾波最重要的一個特點是當濾波增益最優(yōu)時，量測新息序列Yk處處正交。自相關函數[12]可表示為

(10)

式中：Ck為量測新息協方差矩陣，上標opt代表最優(yōu)矩陣。

依據Kalman濾波遞推公式，化簡式(10)，為

(11)

(12)

(13)

式中：上標base代表傳統(tǒng)Kalman濾波器產生的協方差矩陣。

(14)

(15)

聯立式(12)、式(14)、式(15)進行矩陣的跡運算，獲取λk的函數解析式

(16)

(17)

(18)

式中：ξk-j為各歷元新息協方差權值；bk為k歷元下的突變程度系數；[N/μ0]為取整函數。

(19)

(20)

綜合式(16)—(20)，獲取漸消因子λk的三段式函數模型，即

(21)

3 試驗驗證

本文以中科院智能機械研究所自行研制的雙E型彈性體六維力傳感器為研究對象，在靜態(tài)標定環(huán)境下，研究傳統(tǒng)Kalman濾波器(traditional Kalman filter，TKF)、抗差Kalman濾波器[15](robust Kalman filter，RKF)、自適應Kalman濾波器(adaptive Kalman filter，AKF)的魯棒特性。

六維力傳感器標定實驗臺如圖1所示。依次從Fz方向標定數據庫中抽取恒載、卸載、加載3種控制方式的量測數據進行分析。其中：恒載輸出的理論值為25 mV；卸載輸出的理論值為10 mV；加載輸出的理論值為60 mV。取限定記憶窗口長度N=8；儲備系數μ0=2。假設前47歷元，傳感器持續(xù)進行恒載輸出，第48歷元時，分別進行卸載或加載操作，狀態(tài)將產生不同程度的突變，理想突變趨勢如圖2所示。由于系統(tǒng)狀態(tài)突變的原因為傳感器載荷變化，即輸入控制發(fā)生變化，故k歷元下的擾動方程(3)可化為式(22)，分別利用TKF、RKF、AKF對量測值進行濾波處理，結果如圖3—7所示。

Xk=Xk-1+(Uk-1+ΔUk-1)+Wk-1

(22)

圖 1 六維力傳感器標定實驗臺

圖 2 理想狀態(tài)突變信號

圖 3 恒載至卸載3種算法跟蹤效果

圖3—5反映了恒載轉變至卸載環(huán)境下，3種算法的跟蹤效果。從圖4中可以看出，前47歷元，系統(tǒng)處于穩(wěn)態(tài)環(huán)境，AKF與RKF均退化為TKF。3種算法狀態(tài)收斂速度與估計精度均相同。

系統(tǒng)狀態(tài)在第48歷元發(fā)生突變，TKF算法逐漸偏離真實狀態(tài)，后驗誤差無限增大，如圖3所示。反觀AKF與RKF，兩者均能有效地跟蹤系統(tǒng)突變狀態(tài)。由于狀態(tài)突變程度較淺，故AKF采用均權新息協方差求解λk。此時AKF退化為RKF，兩者估計性能相同，如圖5所示。

圖 4 恒載3種算法跟蹤效果

圖 5 卸載后RKF與AKF跟蹤效果

圖6—7反映了恒載轉變至加載環(huán)境下，3種算法的跟蹤效果。系統(tǒng)狀態(tài)在第48歷元突變，TKF濾波發(fā)散。RKF利用均權漸消因子增權量測信息，有效跟蹤突變狀態(tài)。然而，AKF調整了各歷元新息協方差的權值，在保證平滑性的同時，深層次利用當前量測新息跟蹤狀態(tài)，濾波性能優(yōu)于RKF。尤其在第78歷元至第100歷元階段，AKF狀態(tài)收斂速度優(yōu)于RKF。從表1中可以看出，相較于RKF，AKF穩(wěn)態(tài)精度提升了42.05%

圖 6 恒載至加載3種算法跟蹤效果

圖 7 加載后RKF與AKF跟蹤效果

濾波算法突變后誤差精度提升RKFAKF2.645 21.532 9—42.05%

4 結論

為解決傳統(tǒng)Kalman濾波無法精確跟蹤系統(tǒng)突變狀態(tài)的問題，設計了自適應Kalman濾波器，針對不同的狀態(tài)突變程度，采用變權新息協方差求解漸消因子，實時激活濾波增益，增權量測新息。實驗表明，所述自適應Kalman濾波器具有較強健的魯棒性，穩(wěn)態(tài)精度優(yōu)于抗差Kalman濾波。然而，本文僅分析了標量漸消因子的求解方法，對于多維漸消因子的優(yōu)化策略，還有待于進一步深入研究。