基于固定點迭代的Huber魯棒容積卡爾曼濾波算法

李 松，劉 哲，唐小妹，吳 健，王飛雪

國防科技大學(xué) 電子科學(xué)學(xué)院，長沙410073

非線性系統(tǒng)的狀態(tài)估計是許多領(lǐng)域中廣泛面臨的問題，如目標(biāo)跟蹤、導(dǎo)航以及信號和圖像處理[1]。擴展卡爾曼濾波（Extended Kalman Filter，EKF）通過一階泰勒展開式的線性化過程來近似非線性系統(tǒng)，在工程中應(yīng)用廣泛。但是，當(dāng)系統(tǒng)非線性較強時，其性能可能會降低甚至發(fā)散，因為EKF忽略了泰勒展開的高階項，從而只能獲得系統(tǒng)的一階逼近精度[2]。粒子濾波[3]（Particle Filter，PF）作為一種序貫蒙特卡洛方法，通過大量的粒子來近似狀態(tài)的概率分布，能夠獲得更高的估計精度，但其實時性較差且存在粒子退化和粒子貧乏等問題[4]。

為了提高非線性系統(tǒng)的估計精度，近年來，以無跡卡爾曼濾波[5]（Unscented Kalman Filter，UKF）和容積卡爾曼濾波[6]（Cubature Kalman Filter，CKF）為代表的基于確定性采樣的方法受到廣泛關(guān)注。與EKF不同，這類方法通過使用一組確定的加權(quán)采樣點在非線性系統(tǒng)中傳播[7]，能夠精確地捕獲非線性系統(tǒng)的統(tǒng)計特性，從而可以達到三階的逼近精度[8]，并且這類方法無需計算雅克比矩陣，具有無需求導(dǎo)的優(yōu)點。但是，UKF在處理高維系統(tǒng)時可能會不穩(wěn)定，因為其中心采樣點的權(quán)重可能為負(fù)[9]，為了避免非正定協(xié)方差矩陣的傳播，需要對UKF中的參數(shù)進行仔細(xì)的調(diào)整[5]。與UKF不同，CKF使用一組等權(quán)重的容積點來計算非線性變換后隨機變量的均值和協(xié)方差[6]，其估計精度和數(shù)值穩(wěn)定性均優(yōu)于UKF，尤其是在處理高維非線性系統(tǒng)時[9-10]。

然而，UKF與CKF本質(zhì)上都是基于最小化誤差的l2范數(shù)的原則，即基于最小均方誤差（Minimum Mean Square Error，MMSE）準(zhǔn)則進行推導(dǎo)[11]，因此當(dāng)誤差的實際分布與假設(shè)的高斯分布不相符時，兩者的性能都會隨之下降[12]。Huber[13]提出一種基于最小化誤差的l1/l2范數(shù)的方法（即Huber方法），通過使得最大漸進估計的方差最小，能夠提高系統(tǒng)在干擾高斯分布的噪聲下的魯棒性，因此該方法的魯棒性優(yōu)于基于l2范數(shù)的估計方法[14]。Karlgaard等[15]將量測方程進行線性化，并將Huber方法應(yīng)用于差分濾波。Wang等[16]同樣采用線性化的方法將Huber方法應(yīng)用于UKF。通過將Huber方法應(yīng)用于非線性濾波算法，證明了該方法能夠增強濾波算法在非高斯噪聲下的魯棒性。Chang等[17]指出利用Huber方法重構(gòu)量測信息（量測噪聲協(xié)方差矩陣）能夠使得濾波算法更加魯棒。秦康等[12]從貝葉斯估計的角度解釋了Huber方法使濾波算法魯棒的原因是通過對新息進行截斷平均。

如前所述，CKF相比于UKF而言估計精度和數(shù)值穩(wěn)定性更好，因此本文將Huber方法應(yīng)用于CKF中。首先通過對量測方程進行線性化近似，將系統(tǒng)方程構(gòu)造為線性回歸模型，然后采用固定點迭代的方法求解基于Huber方法的最小化問題，推導(dǎo)了基于固定點迭代的Huber魯棒CKF（Fixed-Point Iterated Huber-based robust Cubature Kalman Filter，F(xiàn)P-IHCKF）算法。分析算法可以看出，Huber方法重構(gòu)了量測更新過程中的先驗信息和量測信息，使得所提算法能夠減輕非高斯噪聲對于狀態(tài)估計的影響。對再入目標(biāo)跟蹤問題進行仿真，仿真結(jié)果表明，與傳統(tǒng)方法相比，所提算法在非高斯噪聲下魯棒性更強且估計精度更高。

1 容積卡爾曼濾波算法

考慮如下離散非線性系統(tǒng)：

其中，下標(biāo)k≥0為離散化時間；xk∈?n和zk∈?m分別為狀態(tài)向量和量測向量；f(?)和h(?)分別為非線性的狀態(tài)轉(zhuǎn)移函數(shù)和量測函數(shù)；wk-1∈?n和vk∈?m分別為過程噪聲和量測噪聲，假定兩者為相互獨立的零均值高斯白噪聲，并滿足：

其中，Qk和Rk分別為過程噪聲和量測噪聲的協(xié)方差矩陣。

CKF算法通過在給定模型中傳播一組等權(quán)重的容積點來實現(xiàn)非線性變換，其濾波過程可以總結(jié)如下：

（1）濾波初始化

設(shè)置濾波初值x?0及相應(yīng)的誤差協(xié)方差矩陣P0：

（2）時間更新

①計算基本容積點?i：

其中，ei表示n×n維單位矩陣的第i列。

②基于濾波值x?k-1計算容積點Xi,k-1：

④計算k時刻的狀態(tài)預(yù)測值x?k/k-1及相應(yīng)的誤差協(xié)方差矩陣Pk/k-1：

（3）量測更新

①基于預(yù)測值x?k/k-1計算容積點Xi,k/k-1：

②計算在量測方程中傳播后的容積點Zi,k/k-1：

③計算k時刻的量測預(yù)測值z?k/k-1、新息協(xié)方差矩陣Pzz,k/k-1以及互相關(guān)協(xié)方差矩陣Pxz,k/k-1：

④計算k時刻的狀態(tài)估計值x?k及相應(yīng)的誤差協(xié)方差矩陣Pk：

2 基于Huber方法的魯棒CKF

2.1 線性回歸模型

在狀態(tài)預(yù)測值x?k/k-1處對式（1）中的量測方程進行線性化近似得：其中為線性化量測矩陣。

因此，基于式（1）表示的非線性系統(tǒng)，可構(gòu)造如下線性回歸模型：

注意到，式（19）中Hk為EKF中使用的線性化量測矩陣，考慮到統(tǒng)計線性化量測矩陣可以由式（15）和式（9）求得，具有無需求導(dǎo)且能夠保留CKF高估計精度的優(yōu)點，因此本文用Hˉk代替Hk。根據(jù)統(tǒng)計線性化量測矩陣Hˉk可得量測協(xié)方差矩陣為：

而由文獻[17]知，CKF的量測協(xié)方差矩陣為：

其中，Pηη為統(tǒng)計線性化誤差協(xié)方差矩陣。因此，直接使用Hˉk來完成后續(xù)的量測更新實際上會導(dǎo)致協(xié)方差傳遞過程中的精度損失[18]，從而降低濾波算法的估計精度[12，17，19]。為避免此問題，對量測噪聲的協(xié)方差矩陣進行如下修改[20]：

因此，φk的協(xié)方差矩陣修改為：

其中，

2.2 基于Huber方法的估計準(zhǔn)則

KF和CKF本質(zhì)上都是基于MMSE準(zhǔn)則，即x?k通過最小化如下的l2范數(shù)得到：

其中，ξk,i為ξk的第i個分量。因此CKF無法有效應(yīng)對非高斯噪聲的情況，在實際工程應(yīng)用中，若噪聲不滿足高斯分布的假設(shè)，則CKF的估計精度將降低甚至發(fā)散。

為了增強濾波算法的魯棒性，x?k可通過最小化如下的代價函數(shù)得到：

其中，ρ(?)為有界凸函數(shù)。故隨著殘差ξk,i增大，ρ函數(shù)不會像l2范數(shù)一樣迅速增長，因此幅度較大的、偏離高斯分布的異常值在代價函數(shù)中的權(quán)重相對更小，從而可以減輕狀態(tài)估計值對于非高斯噪聲的敏感程度，使得濾波算法更加魯棒。常用的ρ函數(shù)為如下所示的Huber函數(shù)[21]：

其中，γ為調(diào)節(jié)因子，通常可取為1.345[22]。對于較小的τ,ρ函數(shù)具有l(wèi)2范數(shù)的特點，因此能夠保證高斯噪聲下的估計精度，而對于較大的τ,ρ函數(shù)增長較為緩慢，因此能夠抑制異常值和重尾噪聲的影響[23]。Huber函數(shù)結(jié)合了最小l1范數(shù)估計的魯棒性和最小l2范數(shù)估計的諸多優(yōu)勢，魯棒性較強[11]。

2.3 FP-IHCKF算法推導(dǎo)

對式（31）關(guān)于xk求偏導(dǎo)，并令其為0得：

因此，式（33）可以進一步寫為：

根據(jù)式（26）知ξk=yk-Mk xk，因此式（35）可進一步寫為如下的矩陣形式：

由式（36）可得：

其中，

xk通常未知，而由式（34）可知Ψk是關(guān)于xk的函數(shù)，因此式（37）本質(zhì)上是一個關(guān)于xk的迭代方程：

本文采用固定點迭代的方法求解式（37），即

將式（25）、（27）、（28）和（38）代入式（37）可進一步得：

定義如下變量：

并將其代入式（43）得：

對式（45）使用矩陣求逆引理，式（45）可等效為下式：

其中，

通過上述分析，可將FP-IHCKF算法流程總結(jié)如下：

（1）濾波初始化

同傳統(tǒng)CKF一樣，利用式（3）設(shè)置濾波初值。

（2）時間更新

同傳統(tǒng)CKF一樣，利用式（4）~式（9）完成時間更新過程。

（3）量測更新

①令迭代次數(shù)為t=1，并設(shè)初始迭代估計值，其中表示第t次迭代時的估計值；

其中，

其中，In為n×n維單位矩陣；‖ ‖x表示求取向量x的二范數(shù)；?為迭代門限，本文取?=10-5。

2.4 算法收斂性及魯棒性分析

文獻[24]表明，如果ψ函數(shù)不增加（對于ξk,i>0），則迭代過程將收斂，而式（32）中使用的ρ函數(shù)能夠滿足該條件。值得說明的是，當(dāng)γ→∞時，式（32）中的Huber函數(shù)退化為l2范數(shù)的形式，且FP-IHCKF也退化為CKF。此時，Ψk→I，式（36）僅需一次迭代過程即可求解。

從FP-IHCKF算法流程可以看出，所提算法采用迭代的方式進行量測更新，在迭代過程中通過ψ函數(shù)重構(gòu)了先驗信息Pk/k-1和量測信息Rk，使得在偏離高斯分布的異常值出現(xiàn)時，濾波算法能夠相應(yīng)地調(diào)整對于狀態(tài)預(yù)測值和量測值的置信度，進而降低異常值在濾波中的“貢獻”，從而能夠減輕非高斯噪聲對于狀態(tài)估計的影響，使得算法更加魯棒。特別地，當(dāng)僅有量測噪聲非高斯時，所提算法可以只迭代一次，此時僅有量測信息被重構(gòu)。

需要注意的是，在FP-IHCKF中新息為zk-h(x?k/k-1)（式（48））而在CKF中新息為zk-z?k/k-1（式（17）），因此這里沒有利用到非線性變換的優(yōu)勢。對于非線性較強的量測方程而言，將FP-IHCKF中的新息替換成zk-z?k/k-1能夠進一步提高估計精度，本文對此不做進一步討論。

3 仿真與分析

3.1 仿真模型

對再入目標(biāo)跟蹤問題[25]進行仿真，仿真示意圖如圖1所示。目的是估計目標(biāo)從很高的高度高速重新進入大氣時的位置xt(1)（單位ft）、速度xt(2)（單位ft/s）以及一個常值系數(shù)xt(3)（單位1/s）。狀態(tài)方程為：

圖1 再入目標(biāo)跟蹤問題示意圖Fig.1 Diagram of re-entry target tracking problem

其中，[wt(1),wt(2),wt(3)]T為高斯白噪聲，其協(xié)方差Q=0；常值ρ0=2為海平面的空氣密度；k=2×104為定義空氣密度和高度間關(guān)系的一個常數(shù)；g=32.2 ft/s2為重力加速度。

對式（57）中的狀態(tài)方程x?t=g(xt)采用歐拉積分法，以步長Δt=0.01 s進行離散化，即：

測量雷達測量其到目標(biāo)之間的距離。

量測方程為：其中，雷達的高度為H=105ft；雷達到目標(biāo)下降鉛垂線的水平距離為M=105ft；vk為量測噪聲。

仿真中，雷達量測周期為T=0.5 s。初始狀態(tài)真實值和估計值以及初始誤差協(xié)方差矩陣分別為：

3.2 對比實驗

為了比較算法性能，采用均方根誤差（Root Mean Square Error，RMSE）以及時間平均RMSE（Time-Averaged RMSE，TARMSE）作為評估指標(biāo)。RMSE和TARMSE分別定義為：

分別比較以下幾種算法：PF（粒子數(shù)為1 000）、EKF、Huber-EKF（HEKF）、CKF以及本文的FP-IHCKF。為了比較不同線性化量測矩陣的影響，F(xiàn)P-IHCKF考慮兩種情況，采用線性化量測矩陣記為FPIHCKF1，采用統(tǒng)計線性化量測矩陣記為FP-IHCKF2。

首先，假定量測噪聲服從如下高斯分布：

圖2 ~圖4給出了高斯噪聲下各算法的RMSE結(jié)果。由于各算法在30 s后趨于穩(wěn)定，故計算30~60 s的TARMSE結(jié)果，由于T=0.5 s，因此k1=60，k2=120，TARMSE結(jié)果如表1所示。

圖2 高斯噪聲下的位置RMSEFig.2 RMSE of position under Gaussian noise

圖3 高斯噪聲下的速度RMSEFig.3 RMSE of velocity under Gaussian noise

圖4 高斯噪聲下的系數(shù)RMSEFig.4 RMSE of coefficient under Gaussian noise

表1 高斯噪聲下各算法TARMSETable 1 TARMSE for each algorithm under Gaussian noise

從圖2~圖4和表1可以看出，PF對目標(biāo)位置的估計性能表現(xiàn)較差，造成這種情況的一個可能原因是，當(dāng)無法直接測量某些狀態(tài)量時，PF可能表現(xiàn)不佳（在仿真中，只有雷達到目標(biāo)的距離被測量）。CKF和FP-IHCKF相比于EKF和HEKF而言，估計精度更高。FP-IHCKF2的估計精度優(yōu)于FP-IHCKF1，這是因為前者能夠更好地保留CKF高估計精度的優(yōu)點。注意到，HEKF和FPIHCKF的估計精度稍差于EKF和CKF，這是因為后兩者基于MMSE準(zhǔn)則，能夠在高斯噪聲條件下得到最優(yōu)的估計性能，而前兩者采用Huber代價函數(shù)推導(dǎo)得到，因此在高斯噪聲條件下是次優(yōu)的。

接著，假定量測噪聲服從如下高斯混合分布：

其中，ε為混合高斯分布中的干擾因子[11]，本文取ε=0.1。

圖5 ~圖7給出了非高斯噪聲下各算法的RMSE結(jié)果，TARMSE結(jié)果如表2所示。

圖5 非高斯噪聲下的位置RMSEFig.5 RMSE of position under non-Gaussian noise

圖6 非高斯噪聲下的速度RMSEFig.6 RMSE of velocity under non-Gaussian noise

圖7 非高斯噪聲下的系數(shù)RMSEFig.7 RMSE of coefficient under non-Gaussian noise

表2 非高斯噪聲下各算法TARMSETable 2 TARMSE for each algorithm under non-Gaussian noise

從圖5~圖7和表2可以看出，EKF和CKF的估計精度相比于高斯噪聲條件下明顯降低。PF相對而言受到的影響較小，但其對于位置的估計性能仍然不佳。HEKF和FP-IHCKF相比于EKF和CKF而言估計精度更高，證明了這些算法在非高斯噪聲條件下的魯棒性。FPIHCKF2仍然能夠獲得優(yōu)于FP-IHCKF1的估計性能。

下面比較上述幾種算法的計算復(fù)雜度，仿真軟件為MATLAB，所有算法均運行于處理器為Intel Core i5-4200H（2.80 GHz）、內(nèi)存為8 GB的PC上。設(shè)CKF的運算時間為1，各算法的相對運算時間如表3所示。

表3 各算法相對CKF的運算時間Table 3 CKF-relative computation ratios

從表3可以看出，EKF消耗的運算時間最少，PF消耗的運算時間最多。HEKF和FP-IHCKF相比于EKF和CKF而言，消耗的運算時間更多，這是因為這兩者的量測更新中增加了額外的計算。注意到，F(xiàn)P-IHCKF2消耗的運算時間多于FP-IHCKF1，這是因為FP-IHCKF2采用的是統(tǒng)計線性化量測矩陣，而FP-IHCKF1采用的是線性化量測矩陣，前者的量測更新中涉及到2n個容積點的運算，能更好地保留CKF高估計精度的優(yōu)點，但是以略微犧牲計算復(fù)雜度為代價的。

4 結(jié)束語

針對非高斯噪聲下CKF估計精度下降的問題，本文將Huber方法應(yīng)用于CKF框架中，并采用固定點迭代的方法求解狀態(tài)的后驗估計值，提出了FP-IHCKF算法。與傳統(tǒng)方法不同，所提算法結(jié)合了CKF算法高估計精度以及Huber方法魯棒性的優(yōu)點。算法理論分析與仿真實驗結(jié)果驗證了所提算法的有效性和魯棒性。