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

國(guó)防科技大學(xué) 電子科學(xué)學(xué)院，長(zhǎng)沙410073

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

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

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

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

1 容積卡爾曼濾波算法

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

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

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

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

（1）濾波初始化

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

（2）時(shí)間更新

①計(jì)算基本容積點(diǎn)?i：

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

②基于濾波值x?k-1計(jì)算容積點(diǎn)Xi,k-1：

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

（3）量測(cè)更新

①基于預(yù)測(cè)值x?k/k-1計(jì)算容積點(diǎn)Xi,k/k-1：

②計(jì)算在量測(cè)方程中傳播后的容積點(diǎn)Zi,k/k-1：

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

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

2 基于Huber方法的魯棒CKF

2.1 線(xiàn)性回歸模型

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

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

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

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

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

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

其中，

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

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

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

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

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

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

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

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

因此，式（33）可以進(jìn)一步寫(xiě)為：

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

由式（36）可得：

其中，

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

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

將式（25）、（27）、（28）和（38）代入式（37）可進(jìn)一步得：

定義如下變量：

并將其代入式（43）得：

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

其中，

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

（1）濾波初始化

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

（2）時(shí)間更新

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

（3）量測(cè)更新

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

其中，

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

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

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

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

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

3 仿真與分析

3.1 仿真模型

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

圖1 再入目標(biāo)跟蹤問(wèn)題示意圖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)系的一個(gè)常數(shù)；g=32.2 ft/s2為重力加速度。

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

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

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

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

3.2 對(duì)比實(shí)驗(yàn)

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

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

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

圖2 ~圖4給出了高斯噪聲下各算法的RMSE結(jié)果。由于各算法在30 s后趨于穩(wěn)定，故計(jì)算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對(duì)目標(biāo)位置的估計(jì)性能表現(xiàn)較差，造成這種情況的一個(gè)可能原因是，當(dāng)無(wú)法直接測(cè)量某些狀態(tài)量時(shí)，PF可能表現(xiàn)不佳（在仿真中，只有雷達(dá)到目標(biāo)的距離被測(cè)量）。CKF和FP-IHCKF相比于EKF和HEKF而言，估計(jì)精度更高。FP-IHCKF2的估計(jì)精度優(yōu)于FP-IHCKF1，這是因?yàn)榍罢吣軌蚋玫乇Ａ鬋KF高估計(jì)精度的優(yōu)點(diǎn)。注意到，HEKF和FPIHCKF的估計(jì)精度稍差于EKF和CKF，這是因?yàn)楹髢烧呋贛MSE準(zhǔn)則，能夠在高斯噪聲條件下得到最優(yōu)的估計(jì)性能，而前兩者采用Huber代價(jià)函數(shù)推導(dǎo)得到，因此在高斯噪聲條件下是次優(yōu)的。

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

其中，ε為混合高斯分布中的干擾因子[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的估計(jì)精度相比于高斯噪聲條件下明顯降低。PF相對(duì)而言受到的影響較小，但其對(duì)于位置的估計(jì)性能仍然不佳。HEKF和FP-IHCKF相比于EKF和CKF而言估計(jì)精度更高，證明了這些算法在非高斯噪聲條件下的魯棒性。FPIHCKF2仍然能夠獲得優(yōu)于FP-IHCKF1的估計(jì)性能。

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

表3 各算法相對(duì)CKF的運(yùn)算時(shí)間Table 3 CKF-relative computation ratios

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

4 結(jié)束語(yǔ)

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