后向平滑平方根容積卡爾曼濾波算法及其應用

張 智，姜秋喜，潘繼飛

（解放軍電子工程學院，安徽 合肥230037）

0 引言

非線性濾波來源于含噪聲觀測值非線性系統(tǒng)的狀態(tài)估計問題［1］，廣泛應用于眾多工程領域。其最優(yōu)解需要對系統(tǒng)狀態(tài)的后驗概率分布進行完整描述，但這幾乎是不可能實現(xiàn)的。因此，當前的研究著重于各種次優(yōu)濾波算法。無跡卡爾曼濾波（Unscented Kalman Filter，UKF）［2-4］通過選取確定的加權采樣點來逼近隨機變量的概率分布，其不僅避免了雅克比矩陣的計算，還使精度至少達到二階，優(yōu)于擴展卡爾曼濾波及其衍生算法。而容積卡爾曼濾波（Cubature Kalman Filter，CKF）［5-7］是一種新型的非線性濾波方法，其利用等權值的容積點對后驗概率密度進行近似，具有更好的估計性能。同時，相比于UKF，CKF無需選擇任何參數(shù)，算法實現(xiàn)更加簡單。其中，文獻［3］利用狀態(tài)方程對相鄰時刻狀態(tài)的約束關系以及量測更新，為再次前向濾波提供了更精確的初始值，從而提高了濾波算法的估計性能。文獻［7］利用當前時刻的濾波結果，通過后向平滑得到上一時刻更加精確的狀態(tài)估計值，并以此進行二次前向濾波，提高了濾波算法的精度和收斂速度。

然而，在單站無源定位的實際應用中，由于可觀測性弱、觀測噪聲大以及數(shù)值計算舍入誤差等因素影響，容易造成定位精度低、收斂速度慢和濾波性能不穩(wěn)定甚至不能工作等問題。針對這些問題，本文在文獻［3］及文獻［7］的啟發(fā)下，提出了后向平滑平方根CKF 算 法（Backward Smoothing Square-Root CKF，BSSRCKF）。

1 容積卡爾曼濾波算法

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

其 中，xk+1為系統(tǒng)狀態(tài)向量，yk為觀測值，f和h為非線性函數(shù)，B 為控制矩陣，wk和vk為零均值、協(xié)方差分別為Q 和R 的加性高斯白噪聲。

容積卡爾曼濾波算法［5］由高斯域貝葉斯濾波理論推導而來，在該理論框架下，其可歸結為非線性函數(shù)與高斯概率密度函數(shù)乘積的數(shù)值積分問題，即：

其中，g 為已知的非線性函數(shù)，Rn為積分區(qū)域，為權函數(shù)。

容積卡爾曼濾波采用三階球面-徑向容積準則，利用2n個容積點加權求和來逼近形如式（3）的加權高斯積分，即：

容積點ξi（i=1，2，…，m）及其對應的權值ωi（i=1，2，…，m）分別為：

其中，m=2n為容積點數(shù)，n為狀態(tài)的維數(shù)。

2 后向平滑平方根容積卡爾曼濾波算法

2.1 基于Q-R 分解的SRCKF 算法

基于Q-R分解的SRCKF算法［6］采用誤差協(xié)方差矩陣的平方根代替協(xié)方差矩陣進行遞推運算，能夠提高濾波算法的運行效率和數(shù)值穩(wěn)定性。該算法的具體流程如下：

1）初始化，通過觀測或其他先驗信息確定初始的狀態(tài)矢量^x0以及估計誤差協(xié)方差矩陣的平方根S0，即：

其中，chol表示Cholesky分解。

2）時間更新

①估計容積點的值（i=1，2，…，m）：

②傳播容積點：

③計算狀態(tài)預測值：

④計算狀態(tài)預測誤差協(xié)方差的平方根：

其中，qr表示Q-R 分解取下三角矩陣。

3）量測更新

①估計容積點的值（i=1，2，…，m）：

②傳播容積點：

③計算量測估計值：

④計算矩陣T11、T21以及T22：

其中，d為觀測量的個數(shù)，T11、T22分別為d×d維、m×m維下三角矩陣，T21為m×d維矩陣，0d×m、0m×d分別為d×m 維、m×d 維零矩陣。

⑤計算卡爾曼濾波增益：

其中，符號／表示右除。

⑥計算狀態(tài)估計值：

⑦計算狀態(tài)估計誤差協(xié)方差矩陣的平方根：

2.2 基于Q-R 分解的后向平滑及量測更新

Rauch-Tung-Striebel（RTS）后 向 平 滑［8］是 一種固定區(qū)間平滑算法，采用遞推的形式，計算簡單、易于實現(xiàn)。其在前向濾波過程中，計算并存儲系統(tǒng)的狀態(tài)轉移矩陣以及每一時刻的狀態(tài)值、估計誤差協(xié)方差矩陣，并以此作為后向平滑遞推公式的輸入值，從而獲得最優(yōu)的平滑估計結果。

然而，RTS后向平滑由于求取協(xié)方差矩陣時涉及兩正定矩陣相減，有可能破壞協(xié)方差矩陣的正定性和對稱性。故本文采用基于Q-R分解的RTS后向平滑算法［9］，使用協(xié)方差矩陣的平方根進行遞推運算，提高了數(shù)值穩(wěn)定性以及求逆運算的計算效率。

為了進一步改善算法的性能，本文在RTS后向平滑的基礎上，利用上一時刻的觀測值信息進行量測更新，并將獲得的上一時刻更加精確的估計值作為第二次前向濾波的初始值，從而提高算法的精度和收斂速度。該算法的具體流程如下：

1）計算矩陣U11、U21以及U22：

其中，U11、U22為n×n維下三角矩陣，U21為n×n維矩陣，0n×n為n×n維零矩陣。

2）計算平滑增益：

3）計算平滑預測值：

4）計算狀態(tài)預測誤差協(xié)方差矩陣的平方根：

再根據(jù)式（15）— 式（22）進行量測更新，可以得到k-1時刻更加精確的

2.3 第二次前向濾波

綜上所述，本文提出的后向平滑SRCKF 算法的實現(xiàn)流程如圖1所示，其中①、②、③、④表示算法的運算順序，其執(zhí)行過程具體為：

1）當k≥1時，按式（7）—式（22）得到當前k時刻的狀態(tài)估計值；

2）根據(jù)式（23）— 式（27）進行RTS后向平滑，得到k-1時刻的狀態(tài)預測值；

本算法利用k時刻SRCKF的濾波結果通過后向平滑并利用k-1時刻的觀測值進行量測更新，由于同時利用了k時刻與k-1時刻的觀測值信息，其能夠獲得k-1時刻更加精確的狀態(tài)估計值，以此作為初始條件再次進行前向濾波就能得到當前k時刻更加準確的估計值，提高了算法的定位精度與收斂速度。同時，采用Q-R分解的形式，并使用誤差協(xié)方差的平方根進行運算，從而提高了新算法的穩(wěn)定性與運算效率。

圖1 后向平滑SRCKF算法流程示意圖Fig.1 The operation process of backward smoothing SRCKF

3 單站無源定位仿真實驗

3.1 狀態(tài)方程與觀測方程

觀測站與目標輻射源的位置關系如圖2所示，在三維直角坐標系中，k 時刻觀測站O 的狀態(tài)矢量為，目標輻射源T 的狀態(tài)矢量為兩者之間的徑向距離為rk，方位角為βk，俯仰角為εk，相對運動狀態(tài)矢量為xk=xTk-xOk= ［xk，yk，，以 目 標 輻 射 源 的 方 位 角βk，俯 仰 角εk，相位差變化率和多普勒頻率變化率作為觀測量，則系統(tǒng)的狀態(tài)方程和觀測方程可分別表示為：

圖2 觀測站與目標的位置關系示意圖Fig.2 Geometrical relation between observer and target

3.2 仿真實驗及結果分析

假設在三維直角坐標系中，觀測站固定于原點，目標輻射源的初始位置為（190，160，10）km，速度為（-280，160，0）m／s。為了驗證BSSRCKF算法的性能，在不同的觀測精度下將其同CKF、文獻［7］所提出的BSCKF算法的性能進行對比，其中各組參數(shù)觀測精度如下：在仿真實驗中，觀測周期T=1s，觀測次數(shù)N=100，系統(tǒng)誤差為wxk=wyk=wzk=1m／s2，輻射源信號的載頻fT=10GHz，觀測站中相互正交干涉儀的基線長度分別為dx=10m，dy=5m。采用相對距離誤差（Relative Range Error，RRE）作為評價標準，每一組做100 次Monte Carlo實驗，在定位結束時刻RRE＜15%，則視作本次實驗收斂，否則視為發(fā)散。定位精度為跟蹤結束時刻RRE的平均值，如表1、表2以及圖3— 圖5所示（剔除了不收斂的實驗結果）。

其中，（xk，yk，zk）表示目標k時刻的真實位置表示目標k時刻位置的估計值。

表1 不同算法的定位精度與穩(wěn)定性比較Tab.1 Comparison of algorithms locating accuracy and robustness

圖3 觀測精度1時，各算法的相對位置誤差曲線Fig.3 Relative range error curves of each algorithm in measurement precision 1

圖4 觀測精度2時，各算法的相對位置誤差曲線Fig.4 Relative range error curves of each algorithm in measurement precision 2

圖5 觀測精度3時，各算法的相對位置誤差曲線Fig.5 Relative range error curves of each algorithm in measurement precision 3

從表1、圖3—圖5可以看出，在高精度觀測時，各算法都能很快收斂，且定位精度都比較高，但隨著觀測精度的降低，各算法的定位精度開始下降，收斂速度變慢，且濾波發(fā)散的次數(shù)開始增多。相比之下，BSSRCKF的性能最好，其他依次為BSCKF 以及CKF，且在低觀測精度時各算法的性能差別更明顯。由圖5 可以看出，當觀測精度低時，由于BSCKF采用RTS后向平滑為二次前向濾波提供了更為精確的初始值，故無論是定位精度還是收斂速度都優(yōu)于CKF。而BSSRCKF 在后向平滑的基礎上，利用前一時刻的觀測值進行量測更新，進一步提高了二次前向濾波的初始值，更加改善了定位的性能。

從表1可以看出，當觀測精度降低時，BSCKF由于在后向平滑過程中兩正定矩陣相減容易失去正定性與對稱性，造成濾波結果發(fā)散甚至無法運行。而ISRUKFS采用Q-R 分解的形式，使用誤差協(xié)方差的平方根進行遞推運算，避免了正定矩陣相減的運算，故穩(wěn)定性優(yōu)于其他兩種算法。

表2 不同算法的單次運行時間比較Tab.2 Comparison of algorithms single running time

在Intel酷睿雙核，CPU 主頻2.6GHz，內(nèi)存2GB的計算機使用Matlab7.10運行各算法，分別得到單次運行時間如表2所示，可見由于BSCKF與CKF相比，增加了后向平滑和二次前向濾波，計算量是后者的2倍多。相比之下，BSSRCKF雖然采用了Q-R分解的形式，提高了運算效率，但增加的量測更新處理使得其計算量接近CKF的3倍。然而，在保證實時性的前提下，這種適當增加計算量來換取定位精度、收斂速度以及穩(wěn)定性的較大改善是值得的。

4 結論

本文提出了后向平滑平方根容積卡爾曼濾波算法，并應用于單站無源定位領域。該算法利用當前時刻的濾波結果通過后向平滑算法得到上一時刻的預測值，并采用該時刻的觀測值進行量測更新，為再次前向濾波提供了更加精確的起始值。同時，其采用Q-R 分解的形式，并使用誤差協(xié)方差的平方根代替協(xié)方差參與濾波和平滑。仿真實驗表明，該算法在滿足實時性要求的基礎上，提高了單站無源定位的精度、收斂速度以及穩(wěn)定性。特別地，該算法是以增加算法復雜度的代價來提高性能，當對定位精度要求較高但觀測精度較低時，在保證實時性的前提下，這種以增加計算量來換取定位精度的方法是可行的。因此，本文提出的算法對單站無源定位系統(tǒng)的研究具有一定的工程應用價值，同時也適用于其他非線性濾波領域。

［1］Ito K，Xiong K.Gaussian filters for nonlinear filtering problems［J］.IEEE Transactions on Automatic Control，2000，45（5）：910-927.

［2］Julier S J，Uhlman J K.Unscented filter and nonlinear estimation［J］.Proceedings of IEEE，2004，92（3）：401-422.

［3］張剛兵，劉渝，胥嘉佳.基于UKF的單站無源定位與跟蹤雙向預測濾波算法［J］.系統(tǒng)工程與電子技術，2010，32（7）：1415-1418.

［4］黃耀光，高博，李建新，等.基于平方根UKF雙向濾波的單站無源定位算法［J］.數(shù)據(jù)采集與處理，2013，28（2）：207-212.

［5］Arasaratnam I，Haykin S.Cubature kalman filters［J］.IEEE Transactions on Automatic Control，2009，54（6）：1254-1269.

［6］Arasaratnam I，Haykin S，Hurd T R.Cubature kalman filtering for continuous-discrete systems：theory and simulations［J］.IEEE Transactions on Signal Processing，2010，58（10）：4977-4993.

［7］霍光，李冬海.基于后向平滑容積卡爾曼濾波的單站無源定位算法［J］.信號處理，2013，29（1）：68-74.

［8］Rauch H E，Tung F C，Striebel T.Maximum likelihood estimates of linear dynamic system［J］.AIAA Journal，1965，80（3）：1445-1450.

［9］Arasaratnam I，Haykin S.Cubature kalman smoothers［J］.Automatica，2011，47（10）：2245-2250.