基于雙漸消因子調(diào)節(jié)的自適應(yīng)卡爾曼濾波器

朱文超， 何 飛

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

0 引言

在已知系統(tǒng)和量測(cè)數(shù)學(xué)模型、噪聲統(tǒng)計(jì)特性、狀態(tài)初值的情況下，卡爾曼（Kalman）濾波器可利用輸出信號(hào)的量測(cè)數(shù)據(jù)獲取狀態(tài)變量的估計(jì)值，具有計(jì)算量小，可遞推、實(shí)時(shí)性高等特點(diǎn)［1］。然而，標(biāo)準(zhǔn)Kalman濾波器過(guò)分依賴于系統(tǒng)狀態(tài)空間模型的準(zhǔn)確性，當(dāng)狀態(tài)參數(shù)發(fā)生擾動(dòng)時(shí)，往往無(wú)法精確跟蹤系統(tǒng)突變狀態(tài)，甚至?xí)馂V波發(fā)散現(xiàn)象［2］。針對(duì)這一問題，文獻(xiàn)［3-4］在預(yù)測(cè)協(xié)方差中引入單漸消因子，通過(guò)Sage-Husa估計(jì)法重新解算量測(cè)殘差，增強(qiáng)新息在狀態(tài)估計(jì)中的權(quán)重。文獻(xiàn)［5］通過(guò)強(qiáng)跟蹤中心查分濾波器實(shí)時(shí)估計(jì)汽車前后軸等效時(shí)變側(cè)偏剛度，對(duì)標(biāo)稱模型的不精確與狀態(tài)擾動(dòng)等因素具有強(qiáng)魯棒性。文獻(xiàn)［6-7］在強(qiáng)跟蹤濾波器中引入了限定記憶的思想，通過(guò)次優(yōu)漸消因子修正濾波增益，增強(qiáng)跟蹤突變狀態(tài)的能力。文獻(xiàn)［8-9］將強(qiáng)跟蹤濾波器和UKF濾波器相融合，引入時(shí)變漸消因子調(diào)節(jié)卡爾曼增益，強(qiáng)迫輸出殘差序列保持正交，提取殘差有效信息，提升突變狀態(tài)的跟蹤能力。文獻(xiàn)［10］在預(yù)測(cè)協(xié)方差中融合弱化因子與漸消因子，依據(jù)模糊控制理論，膨脹狀態(tài)突變下的量測(cè)新息，提升跟蹤精度，已成功應(yīng)用于GPS定位解算領(lǐng)域。

然而，上述研究均未對(duì)狀態(tài)突變程度進(jìn)行等級(jí)劃分，致使收斂速度慢。針對(duì)這一問題，本文基于濾波發(fā)散判據(jù)，分析Sage窗量測(cè)新息與標(biāo)準(zhǔn)Kalman量測(cè)新息的關(guān)系，以儲(chǔ)備系數(shù)為等級(jí)，判斷狀態(tài)突變程度，利用雙漸消因子調(diào)整預(yù)測(cè)協(xié)方差，增權(quán)量測(cè)新息，提升突變狀態(tài)的估計(jì)精度。

1 Kalman濾波簡(jiǎn)介

1.1 線性離散系統(tǒng)狀態(tài)空間模型

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

式中，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)過(guò)程噪聲序列；Ψk，k-1為m×q維過(guò)程噪聲系數(shù)矩陣。Zk為j 維量測(cè)向量；Hk為j×m 維量測(cè)系數(shù)矩陣；Vk為j 維系統(tǒng)量測(cè)噪聲序列。

1.2 Kalman濾波魯棒性

Kalman濾波是一種線性、無(wú)偏且誤差方差最小的隨機(jī)系統(tǒng)最優(yōu)估計(jì)算法［11］，但魯棒性較差，無(wú)法精準(zhǔn)跟蹤系統(tǒng)突變狀態(tài)。究其原因，主要有2點(diǎn)：①狀態(tài)擾動(dòng)下的先驗(yàn)預(yù)測(cè)偏差較大；②穩(wěn)態(tài)Kalman濾波器的增益為定量，狀態(tài)突變時(shí)，無(wú)法實(shí)時(shí)增權(quán)量測(cè)新息。假設(shè)狀態(tài)擾動(dòng)（系統(tǒng)控制方式改變、噪聲特性變化等）于k歷元加載至穩(wěn)態(tài)系統(tǒng)，則突變信息將直接作用于狀態(tài)方程中的系數(shù)矩陣，如擾動(dòng)方程（3）中ΔΦk，k-1、ΔΓk，k-1、ΔΨk，k-1所示。此時(shí)，若仍依靠舊狀態(tài)方程進(jìn)行先驗(yàn)預(yù)測(cè)，必然產(chǎn)生較大的偏差。然而，穩(wěn)態(tài)Kalman的濾波增益無(wú)法實(shí)時(shí)增權(quán)量測(cè)新息，則最優(yōu)估計(jì)將逐漸偏離真實(shí)值，最終致使濾波器發(fā)散。

精確跟蹤系統(tǒng)突變狀態(tài)，避免濾波器發(fā)散的方法有2種：①分析狀態(tài)擾動(dòng)的統(tǒng)計(jì)特性，建立精確的數(shù)學(xué)模型，獲取準(zhǔn)確先驗(yàn)預(yù)測(cè)。②尋找激活穩(wěn)態(tài)濾波增益方法，當(dāng)狀態(tài)擾動(dòng)來(lái)臨時(shí)，實(shí)時(shí)增權(quán)測(cè)量新息。

然而，狀態(tài)擾動(dòng)種類繁多，統(tǒng)計(jì)特性復(fù)雜，難以準(zhǔn)確建立數(shù)學(xué)模型，故本文針對(duì)不同程度的突變狀態(tài)，利用雙漸消因子優(yōu)化預(yù)測(cè)協(xié)方差，激活穩(wěn)態(tài)濾波增益，采用實(shí)時(shí)調(diào)整量測(cè)新息在最優(yōu)估計(jì)中權(quán)重的策略，精確跟蹤系統(tǒng)突變狀態(tài)。

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

2.1 遞推規(guī)則設(shè)計(jì)

鑒于線性離散系統(tǒng)狀態(tài)空間模型，在Kalman遞推規(guī)則中，引入雙漸消因子，實(shí)時(shí)調(diào)節(jié)先驗(yàn)預(yù)測(cè)協(xié)方差，獲取自適應(yīng)Kalman濾波迭代公式。

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

狀態(tài)一步預(yù)測(cè)

一步預(yù)測(cè)協(xié)方差

觀測(cè)更新：

量測(cè)新息

濾波增益

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

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

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

2.2 雙漸消因子的求解

最優(yōu)Kalman濾波具備量測(cè)新息處處正交的特性，新息序列Y（k）的正交方程可用自相關(guān)函數(shù)表示為［12］

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

依據(jù)Kalman濾波準(zhǔn)則，化簡(jiǎn)式（11）為

若系統(tǒng)狀態(tài)未發(fā)生突變，則標(biāo)準(zhǔn)Kalman濾波器的預(yù)測(cè)協(xié)方差Pbasek，k-1可滿足正交方程

式中，上標(biāo)base代表標(biāo)準(zhǔn)Kalman濾波器產(chǎn)生的協(xié)方差矩陣。

然而，隨著時(shí)間的推移，Kalman濾波器逐漸進(jìn)入穩(wěn)態(tài)，濾波增益及后驗(yàn)誤差協(xié)方差的取值固定。倘若此時(shí)系統(tǒng)狀態(tài)發(fā)生突變，需激活濾波增益，利用優(yōu)化預(yù)測(cè)協(xié)方差滿足正交方程，則式（12）可化為

根據(jù)Sage開窗估計(jì)法［13］，由量測(cè)新息獲取Coptk在k 歷元下的估計(jì)值Ck，即

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

需要說(shuō)明的是，更新λk+1時(shí)，需將k歷元下的后驗(yàn)方差作為Pkbase，代入式（13）與式（14），求解Ckba+se1，判斷其與的關(guān)系，構(gòu)造邊界條件，再進(jìn)入k+1歷元的迭代估計(jì)階段。

綜合式（6）、式（14）、式（18）進(jìn)行矩陣的跡運(yùn)算，得到γk函數(shù)解析式

綜合式（17）～式（19），獲得因子λk與γk的終解，即

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

本節(jié)以中科院智能機(jī)械研究所自行研制的雙E 型彈性體六維力傳感器為靜態(tài)標(biāo)定對(duì)象，研究標(biāo)準(zhǔn)Kalman濾波器（SKF）、抗差Kalman濾波器［15］（RKF）、自適應(yīng)Kalman濾波器（AKF）的魯棒特性。

六維力傳感器標(biāo)定實(shí)驗(yàn)臺(tái)如圖1所示。依次從Fz方向標(biāo)定數(shù)據(jù)庫(kù)中抽取3組量測(cè)值進(jìn)行分析。其中，第一組為恒載輸出電壓（理想值25 m V）；第二組為卸載輸出電壓（理想值10 m V）；第三組為加載輸出電壓（理想值60 m V）。假設(shè)前47歷元，傳感器持續(xù)進(jìn)行恒載輸出。第48歷元時(shí)，分別進(jìn)行不同量級(jí)的卸載或加載操作，則系統(tǒng)狀態(tài)將發(fā)生不同程度突變，理想趨勢(shì)如圖2所示。狀態(tài)突變?cè)驗(yàn)閭鞲衅鞯目刂戚d荷發(fā)生變化，故k歷元下的擾動(dòng)方程（3）可化為式（21），分別利用SKF、RKF、AKF對(duì)輸出信號(hào)的量測(cè)數(shù)據(jù)進(jìn)行濾波處理，結(jié)果如圖3～圖10所示。

圖2 傳感器階躍輸出信號(hào)

圖3～圖5反映了傳感器輸出由恒載向卸載轉(zhuǎn)變時(shí)，3種算法的濾波效果。從圖4中可以看出，前47歷元，即系統(tǒng)狀態(tài)未突變前，量測(cè)新息序列滿足正交性要求，故AKF與RKF均退化為SKF。3種算法狀態(tài)收斂速度與精度均相同。

圖3 卸載前后3種算法跟蹤效果

圖4 卸載前3種算法跟蹤效果

然而，當(dāng)系統(tǒng)狀態(tài)在第48歷元發(fā)生突變時(shí)，如圖3所示，SKF算法未更新濾波增益，逐漸偏離真實(shí)狀態(tài)，估計(jì)誤差無(wú)限增大。反觀AKF與RKF，兩者均能有效地跟蹤系統(tǒng)突變狀態(tài)。然而，由于狀態(tài)突變程度較淺，AKF退化為RKF，兩者濾波精度相同，如圖5所示。

圖6反映了狀態(tài)突變前后漸消因子的時(shí)變趨勢(shì)。不難發(fā)現(xiàn)，因子γk處于冷卻狀態(tài)，AKF僅用因子λk進(jìn)行狀態(tài)調(diào)節(jié)。λk的時(shí)變趨勢(shì)分為5個(gè)階段。

（1）階段1（47歷元至48歷元）。狀態(tài)于此階段突變，突變信息作用于量測(cè)新息協(xié)方差，致使估計(jì)值驟然變大，λk由冷卻驟變?yōu)榧せ顟B(tài)。

（2）階段2（48歷元至49歷元）。該階段的先驗(yàn)估計(jì)涵蓋了量測(cè)新息，其效果優(yōu)于階段1。然而，依據(jù)圖5所示，跟蹤初期的后驗(yàn)誤差大于突變前的穩(wěn)態(tài)誤差。故減小，Ckbase增大，λk處于急速下降狀態(tài)。

（3）階段3（49歷元至71歷元）。該階段狀態(tài)后驗(yàn)誤差開始減小，估計(jì)精度開始提升，則與Ckbase均開始減小，λk的下降速率減緩。

（4）階段4（71歷元至76歷元）。該階段狀態(tài)后驗(yàn)誤差緩慢減小，估計(jì)精度緩慢提升，則與Ckbase均緩慢減小，λk的下降速率進(jìn)一步減緩。

（5）階段5（76歷元至100歷元）。該階段后驗(yàn)誤差穩(wěn)定，則λk僅在穩(wěn)態(tài)值范圍進(jìn)行微調(diào)。

圖5 卸載后3種算法跟蹤效果

圖6 卸載后AKF漸消因子時(shí)變趨勢(shì)

圖7～圖8反映了傳感器輸出由恒載向加載轉(zhuǎn)變時(shí)，3種算法的濾波效果。當(dāng)系統(tǒng)狀態(tài)在第48歷元發(fā)生突變時(shí)，SKF濾波發(fā)散。RKF僅利用因子λk調(diào)節(jié)濾波增益，增權(quán)量測(cè)新息，能夠有效跟蹤突變狀態(tài)。然而，AKF 的總體濾波效果優(yōu)于RKF。尤其在第72 歷元至第100 歷元階段，AKF 狀態(tài)收斂速度優(yōu)于RKF。

圖7 加載前后3種算法跟蹤效果

圖8 加載后3種算法跟蹤效果

從圖9～圖10可以看出，狀態(tài)突變后，AKF持續(xù)激活雙漸消因子λk與γk，能夠更深層次地利用量測(cè)新息糾正估計(jì)值。加載環(huán)境下，γk的時(shí)變趨勢(shì)分為4個(gè)階段；λk的時(shí)變趨勢(shì)分為6個(gè)階段。

圖9 加載后AKF因子λk 時(shí)變趨勢(shì)

圖10 加載后AKF因子γk 時(shí)變趨勢(shì)

γk的時(shí)變趨勢(shì)解析：

（1）階段1（47 歷元至48 歷元）。狀態(tài)于此階段突變，突變信息體現(xiàn)于量測(cè)數(shù)據(jù)，致使后驗(yàn)協(xié)方差PLSW48驟然變大，γk由冷卻驟變?yōu)榧せ顟B(tài)。

（2）階段2（48歷元至49歷元）。該階段的先驗(yàn)估計(jì)涵蓋了量測(cè)新息，先驗(yàn)誤差Pbase48，48的精度回升，則相較于階段1，γk處于急速下降狀態(tài)。

（3）階段3（49歷元至72歷元）。該階段后驗(yàn)誤差開始減小，先驗(yàn)估計(jì)精度穩(wěn)步提升，則相較于階段2，γk的下降速率減緩。

（4）階段4（72歷元至100歷元）。該階段狀態(tài)后驗(yàn)誤差繼續(xù)減小，第72歷元與Cbkase之間的關(guān)系到達(dá)臨界條件，γk由激活變?yōu)槔鋮s態(tài)。

λk的時(shí)變趨勢(shì)解析：

（1）階段1、階段2、階段3的趨勢(shì)分析同卸載環(huán)境λk。

（2）階段4（72歷元至81歷元）。該階段γk處于冷卻狀態(tài)，則Ckbase的衰減速率變慢。相較于階段3，λk的下降速率加快。

（3）階段5（81歷元至94歷元）。該階段后驗(yàn)誤差緩慢減小，估計(jì)精度緩慢提升與Cbkase均緩慢減小，相較于階段4，λk的下降速率有所減緩。

（4）階段6（94歷元至100歷元）。該階段后驗(yàn)誤差穩(wěn)定，則λk僅在穩(wěn)態(tài)值范圍進(jìn)行微調(diào)。

通過(guò)式（22）解算72歷元至第100歷元3種算法估計(jì)結(jié)果的均方根誤差X~kRMSE，詳見表1。結(jié)果顯示，SKF誤差較大，處于發(fā)散狀態(tài)。相較于RKF，AKF穩(wěn)態(tài)精度提升了44．76%。

表1 濾波算法性能對(duì)比

4 結(jié)論

為解決標(biāo)準(zhǔn)Kalman濾波無(wú)法實(shí)時(shí)精確估計(jì)突變狀態(tài)的問題，設(shè)計(jì)了雙漸消因子自適應(yīng)Kalman濾波器。面對(duì)不同程度的狀態(tài)突變，采用合適的漸消因子組合，實(shí)時(shí)調(diào)節(jié)預(yù)測(cè)協(xié)方差，變權(quán)量測(cè)新息。實(shí)驗(yàn)結(jié)果表明，自適應(yīng)Kalman濾波器，具有較強(qiáng)的魯棒性，穩(wěn)態(tài)精度優(yōu)于抗差Kalman濾波，可運(yùn)用于六維力傳感器靜態(tài)標(biāo)定領(lǐng)域。然而，本文僅研究標(biāo)量漸消因子，對(duì)于多維度漸消因子的求解方法，還有待于進(jìn)一步深入研究。