迭代收縮非線性狀態(tài)約束濾波算法

賀 姍，劉沫萌,2,3，李 迎

(1.西安工程大學(xué) 計算機科學(xué)學(xué)院，陜西 西安 710048；2.陜西省服裝設(shè)計智能化重點實驗室，陜西 西安 710048；3.新型網(wǎng)絡(luò)智能信息服務(wù)國家地方聯(lián)合工程研究中心，陜西 西安 710048)

0 引 言

在狀態(tài)估計問題中，系統(tǒng)的狀態(tài)向量可能會受到許多約束條件的限制，如果可以用這些約束條件對狀態(tài)向量進行修正，便可以更加逼近系統(tǒng)真實值，從而使得濾波估計結(jié)果更加精確。因此，研究約束條件下濾波算法是非常有意義的[1-3]。

約束條件分為線性以及非線性。解決線性約束問題的相關(guān)方法可以參見Simon的綜述文章[1]。針對非線性約束下的狀態(tài)估計問題，人們也進行了很多研究并提出了很多算法。1995年，Michalska等人提出了水平滑動估計算法(Moving Horizon Estimation，MHE)[4]，該算法在非線性約束條件的限制下，采用最優(yōu)化方法對一個區(qū)間內(nèi)的估計誤差和觀測誤差進行最小化，能夠獲得很高的濾波精度，但其算法時間復(fù)雜度較大，即使實現(xiàn)過程中使用了較小的滑動窗口，仍然需要花費很多的時間。1997年，De Geeter等人給出了平滑約束卡爾曼濾波(Smoothly Constrained Kalman Filter，SCKF)[5]，該算法濾波精度較高，時間復(fù)雜度較低。2002年，Simon等人提出的一階泰勒級數(shù)展開非線性約束算法(Linearized Constrained，LC)[6]，該算法采用一階泰勒級數(shù)展開方法對非線性狀態(tài)約束函數(shù)進行線性化，再通過線性化約束算法中的投影方法進行濾波。為了提高濾波精度，2009年，Yang等人提出了一種新的濾波算法─二階泰勒級數(shù)展開非線性約束方法(2ord Nonlinear Constrained，2ord NC)[7]，其濾波精確度高于LC算法，但其耗費時間較大。除了MHE算法之外，其余幾種算法也都只能在弱非線性約束中適用，且都需要計算非線性狀態(tài)約束函數(shù)的雅克比矩陣。

針對以上問題，文獻[8]中提出了一種基于UT變換的迭代收縮非線性狀態(tài)約束濾波算法(為了與下文保持一致，此處簡稱為ISNC-UKF)，該算法可以處理強非線性約束下的狀態(tài)估計問題，能夠獲得較好的濾波精度，時間復(fù)雜較小，且實現(xiàn)過程中無需計算非線性狀態(tài)約束函數(shù)的雅克比矩陣。在該算法的啟發(fā)下，將文獻[8]中的算法進行一般化，該文提出了一種解決非線性狀態(tài)約束的算法框架。該方法可以采用多種基于確定點采樣濾波算法。包括：不敏卡爾曼濾波算法(Unscented Kalman Filter，UKF)[9]、求積分卡爾曼濾波算法(Quadrature Kalman Filter，QKF)[10]、中心差分卡爾曼濾波算法(Central Divided Differences Kalman Filter，CDKF)[11]、容積卡爾曼濾波算法(Cubature Kalman Filter，CKF)[12]。結(jié)合上述四種非線性濾波算法，提出了高斯分布下的一般性非線性狀態(tài)約束方法的框架。為了減小基點誤差[7]對于濾波結(jié)果的影響，采用迭代的方法，使得約束逐漸變強，最終獲得更好的濾波精度。從而得到了幾種相應(yīng)的解決非線性狀態(tài)約束的濾波算法，即：迭代收縮非線性狀態(tài)約束求積分卡爾曼濾波算法(Iterative Shrinking Nonlinear State Constraints Based on Quadrature Kalman Filter，ISNC-QKF)、迭代收縮非線性狀態(tài)約束中心差分卡爾曼濾波算法(Iterative Shrinking Nonlinear State Constraints Based on Central Divided Differences Kalman Filter，ISNC-CDKF)、迭代收縮非線性狀態(tài)約束容積卡爾曼濾波算法(Iterative Shrinking Nonlinear State Constraints Based on Cubature Kalman Filter，ISNC-CKF)。這些算法在保證較高濾波精度和較低時間復(fù)雜度的前提下，能夠解決非線性約束函數(shù)雅克比矩陣不存在或者雅克比矩陣難以求解的問題。

1 問題描述

假定高斯條件下系統(tǒng)的狀態(tài)方程和量測方程分別為：

xk=f(xk-1)+vk

(1)

yk=h(xk)+nk

(2)

其中，xk和yk分別是k時刻的狀態(tài)值和量測值，f和h分別為狀態(tài)轉(zhuǎn)移函數(shù)和量測函數(shù)，vk～N(0,Qk)和nk～N(0,Rk)分別為過程噪聲和量測噪聲，且它們互不相關(guān)。現(xiàn)假設(shè)狀態(tài)值xk受到的非線性約束函數(shù)如下：

g(xk)=dk

(3)

式中，g與dk分別為已知的非線性狀態(tài)約束函數(shù)和向量。此約束問題即為通過式(3)對狀態(tài)向量進行修正從而得到更加精確的狀態(tài)估計值。

2 高斯非線性約束濾波的貝葉斯形式

在式(3)中，x為服從高斯分布的隨機向量，因此函數(shù)g(x)的值也可以寫成一個積分形式：

(4)

其中，D為積分區(qū)域，g(.)為任意非線性函數(shù)，w(.)為一個加權(quán)函數(shù)。式的積分為“非線性函數(shù)×高斯密度”形式，因此可以采用數(shù)值解法進行近似[13-14]，即：

(5)

上述數(shù)值積分問題的關(guān)鍵點在于：確定西格瑪點以及相應(yīng)權(quán)值。假設(shè)對于n維的狀態(tài)向量，可以分別借助UKF、QKF、CDKF、CKF算法中的思路進行求取。其中借助UKF算法的實現(xiàn)思路已經(jīng)在文獻[8]中予以介紹，因此該文予以省略。

QKF濾波是在高斯厄米特濾波基礎(chǔ)上提出的，下面給出高斯厄米特求積分規(guī)則[10]。假定一個隨機變量x，其高斯密度為N(x;0,1)，則可得函數(shù)g(x)的期望值為：

(6)

(7)

(8)

其中，(vi)1表示J的第i個特征向量的第一個元素。因為隨機變量x中各個元素互不相關(guān)，可將式擴展至多維形式：

(9)

CDKF是借助Stirling插值多項式近似非線性函數(shù)，采用中心差分代替泰勒級數(shù)展開中的一階和二階導(dǎo)數(shù)，實現(xiàn)非線性函數(shù)向線性函數(shù)的轉(zhuǎn)換[11]。即有：

(10)

(11)

其中，h表示中心差分區(qū)間長度，對于高斯假設(shè)下的系統(tǒng)，h取值為3，δx表示一個具有零均值且與隨機變量x有著相同協(xié)方差的隨機變量。

CDKF的具體實現(xiàn)方法和UKF的類似，都是采用了對稱策略求取Sigma點χi，對于n維的隨機變量x需計算2n+1個Sigma點χi。

(12)

(13)

(14)

其相應(yīng)權(quán)值為：

w0=(h2-n)/h2

(15)

wi=1/2h2,i=1,2,…,2n

(16)

CKF確定點以及相應(yīng)權(quán)值的計算根據(jù)容積規(guī)則[12]。考慮下面一個求積分問題:

(17)

為了數(shù)值化計算上式積分問題，首先將其轉(zhuǎn)換為一個更常用的球面徑向形式，需將x轉(zhuǎn)換成球面半徑r和方向向量y的乘積，即：x=ry，其中，yTy=1,那么上式可寫為：

(18)

其中，Un表示半徑為1的球表面，δ(·)為積分域Un內(nèi)的元素。令:

(19)

則式(18)可寫為：

(20)

針對式(19)和式(20)，分別采用mr點高斯厄米特求積分規(guī)則和ms點球面規(guī)則，可得到mr×ms點的球面徑向求容積規(guī)則為：

(21)

(22)

(23)

那么，對于式(17)、式(4)中的積分問題就可以通過上述方法解決。

(24)

當(dāng)mr=1，ms=2n時，對于一個三階球面徑向規(guī)則，需計算出m=2n個容積點εi，計算公式為：

(25)

其相應(yīng)權(quán)值為：

wi=1/m,i=1,2,…,m

(26)

3 迭代收縮非線性狀態(tài)約束濾波

為了解決非線性狀態(tài)約束問題，在狀態(tài)向量符合高斯分布的假設(shè)下，采用上述方法對積分進行數(shù)值近似。為了進一步提高狀態(tài)估計的精度，給非線性約束施加一系列由強到弱的噪聲，通過最佳量測[15-16]的方法迭代求取狀態(tài)估計值。下面分別對這四種方法具體實現(xiàn)過程進行闡述。

ζi,k=g(εi,k),i=0,1,…,2n

(27)

計算非線性變換后的均值和協(xié)方差：

(28)

(29)

(30)

為了減小基點誤差對于狀態(tài)向量估計的影響，給非線性狀態(tài)約束函數(shù)施加一系列噪聲Rd,k。首先用a×Pζζ,k將其乘積值作為第一次迭代時Rd,k的值，其中a∈[0.01,0.1]為一個常數(shù)，在實現(xiàn)過程中通常取經(jīng)驗值，文中的取值為0.01。在之后的迭代中，Rd,k=Rd,kexp(-i)，i為迭代的次數(shù)，使得方差逐漸遞減，由最小方差估計準(zhǔn)則得到具有非線性狀態(tài)約束條件下的均值和協(xié)方差，即：

Pζζ,k=Pζζ,k+Rd,k

(31)

(32)

(33)

ISNC-UKF方法的具體實現(xiàn)過程已經(jīng)在文獻[8]中詳細(xì)介紹，不再贅述。其他幾種方法的實現(xiàn)過程與ISNC-CKF類似，下面直接給出這些方法的實現(xiàn)步驟。

ISNC-QKF方法的實現(xiàn)步驟如下：

Step2：用式(27)～式(30)計算非線性變換后的均值及其協(xié)方差；

Step3：第一次迭代時，令Rd,k=aPζζ,k，在之后的迭代中Rd,k=Rd,kexp(-i)，i為迭代的次數(shù)；

Step4：采用式(31)獲得當(dāng)前的方差值Pζζ,k；

Step5：采用式(32)和式(33)獲得系統(tǒng)的狀態(tài)估計值以及方差；

ISNC-CDKF方法的實現(xiàn)步驟如下：

Step2：用式(27)～式(30)計算非線性變換后的均值及其協(xié)方差；

Step3：第一次迭代時，令Rd,k=aPζζ,k，在之后的迭代中Rd,k=Rd,kexp(-i)，i為迭代的次數(shù)；

Step4：采用式(31)獲得當(dāng)前的方差值Pζζ,k；

Step5：采用式(32)和式(33)獲得系統(tǒng)的狀態(tài)估計值以及方差；

在上述迭代收縮非線性狀態(tài)約束濾波方法實現(xiàn)過程中，迭代次數(shù)為3～5次時就能夠得到一個較小且較為穩(wěn)定的估計均方根誤差值，迭代更多次并不能對濾波結(jié)果有明顯改善，這是因為經(jīng)過多次迭代后Rd,k=Rd,kexp(-i)使得方差逐漸遞減，對于方差Pζζ,k的影響就會很越來越小，從而也減弱了對于估計均方根誤差收斂的影響。

4 仿真實驗及結(jié)果分析

利用鐘擺運動[1]仿真實例驗證濾波算法?？稍O(shè)系統(tǒng)的動態(tài)方程及量測方程為：

θk+1=θk+Tωk+vθ,k

(34)

(35)

yk=[θkωk]T+nk

(36)

其中，已知k時刻的系統(tǒng)狀態(tài)向量為xk=[θkωk]T，θk則為k時刻鐘擺所擺動的角度值，ωk為k時刻鐘擺動的角速度值。yk為k時刻系統(tǒng)量測值，T表示步長，g表示重力加速度，L表示鐘擺長度，且[vθ,kvω,k]T～N(0,Qk)和nk～N(0,Rk)分別為互不相關(guān)的過程噪聲及量測噪聲。

由機械能守恒定理可知，鐘擺運動系統(tǒng)中的狀態(tài)向量應(yīng)符合如下的非線性狀態(tài)約束條件：

(37)

狀態(tài)估計過程采用擴展卡爾曼濾波算法(Extended Kalman Filter，EKF)[17]，再通過非線性約束函數(shù)即式(37)、式(3)對狀態(tài)估計結(jié)果進一步進行修正，采用以下濾波算法：水平滑動估計濾波算法(MHE)、平滑約束卡爾曼濾波算法(SCKF)、一階泰勒級數(shù)展開非線性約束算法(LC)、二階泰勒級數(shù)展開非線性約束濾波算法(2ordNC)、最佳量測濾波算法(PM)、迭代收縮非線性狀態(tài)約束不敏卡爾曼濾波算法(ISNC-UKF)以及文中給出的迭代收縮非線性狀態(tài)約束求積分卡爾曼濾波算法(ISNC-QKF)、迭代收縮非線性狀態(tài)約束中心差分卡爾曼濾波算法(ISNC-CDKF)及迭代收縮非線性狀態(tài)約束容積卡爾曼濾波算法(ISNC-CKF)進行實驗仿真。

表1為各種約束濾波算法的誤差值，包括角度誤差值以及角速度誤差值，將算法誤差性能由高到低排序為：MHE4、MHE2、SCKF、ISNC-CDKF、ISNC-QKF、ISNC-CKF、ISNC-UKF、2ordNC、LC、PM及未施加非線性約束函數(shù)的EKF。從表中可以看出，文中給出的濾波算法ISNC-CDKF、ISNC-QKF、ISNC-CKF的誤差差別很小，和已有的濾波算法ISNC-UKF類似，且和濾波算法SCKF的誤差基本相當(dāng)。

表1 各種算法誤差性能對比

表2給出了上述濾波算法實現(xiàn)過程中所耗費的時間，算法耗費時間從高到低依次是MHE4、MHE2、ISNC-QKF、ISNC-UKF、ISNC-CDKF、ISNC-CKF、2ordNC、SCKF、PM、LC以及沒有加非線性約束狀態(tài)條件的EKF。從表中的數(shù)據(jù)可以看出，該文提出的濾波算法中ISNC-CDKF 及ISNC-CKF其耗費時間基本相當(dāng)，和ISNC-UKF算法耗費時間類似，而ISNC-QKF的耗費時間也應(yīng)該和上述三種濾波算法相當(dāng)，但是由于在算法實現(xiàn)過程中計算積分點時耗費了額外的時間，而積分點的計算可以離線獲得，故ISNC-QKF和ISNC-CKF、ISNC-CDKF及ISNC-UKF的耗費時間是基本相當(dāng)?shù)?。雖然SCKF濾波算法和該文提出的三種濾波算法相比較，誤差較低、耗費時間少，但該算法值僅適用于弱非線性情況，對于非線性約束函數(shù)雅克比矩陣不存在或者雅克比矩陣難以求解的情況，若使用該算法，只會使得濾波失效，而該文給出的這三種算法可以適用于強非線性情況，能夠避免上述問題。MHE濾波算法和這三種算法相比較，雖然誤差很低，但是其耗費時間卻很大。綜上所述，給出的三種算法不僅無需求解非線性約束函數(shù)雅克矩陣，且能夠使得濾波結(jié)果更加逼近真實值，算法處理時長也較短。

表2 各種算法消耗時間對比

5 結(jié)束語

在處理非線性狀態(tài)約束估計問題時，若約束函數(shù)為弱非線性時，可采用泰勒級數(shù)展開方式近線性化非線性約束函數(shù)，但是在實現(xiàn)過程中可能會出現(xiàn)以下問題：狀態(tài)估計中需求解非線性函數(shù)的雅克比矩陣，然而雅克比矩陣也可能不存在或易于求解，另外在許多應(yīng)用條件的影響下，濾波結(jié)果也可能會出現(xiàn)精度差、估計有偏或者發(fā)散等問題。為了避免上述問題，提出了高斯假設(shè)下的一類迭代收縮非線性狀態(tài)約束濾波方法，并給出了幾種實現(xiàn)算法，包括：ISNC-QKF、ISNC-CDKF、ISNC-CKF。與已有的ISNC-UKF算法性能類似，都無需求解非線性函數(shù)的雅克比矩陣，算法實現(xiàn)過程中采用最佳量測的方法迭代求解狀態(tài)估計值，濾波精度較好，時間復(fù)雜度適中。