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

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

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

0 引 言

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

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

針對(duì)以上問(wèn)題，文獻(xiàn)[8]中提出了一種基于UT變換的迭代收縮非線性狀態(tài)約束濾波算法(為了與下文保持一致，此處簡(jiǎn)稱為ISNC-UKF)，該算法可以處理強(qiáng)非線性約束下的狀態(tài)估計(jì)問(wèn)題，能夠獲得較好的濾波精度，時(shí)間復(fù)雜較小，且實(shí)現(xiàn)過(guò)程中無(wú)需計(jì)算非線性狀態(tài)約束函數(shù)的雅克比矩陣。在該算法的啟發(fā)下，將文獻(xiàn)[8]中的算法進(jìn)行一般化，該文提出了一種解決非線性狀態(tài)約束的算法框架。該方法可以采用多種基于確定點(diǎn)采樣濾波算法。包括：不敏卡爾曼濾波算法(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)約束方法的框架。為了減小基點(diǎn)誤差[7]對(duì)于濾波結(jié)果的影響，采用迭代的方法，使得約束逐漸變強(qiáng)，最終獲得更好的濾波精度。從而得到了幾種相應(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)。這些算法在保證較高濾波精度和較低時(shí)間復(fù)雜度的前提下，能夠解決非線性約束函數(shù)雅克比矩陣不存在或者雅克比矩陣難以求解的問(wèn)題。

1 問(wèn)題描述

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

xk=f(xk-1)+vk

(1)

yk=h(xk)+nk

(2)

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

g(xk)=dk

(3)

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

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

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

(4)

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

(5)

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

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

(6)

(7)

(8)

其中，(vi)1表示J的第i個(gè)特征向量的第一個(gè)元素。因?yàn)殡S機(jī)變量x中各個(gè)元素互不相關(guān)，可將式擴(kuò)展至多維形式：

(9)

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

(10)

(11)

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

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

(12)

(13)

(14)

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

w0=(h2-n)/h2

(15)

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

(16)

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

(17)

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

(18)

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

(19)

則式(18)可寫(xiě)為：

(20)

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

(21)

(22)

(23)

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

(24)

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

(25)

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

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

(26)

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

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

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

(27)

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

(28)

(29)

(30)

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

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

(31)

(32)

(33)

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

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

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

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

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

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

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

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

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

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

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

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

4 仿真實(shí)驗(yàn)及結(jié)果分析

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

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

(34)

(35)

yk=[θkωk]T+nk

(36)

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

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

(37)

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

表1為各種約束濾波算法的誤差值，包括角度誤差值以及角速度誤差值，將算法誤差性能由高到低排序?yàn)椋篗HE4、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 各種算法誤差性能對(duì)比

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

表2 各種算法消耗時(shí)間對(duì)比

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

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