湯霞清， 黃湘遠， 武 萌， 張 環(huán)

(裝甲兵工程學院控制工程系，北京 100072)

平方根形式的CKF/后向平滑非線性濾波研究

湯霞清， 黃湘遠， 武 萌， 張 環(huán)

(裝甲兵工程學院控制工程系，北京 100072)

為了提高非線性濾波的精度、數(shù)值穩(wěn)定性并保證計算速度，將非線性平方根后向平滑 (Square Root Backward Smoothing, SR-BS) 算法和平方根容積卡爾曼濾波(Square Root Cubature Kalman Filter, SR-CKF)引入到前向濾波-后向平滑-前向濾波的濾波框架中，形成平方根形式的CKF/后向平滑(SR-BS-CKF)非線性濾波算法。該算法在首次前向濾波的基礎(chǔ)上利用后向平滑-前向濾波的思想來提高濾波精度，利用平方根更新來保證濾波的數(shù)值穩(wěn)定性；該算法包含2次SR-CKF和1次SR-BS運算，由于SR-BS計算量較小，因此該算法計算量為SR-CKF的2倍左右，增加不大。仿真結(jié)果表明：SR-BS-CKF算法濾波精度高，數(shù)值穩(wěn)定性強，計算速度較快，濾波性能優(yōu)于CKF。

容積卡爾曼濾波；非線性后向平滑；平方根濾波；濾波性能

基于量測信息和系統(tǒng)方程的最優(yōu)估計理論在導航、信息處理、金融等領(lǐng)域獲得了大量的應(yīng)用。根據(jù)利用的量測信息的時間區(qū)別，可分為預測、濾波和平滑3種[1]。

濾波是利用當前時刻以及以前的量測信息對當前狀態(tài)進行估計，典型算法有卡爾曼濾波及其非線性形式，如:擴展卡爾曼濾波(Extended Kalman Filter,EKF)、無損卡爾曼濾波(Unscented Kalman Filter,UKF)、容積卡爾曼濾波(Cubature Kalman Fiter, CKF)等[2]。CKF是一種計算量小、精度高的非線性濾波算法，能夠?qū)崿F(xiàn)任意高斯非線性系統(tǒng)的狀態(tài)估計[3-4]。平方根CKF (Square Root CKF, SR-CKF)利用平方根矩陣代替協(xié)方差矩陣，降低計算機字長限制對數(shù)據(jù)處理精度的影響，避免協(xié)方差矩陣Cholesky分解，提高了算法的數(shù)值穩(wěn)定性[5]。

平滑是一種離線處理算法，其利用以前、當前、以后的量測信息進行狀態(tài)估計，估計精度理論上優(yōu)于濾波?；诰€性狀態(tài)的平滑算法[6-7]已經(jīng)獲得了大量應(yīng)用，而基于非線性狀態(tài)的平滑算法[8-9]應(yīng)用還較少。

為了提高濾波精度，曹意等[10]將非線性濾波和線性平滑結(jié)合起來，形成多次迭代和窗口平滑算法；霍光等[11]在此基礎(chǔ)上，從計算量和濾波精度考慮，提出了后向平滑CKF (Backward Smoothing CKF, BS-CKF)算法。以上2種算法的本質(zhì)依然是濾波算法，而不是平滑算法，只是借用了平滑算法的思想；而且2種算法均基于線性狀態(tài)方程推導，具有一定的局限性。

為了提高非線性狀態(tài)的濾波精度，本文重新推導了非線性狀態(tài)的后向平滑算法，提出了平方根形式的后向平滑算法(Square Root Backward Smoothing，SR-BS)，將SR-CKF和SR-BS算法引入到前向濾波-后向平滑-前向濾波框架[10]中，形成SR-BS-CKF非線性濾波算法，最后通過仿真試驗對該算法進行驗證。

1 平方根容積卡爾曼濾波

非線性濾波就是基于非線性系統(tǒng)方程，根據(jù)當前及此前的觀測量序列估計出當前系統(tǒng)狀態(tài)?？紤]如下離散非線性系統(tǒng)：

(1)

式中：xk和zk分別為k時刻系統(tǒng)的狀態(tài)量和觀測量；f(·)和h(·)為已知任意函數(shù)；系統(tǒng)噪聲wk～N(wk;0,Qk)，其中Qk為系統(tǒng)噪聲方差矩陣；觀測噪聲vk～N(vk;0,Rk)，其中Rk為觀測噪聲方差矩陣，且wk和vk互相獨立。

1.1 球面徑向容積規(guī)則

I(f)=∫Rnf(x)exp(-xTx)dx。

(2)

式中：Rn為積分區(qū)域。

取x=ry，yTy=1，r∈[0,∞)，積分式I(f)在Spherical-radial坐標系可寫成

(3)

式中：Un為n維單位球面；σ(·)為Un上的元素。

積分I(f)可分離為球面積分S(r)和徑向積分R，其中:

S(r)=∫Unf(ry)dσ(y);

三階球面徑向容積規(guī)則由三階球面規(guī)則和一階徑向規(guī)則組成[3]，基本思路是分別對S(r)和R進行多項式逼近，利用兩者的逼近方式對積分I(f)進行逼近。對于高斯分布x～N(x;μ,Σ)，I(f)的三階近似規(guī)則為

(4)

CKF采用球面徑向容積規(guī)則和Guass-Bayes濾波框架，利用2n個等權(quán)值的容積點來實現(xiàn)非線性積分的多項式逼近，避免了非線性模型的線性化，能夠?qū)崿F(xiàn)任意高斯非線性系統(tǒng)的狀態(tài)估計，濾波精度達到三階。

1.2 平方根濾波算法

常規(guī)CKF需對Pk-1和Pk|k-1進行Cholesky分解，這是一項計算量大且非常危險的工作。因為受計算機字長有限、初始矩陣P0設(shè)置不當?shù)纫蛩氐挠绊懀琍k-1和Pk|k-1易失去正定性，Cholesky分解無法進行，導致濾波精度急劇下降甚至濾波發(fā)散。SR-CKF是一種有效的解決方法，其降低了計算機字長限制對數(shù)據(jù)處理精度的影響，提高了算法的數(shù)值穩(wěn)定性[4-5]。

1.2.1 時間更新

1.2.2 量測更新

1) 重采樣及觀測傳播：

zi,k|k-1=h(xi,k|k-1)。

3) 計算Pzz的均方根Szz和Pxz：

4) 估計濾波增益陣，最優(yōu)估計及協(xié)方差陣：

2 非線性平方根后向平滑算法

2.1 非線性狀態(tài)后向平滑算法

后向平滑算法根據(jù)當前及此前的觀測量序列及前向濾波獲得的最優(yōu)狀態(tài)估計序列進行后向遞推，來獲得此前任意時刻狀態(tài)的最優(yōu)估計。后向平滑分為固定區(qū)間平滑、固定點平滑和固定滯后平滑[10]。三者之間沒有本質(zhì)的區(qū)別，主要區(qū)別在于針對的對象不同?；诰€性系統(tǒng)的平滑算法結(jié)構(gòu)簡單、易于實現(xiàn)，獲得了較多的應(yīng)用。

首先，本文不加證明地給出定理1[9]。

定理1:設(shè)x、a、b是3個具有前二階矩的隨機矢量，令z=[ab]，則x基于z的最優(yōu)估計為

(5)

(6)

(7)

將式(6)、(7)分別改寫為如下形式：

(8)

將式(10)代入式(8)，由于Pk|k-1和Ck|k-1理論上均為可逆正定矩陣，則有

(11)

則k到k-1時刻的一步后向平滑狀態(tài)為

(12)

(13)

協(xié)方差陣Pk-1|k為

(14)

(15)

由式(12)、(15)構(gòu)成了非線性系統(tǒng)的一步后向平滑算法。

2.2 平方根后向平滑算法(SR-BS)

前文推導了非線性狀態(tài)前向濾波的平方根算法和后向一步平滑算法，為了將兩者結(jié)合起來，需推導后向平滑的平方根算法。

首先考慮濾波增益陣Wk的平方根算法，即

(16)

考慮到Pk|k-1為對稱正定陣，則有

(17)

從而獲得

(18)

將式(18)代入式(15)，可得

(19)

可得Pk-1|k的平方根矩陣Sk-1|k為

(20)

3 CKF/后向平滑非線性濾波

3.1 前向濾波-后向平滑-前向濾波

前向濾波是一種實時狀態(tài)估計方法，后向平滑過程是一種精度優(yōu)于前向濾波的事后狀態(tài)估計方法，因為前向濾波能夠消除大部分誤差，經(jīng)后向平滑后再次前向濾波可進一步消除誤差，兩者結(jié)合起來可獲得一種高精度的實時濾波方法。曹意等[10]推導了UKF和線性平滑的多次迭代和窗口平滑濾波算法，證明了2種方案均能夠提高估計精度，但是濾波精度并不一定隨著迭代次數(shù)或窗口長度的增加而增加。

圖1 濾波算法基本框架

3.2 平方根形式的CKF/后向平滑濾波算法

霍光等[11]基于線性狀態(tài)方程設(shè)計了CKF/后向平滑濾波器，本文在此基礎(chǔ)上考慮到SR-CKF和SR-BS的強數(shù)值穩(wěn)定性，以及對非線性狀態(tài)的適用性，將兩者引入到前向濾波-后向平滑-前向濾波框架中，形成平方根形式的CKF/后向平滑非線性濾波器(SR-BS-CKF)，實現(xiàn)任意高斯非線性模型的高精度實時濾波。基本濾波框架如下：

4 仿真實驗

考慮一個典型的飛行器控制問題[4]，其非線性離散方程為

(21)

x0=(1 000 m,300 m/s,1 000 m,0 m/s,-3°/s)T,

10 mrad/s)]2。

(22)

圖2給出了200次仿真實驗的位置、速度、角速度的RMSE曲線，考慮到濾波器穩(wěn)定時間，只給出了50～100 s的誤差曲線。圖2表明：2種算法均能對狀態(tài)進行有效估計，SR-BS-CKF的估計精度高于SR-CKF，其中位置估計體現(xiàn)得最明顯，速度估計次之，角速度估計相差不大。表1給出了2種算法后50 s位置、速度和角速度的RMSE平均值，經(jīng)計算可知：SR-BS-CKF算法的位置、速度和角速度估計精度比SR-CKF算法分別提高了10%、4%、1%左右，表明SR-BS-CKF算法對提高濾波精度有效。

圖2 2種濾波算法的RMSE曲線對比

表1 2種濾波算法的RMSE均值

濾波算法位置/m速度/(m·s-1)角速度/((°)·s-1)SR-CKF24.978914.09391.4096SR-BS-CKF22.910913.61391.4018

表2給出了一次仿真實驗(100 s)2種濾波算法的運算時間，可以看出：SR-BS-CKF所用時間為SR-CKF的2～3倍。因為每次濾波過程中，SR-BS-CKF需進行2次SR-CKF及1次SR-BS操作，而SR-BS的計算量相對SR-CKF較小，因此濾波運算主要集中在2次SR-CKF操作上。試驗結(jié)果表明：SR-BS-CKF算法的計算量相對于SR-CKF算法增長不大，濾波實時性的實現(xiàn)并不是非常困難。

表2 2種濾波算法的運算時間

5 結(jié)論

為了提高CKF的濾波精度和數(shù)值穩(wěn)定性并保證濾波實時性，本文將SR-BS、SR-CKF引入到前向濾波-后向平滑-前向濾波的框架中，形成了一種新的非線性濾波，能夠適用于任何高斯非線性系統(tǒng)。為了進一步提高濾波精度，有以下3種可能的方式：1) 利用高階非線性濾波代替常規(guī)CKF；2) 使用“多步后向平滑-前向濾波”的框架；3) 進行多次“一步后向平滑-前向濾波”，形成迭代算法。高精度的濾波方式必然帶來較大的計算量，影響濾波的實時性，如何在兩者之間找到一個平衡點需進一步研究。

[1] 宮曉琳，張蓉，房建成. 固定區(qū)間平滑算法及其在組合導航系統(tǒng)中的應(yīng)用[J]. 中國慣性技術(shù)學報，2012，20(6)：687-693.

[2] 王小旭，潘泉，黃鶴，等. 非線性系統(tǒng)確定采樣型濾波算法綜述[J]. 控制與決策，2012，27(6)：801-812.

[3] 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.

[4] Arasaratnam I, Haykin S. Cubature Kalman Filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[5] 郝燕玲，楊峻巍，陳亮，等.基于SRCKF的水下航行器動基座初始對準技術(shù)[J]. 華中科技大學學報：自然科學版，2012，40(2)：123-127.

[6] 康泰鐘，鐘麥英，李建利. 基于時間雙向解算融合的POS事后處理算法[J]. 儀器儀表學報，2012，33(9)：2067-2072.

[7] 秦顯平，楊元喜.平方根濾波/平滑/雙向濾波在LEO星載GPS定軌中的應(yīng)用[J]. 武漢大學學報：信息科學版，2009，34(10)： 1176-1180.

[8] Simo S. Unscented Rauch-Tung-Striebel Smoother[J]. IEEE Transactions on Automatic Control, 2008,53(3):845-849.

[9] Wang X X, Pan Q, Liang Y, et al. Application of Unscented Transformation for Nonlinear State Smoothing [J]. Acta Automatica Sinica, 2012,38(7):1097-1101.

[10] 曹意，茅旭初.基于非線性濾波和平滑相迭代的GPS定位估計算法[J]. 上海交通大學學報，2009，43(7)：1108-1112.

[11] 霍光，李冬梅. 基于后向平滑容積卡爾曼濾波的單站無源定位算法[J]. 信號處理，2013，29(1)：68-74.

[12] Ito K, Xiong K Q. Gaussian Filter for Nonlinear Filtering Problem[J]. IEEE Transactions on Automatic Control, 2000, 45(5):910- 927.

(責任編輯:尚彩娟)

Research on Square Root CKF/Backward Smoothing Nonlinear Filter

TANG Xia-qing, HUANG Xiang-yuan, WU Meng, ZHANG Huan

(Department of Control Engineering, Academy of Armored Force Engineering, Beijing 100072, China)

In order to achieve higher precision and stronger numeric stability and an efficient computation, the paper introduces nonlinear Square Root Backward Smoothing (SR-BS) algorithm and Square Root Cubature Kalman Filter (SR-CKF) into the framework of forward filtering-backward smoothing-forward filtering, so as to form square root CKF/BS nonlinear filter (SR-BS-CKF) algorithm. The algorithm utilizes backward smoothing-forward filtering based on the first forward filtering to improve the accuracy. It ensures the stability of the filter values by using square root updating method. The algorithm contains two SR-CKF operations and one SR-BS operation, because SR-BS has the low calculating amount, the calculation amount of the algorithm approximately equals to two times of SR-CKF and increases slightly. The simulation results indicate that the algorithm SR-BS-CKF has high precision, stronger numerical stability and rapid calculating speed, which has more effective filter performance than CKF.

Cubature Kalman Filter (CKF); nonlinear backward smoothing; square root filter; filter performance

1672-1497(2015)02-0065-05

2014-11-20

軍隊科研計劃項目

湯霞清(1965-)，男，教授，博士。

TN713

A

10.3969/j.issn.1672-1497.2015.02.013