基于變權(quán)新息協(xié)方差的自適應(yīng)卡爾曼濾波器

朱文超， 何 飛

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

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

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

1 Kalman濾波

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

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

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)轉(zhuǎn)移矩陣；Uk-1是p維輸入控制量；Γk，k-1是m×p維控制輸入系數(shù)矩陣；Wk-1是q維系統(tǒng)過程噪聲序列；Ψk，k-1是m×q維過程噪聲系數(shù)矩陣；Zk是j維量測(cè)向量；Hk是j×m維量測(cè)系數(shù)矩陣；Vk是j維系統(tǒng)量測(cè)噪聲序列。

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

假設(shè)狀態(tài)擾動(dòng)(控制參數(shù)突變、噪聲統(tǒng)計(jì)特性變化等)于k歷元加載至穩(wěn)態(tài)系統(tǒng)，則真實(shí)狀態(tài)將發(fā)生突變，突變信息首先表現(xiàn)在量測(cè)新息，作用于狀態(tài)模型中的系數(shù)矩陣，如擾動(dòng)方程(3)中ΔΦk，k-1、ΔΓk，k-1、ΔΨk，k-1。若仍依靠舊模型進(jìn)行狀態(tài)估計(jì)，則必然產(chǎn)生較大的偏差。然而，穩(wěn)態(tài)系統(tǒng)的濾波增益為定值，無法實(shí)時(shí)增權(quán)量測(cè)新息，估計(jì)量將逐漸偏離真實(shí)值，最終發(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)研究擾動(dòng)參數(shù)特性，構(gòu)建精確的數(shù)學(xué)模型，獲取準(zhǔn)確預(yù)測(cè)估計(jì)；2)實(shí)時(shí)激活濾波增益，在狀態(tài)擾動(dòng)環(huán)境下，增權(quán)量測(cè)新息。

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

2 自適應(yīng)Kalman濾波

2.1 自適應(yīng)Kalman遞推公式

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

1)狀態(tài)預(yù)測(cè)。

狀態(tài)預(yù)測(cè)估計(jì)

(4)

預(yù)測(cè)估計(jì)協(xié)方差

(5)

2)觀測(cè)更新。

量測(cè)新息

(6)

濾波增益

(7)

最優(yōu)估計(jì)

(8)

后驗(yàn)協(xié)方差

(9)

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

2.2 漸消因子解算

線性最優(yōu)Kalman濾波最重要的一個(gè)特點(diǎn)是當(dāng)濾波增益最優(yōu)時(shí)，量測(cè)新息序列Yk處處正交。自相關(guān)函數(shù)[12]可表示為

(10)

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

依據(jù)Kalman濾波遞推公式，化簡(jiǎn)式(10)，為

(11)

(12)

(13)

式中：上標(biāo)base代表傳統(tǒng)Kalman濾波器產(chǎn)生的協(xié)方差矩陣。

(14)

(15)

聯(lián)立式(12)、式(14)、式(15)進(jìn)行矩陣的跡運(yùn)算，獲取λk的函數(shù)解析式

(16)

(17)

(18)

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

(19)

(20)

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

(21)

3 試驗(yàn)驗(yàn)證

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

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

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

(22)

圖 1 六維力傳感器標(biāo)定實(shí)驗(yàn)臺(tái)

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

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

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

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

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

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

圖6—7反映了恒載轉(zhuǎn)變至加載環(huán)境下，3種算法的跟蹤效果。系統(tǒng)狀態(tài)在第48歷元突變，TKF濾波發(fā)散。RKF利用均權(quán)漸消因子增權(quán)量測(cè)信息，有效跟蹤突變狀態(tài)。然而，AKF調(diào)整了各歷元新息協(xié)方差的權(quán)值，在保證平滑性的同時(shí)，深層次利用當(dāng)前量測(cè)新息跟蹤狀態(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 結(jié)論

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