基于容積變換的平滑變結(jié)構(gòu)濾波及應(yīng)用

林孝工, 焦玉召, 李 恒, 梁 坤, 聶 君

(哈爾濱工程大學(xué)自動化學(xué)院, 黑龍江 哈爾濱 150001)

0 引 言

隨著對海洋資源的探索,船舶動力定位技術(shù)也得到了更多關(guān)注[1-2]。對可微非線性系統(tǒng),將非線性系統(tǒng)模型展開為關(guān)于均值的泰勒多項式,僅保留線性部分,即得擴展卡爾曼濾波(extended Kalman filter,EKF)[3]算法。EKF需計算Jacoby矩陣,且有高階截斷誤差。因而對強非線性系統(tǒng),可能會出現(xiàn)估計誤差較大或者無法收斂的情況。此時可計算Jacoby和Hessian矩陣得到二階擴展卡爾曼濾波(second-order extended Kalman filter,SOEKF)[4],但計算量大。非線性濾波的另一種策略是基于Sigma點的無跡卡爾曼濾波(unscented Kalman filter,UKF)[5],UKF不需要Jacoby矩陣。同時基于球面徑向容積規(guī)則,得到容積卡爾曼濾波(cubature Kalman filter,CKF)[6]。且這些非線性狀態(tài)估計方法,已廣泛應(yīng)用于慣性導(dǎo)航[7]、狀態(tài)估計[8]等。同時通過平方根因子傳遞,可保證算法的數(shù)值穩(wěn)定性[9-10]。

模型精確時,基于Kalman的濾波器具有較高估計精度,然而模型參數(shù)不確定或噪聲干擾的隨機性,傳統(tǒng)算法可能會發(fā)散[11]。故文獻[12]將滑模概念引入濾波增益的計算中,提出變結(jié)構(gòu)濾波(variable structure filter,VSF)。VSF是一種基于不確定模型的濾波算法,通過引入平滑邊界層,得到具有固定平滑邊界層的平滑變結(jié)構(gòu)濾波[13]。實際系統(tǒng)干擾和不確定性具有隨機性,令估計誤差協(xié)方差對平滑邊界層求偏導(dǎo),可得近似最優(yōu)平滑邊界層[14]。文獻[15]將邊界層擴展為全矩陣形式,得到具有最優(yōu)平滑邊界層的平滑變結(jié)構(gòu)濾波(smoothing variable structure filter,SVSF)算法。

模型不確定時,SVSF是一種有效的濾波算法[16-17]。但仍具有一定局限性:①要求系統(tǒng)方程是適度非線性且可微,需計算系統(tǒng)的Jacoby矩陣;②受計算機字長限制,誤差積累可能使協(xié)方差失去非負定性,導(dǎo)致濾波發(fā)散。為解決這兩個問題,一方面將CKF與具有最優(yōu)平滑邊界層的SVSF算法相結(jié)合,利用容積規(guī)則計算狀態(tài)預(yù)測值、狀態(tài)協(xié)方差預(yù)測、測量預(yù)測值和測量協(xié)方差預(yù)測值,得到了基于容積規(guī)則的平滑變結(jié)構(gòu)濾波(cubature-smoothing variable structure filter,C-SVSF)算法,既提高了SVSF算法的估計精度,又擴展了SVSF算法的應(yīng)用范圍。另一方面,通過在狀態(tài)傳遞過程中使用協(xié)方差的平方根因子,保證協(xié)方差的正定性和對稱性,得到平方根容積平滑變結(jié)構(gòu)濾波(square root cubature-smoothing variable structure filter,SRC-SVSF),SRC-SVSF是具有數(shù)值穩(wěn)定性的濾波算法。

1 基于Jacoby的SVSF

考慮非線性系統(tǒng):

xk=fk-1(xk-1)+ωk

(1)

zk=hk(xk)+υk

(2)

式中,fk-1(·)和hk(·)表示狀態(tài)方程和測量方程;xk是狀態(tài)矢量;zk是觀測值;ωk和υk是狀態(tài)噪聲和觀測噪聲,服從高斯分布ωk～N(q,Q),υk～N(r,R),其中q和r是均值,Q和R是方差。

假設(shè)1假設(shè)系統(tǒng)式(1)和式(2)滿足文獻[13]中的定義,即不考慮測量噪聲時,系統(tǒng)xk=fk-1(xk-1)和zk=hk(xk)是連續(xù)的雙射或是完全可觀和完全可控的,即通過觀測矢量zk存在一個逆映射能唯一的得到狀態(tài)矢量xk,即

(3)

如果系統(tǒng)式(1)、式(2)轉(zhuǎn)換矩陣fk和hk是常系數(shù),那么直接得卡爾曼濾波器。如果轉(zhuǎn)換矩陣是非線性的,那么可通過計算Jacoby矩陣得EKF算法。

對模型不精確系統(tǒng),EKF誤差較大,而SVSF可有效抑制模型不精確帶來的濾波發(fā)散。原理如圖1所示,當預(yù)測值超出存在的子空間時,說明有不確定干擾,可通過不連續(xù)增益將估計值矯正到真實值附近,保證算法對不確定干擾的魯棒性。

圖1 SVSF算法概念圖Fig.1 Concept of SVSF

同時,為消除不連續(xù)增益的頻繁切換造成的系統(tǒng)顫動,文獻[13]引入固定平滑邊界層。假設(shè)系統(tǒng)存在有界隨機干擾,那么令平滑邊界層等于該干擾的保守上界。通過引入飽和函數(shù)式(4),當估計誤差大于邊界層時,用不連續(xù)增益來保證算法穩(wěn)定運行;當誤差在邊界層內(nèi)時,用連續(xù)增益進行狀態(tài)估計,進而消除一定的顫動影響。

(4)

圖2 具有邊界層的SVSFFig.2 SVSF with smooth boundary layer

實際中該上界不容易獲得,文獻[14-15]提出具有協(xié)方差導(dǎo)引的平滑變結(jié)構(gòu)濾波算法?；跔顟B(tài)估計誤差協(xié)方差最小原則得到具有最優(yōu)平滑邊界層的SVSF。狀態(tài)預(yù)測值和測量估計值分別為

(5)

(6)

(7)

狀態(tài)預(yù)測誤差協(xié)方差為

(8)

測量預(yù)測值為

(9)

新息ez,k+1|k和測量估計誤差ez,k+1|k+1為

(10)

(11)

基于新息和測量誤差,得增益[15]為

?

(12)

進而,可得狀態(tài)估計更新值為

(13)

估計誤差協(xié)方差更新值為

(14)

(15)

式(5)～式(14)即是變結(jié)構(gòu)濾波算法。下面通過引理1進一步證明算法的穩(wěn)定性問題。

引理1若估計誤差滿足式(16),則狀態(tài)估計過程是穩(wěn)定和收斂的[13]。

|ek|<|ek-1|

(16)

因而,只需證明狀態(tài)估計誤差滿足引理1,就可證明出狀態(tài)估計過程是穩(wěn)定和收斂的。

定理1若系統(tǒng)滿足假設(shè)1,狀態(tài)增益滿足不等式,那么濾波算法是穩(wěn)定和收斂的。

|ez,k+1|k|<|Hk+1Kk+1ez,k+1|k|<|ez,k+1|k|+|ez,k|k|

(17)

sign(Hk+1Kk+1ez,k+1|k)=sign(ez,k+1|k)

(18)

證明由不等式(17)可知,|Hk+1Kk+1ez,k+1|k|-|ez,k+1|k|<|ez,k|k|成立,又由于Hk+1Kk+1ez,k+1|k與ez,k+1|k同號,那么可得,|ez,k+1|k-Hk+1Kk+1ez,k+1|k|<|ez,k|k|。結(jié)合式(6)和式(9)～式(13),得ez,k+1|k-Hk+1Kk+1ez,k+1|k=ez,k+1|k+1。則不等式|ez,k+1|k+1|<|ez,k|k|成立。又由于系統(tǒng)是完全可觀和可控的,因而知|ex,k+1|k+1|<|ex,k|k|成立,滿足引理1,故估計算法是穩(wěn)定和收斂的。

證畢

由上可知,對于完全可觀和完全可控的系統(tǒng),狀態(tài)估計誤差具有穩(wěn)定性和收斂性的充分條件是輸出估計誤差是穩(wěn)定和收斂的。因而,由定理1,令增益為

(19)

綜上,SVSF對不確定非線性系統(tǒng)具有一定的魯棒性,但是仍無法克服EKF算法的局限性。針對此,一種直接的思路是通過容積變換來代替非線性系統(tǒng)的線性化,得到CKF算法。CKF由于不需要計算Jacoby矩陣,同時簡化了計算,因而得到了廣泛的應(yīng)用。

2 基于容積變換的非線性濾波策略

2.1 CKF算法

(20)

狀態(tài)預(yù)測值和協(xié)方差預(yù)測值為

(21)

(22)

測量預(yù)測值和新息協(xié)方差為

(23)

(24)

估計誤差互協(xié)方差為

(25)

以上即是CKF的狀態(tài)和協(xié)方差預(yù)測方程。

2.2 C-SVSF算法

基于以上描述,CKF預(yù)測方程可有效克服EKF的缺點。因而本文考慮采用容積規(guī)則來計算SVSF的狀態(tài)預(yù)測和協(xié)方差預(yù)測值,從而得到C-SVSF算法。C-SVSF不僅保持了SVSF算法的魯棒性,而且不需要計算系統(tǒng)的Jacoby矩陣,因而估計精度將高于SVSF算法。C-SVSF算法流程如下。

步驟1狀態(tài)預(yù)測式(5)和協(xié)方差預(yù)測式(8)分別變?yōu)槭?21)和式(22)。從而得到基于容積規(guī)則的狀態(tài)預(yù)測和協(xié)方差預(yù)測值。

步驟2上一時刻的測量估計誤差ez,k-1|k-1

首先，計算觀測方程的傳遞容積點為

(26)

(27)

則測量估計誤差為

(28)

以上即是C-SVSF算法,克服了非線性系統(tǒng)式(1)必須連續(xù)可微的條件,且不需要計算系統(tǒng)的Jacoby矩陣,即保持了SVSF的魯棒性,又由于容積規(guī)則的引入使得狀態(tài)預(yù)測和協(xié)方差預(yù)測的精度得到了進一步提高。因而得到的C-SVSF算法估計精度理論上要高于基于Jacoby矩陣的擴展平滑變結(jié)構(gòu)濾波(extended-SVSF,E-SVSF)算法。同時與標準E-SVSF算法具有相同的濾波框架,因而仍是一種穩(wěn)定的濾波算法。

與CKF相似,C-SVSF在最優(yōu)邊界層計算中要求計算協(xié)方差的逆矩陣,因而協(xié)方差矩陣需要滿足非負定性的要求。然而由于計算機字長限制或者誤差的累計可能會使協(xié)方差失去非負定性,從而導(dǎo)致算法數(shù)值發(fā)散。因而,為了提高C-SVSF算法的數(shù)值穩(wěn)定性,類似于平方根容積卡爾曼濾波算法[6],由下三角分解的方法給出了SRC-SVSF算法。

2.3 SRC-SVSF算法

(29)

式中

步驟3新息協(xié)方差平方根因子為

(30)

步驟5估計誤差協(xié)方差平方根因子更新公式為

(31)

綜上,將容積卡爾曼濾波策略中計算狀態(tài)和協(xié)方差預(yù)測的方法引入到SVSF算法中,得到的C-SVSF由于不需要計算非線性系統(tǒng)式的Jacoby矩陣,因而其估計精度要高于E-SVSF算法,且SRC-SVSF保障了算法的數(shù)值穩(wěn)定性。

3 仿真實驗及分析

建立三自由度船舶運動方程[2]為

(32)

三自由度觀測方程為

zk+1=hk(xk)+υk

(33)

對非線性動力定位船舶位置估計系統(tǒng),CKF算法的估計精度要高于EKF算法的估計精度。因而以CKF作為對比算法，分別在觀測噪聲協(xié)方差已知和未知兩種情況下,與E-SVSF、C-SVSF和SRC-SVSF算法進行仿真對比。

3.1 觀測噪聲協(xié)方差精確已知

假設(shè)測量噪聲協(xié)方差是精確已知的,即R=diag([20,20,20,20,20,20])。采樣周期1 s,采樣時間1 800 s。通過200次蒙特卡羅仿真實驗,4種算法下船舶位置和速度RMSE分別如圖3和圖4所示。

圖3 已知噪聲下的北-東-艏向位置RMSEFig.3 RMSE of north-east-head with known noise

圖4 已知噪聲下的縱蕩-橫蕩-艏搖RMSEFig.4 RMSE of surge-sway-yaw with known noise

由于在噪聲已知時,CKF是最優(yōu)狀態(tài)估計算法,故CKF具有最小RMSE。由于狀態(tài)和協(xié)方差預(yù)測引入了Jacoby矩陣,因而E-SVSF誤差最大。同時,容積規(guī)則的引入使C-SVSF算法精度高于E-SVSF。而SRC-SVSF算法由于狀態(tài)傳遞過程中使用協(xié)方差的平方根因子,因而與C-SVSF具有幾乎相同的估計誤差。表1是狀態(tài)在不同算法下的平均RMSE?？芍狢-SVSF及其平方根形式SRC-SVSF具有相同估計誤差,且都高于E-SVSF,說明了改進算法的有效性。

表1 已知噪聲下算法的平均RMSE

3.2 觀測噪聲協(xié)方差未知

假設(shè)測量噪聲協(xié)方差未知,即可令R=diag([500,500,1 000,1 000,1 000,1 000])。采樣周期仍為1 s,采樣時間1 800 s。通過200次蒙特卡羅仿真實驗,可得此時船舶位置和速度RMSE分別如圖5和圖6所示。可知當測量噪聲協(xié)方差未知時,由于CKF算法不具有魯棒性,因而CKF估計誤差最大。而基于SVSF的濾波算法對不確定干擾具有一定的魯棒性,因而估計精度高于CKF算法。同時由于容積規(guī)則的引入,C-SVSF和SRC-SVSF算法的估計精度要高于E-SVSF算法,這與理論分析相一致。同時，表2給出了測量噪聲協(xié)方差未知時,4種算法下的平均RMSE。

圖5 未知噪聲下的北-東-艏向RMSEFig.5 RMSE of north-east-head with unknown noise

圖6 未知噪聲下的縱蕩-橫蕩-艏搖RMSEFig.6 RMSE of surge-sway-yaw with unknown noise

算法參數(shù)x′/my′/m/(°)u/(m/s)v/(m/s)r/((°)/s)CKF6．676．668．325．785．785．39E?SVSF4．224．227．044．954．954．12C?SVSF4．124．123．894．594．613．65SRC?SVSF4．124．123．894．594．613．65

綜上可知:測量噪聲協(xié)方差已知時,C-SVSF、SRC-SVSF與CKF具有相近估計精度,且都高于E-SVSF的估計精度;當測量噪聲協(xié)方差未知時,C-SVSF、SRC-SVSF具有相似估計精度,且高于E-SVSF,同時由于SVSF對噪聲干擾的魯棒性,因而精度均高于CKF。進一步說明本文C-SVSF和SRC-SVSF算法的有效性以及存在噪聲干擾時算法的魯棒性。

4 結(jié) 論

本文提出了C-SVSF算法,該算法既能消除Jacoby矩陣計算,又能充分利用SVSF的魯棒性;為保證算法的數(shù)值穩(wěn)定性,給出SRC-SVSF算法。由仿真知,當測量噪聲協(xié)方差已知,C-SVSF、SRC-SVSF與CKF具有相似估計精度且高于E-SVSF;當測量噪聲協(xié)方差未知,C-SVSF、SRC-SVSF估計精度仍高于E-SVSF,同時由于SVSF的魯棒性,基于SVSF的估計精度均高于CKF,進一步說明所提算法的優(yōu)越性。因而,C-SVSF和SRC-SVSF可以作為船舶狀態(tài)估計器使用,且當測量噪聲協(xié)方差未知時,可以替代已有的E-SVSF或CKF進行狀態(tài)估計。

[1] 邊信黔, 付明玉, 王元慧. 船舶動力定位[M]. 北京:科學(xué)出版社, 2011.

BIAN X Q, FU M Y, WANG Y H. Ship dynamic positioning[M]. Beijing: Science Publishing Company, 2011.

[2] 徐樹生. 船舶動力定位系統(tǒng)多傳感器信息融合方法研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 2013.

XU S S. Research on the multi-sensor information fusion methods of vessel dynamic positioning system[D]. Harbin: Harbin Engineering University, 2013.

[3] HAUG A J. Bayesian estimation and tracking: a practical guide[M]. New Jersey: Wiley, 2012.

[4] MAHALANABIS A K, FAROOQ M. A second-order method for state estimation of non-linear dynamical systems[J]. International Journal of Control, 1971, 14(4):631-639.

[5] QUINE B, UHLMANN J, DURRANTWHYTE H. Implicit Jacobians for linearised state estimation in nonlinear systems[C]∥Proc.of the American Control Conference, 1995:1645-1646.

[6] ARASARATNAM I, HAYKIN S. Cubature Kalman filters[J]. IEEE Trans.on Automatic Control, 2009, 54(6):1254-1269.

[7] LIU M, LAI J, LI Z, et al. An adaptive cubature Kalman filter algorithm for inertial and land-based navigation system[J]. Aerospace Science & Technology, 2016, 51(2):52-60.

[8] LIN C, GONG X, XIONG R, et al. A novelH∞, and EKF joint estimation method for determining the center of gravity position of electric vehicles[J].Applied Energy,2017,194:609-616.

[9] 徐樹生, 林孝工, 趙大威. 強跟蹤SRCKF及其在船舶動力定位中的應(yīng)用[J]. 儀器儀表學(xué)報, 2013, 34(6):1266-1272.

XU S S, LIN X G, ZHAO D W. Strong tracking SRCKF and its application in vessel dynamic positioning[J]. Chinese Journal of Scientific Instrument, 2013, 34(6): 1266-1272.

[10] 付夢印. Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M]. 北京:科學(xué)出版社, 2010.

FU M Y. Kalman filtering theory and its application in navigation system[M]. Beijing: Science Publishing Company, 2010.

[11] TIAN T, SUN S, LI N. Multi-sensor information fusion estimators for stochastic uncertain systems with correlated noises[J]. Information Fusion, 2016, 27(C):126-137.

[12] HABIBI S R, BURTON R, HABIBI S R, et al. The variable structure filter[J]. Journal of Dynamic Systems Measurement & Control, 2002, 125(3):157-165.

[13] HABIBI S. The smooth variable structure filter[J]. Proceedings of the IEEE, 2007, 95(5):1026-1059.

[14] GADSDEN S A, EL SAYED M, HABIBI S R. Derivation of an optimal boundary layer width for the smooth variable structure filter[C]∥Proc.of the American Control Conference, 2011:4922-4927.

[15] GADSDEN S A, HABIBI S R. A new robust filtering strategy for linear systems[J]. Journal of Dynamic Systems Measurement & Control, 2012, 135(1):359-370.

[16] ATTARI M, GADSDEN S A, HABIBI S. Target tracking formulation of the SVSF with data association techniques[J]. IEEE Trans.on Aerospace & Electronic Systems, 2017, 53(1):12-25.

[17] ATTARI M, LUO Z, HABIBI S. An SVSF-based generalized robust strategy for target tracking in clutter[J]. IEEE Trans.on Intelligent Transportation Systems, 2016, 17(5):1381-1392.