帶邊界約束的容積卡爾曼濾波?

劉志彬, 張 玲, 褚東升

(中國(guó)海洋大學(xué)工程學(xué)院,山東省高校海洋機(jī)電裝備與儀器重點(diǎn)實(shí)驗(yàn)室， 山東 青島 266100)

1960年,卡爾曼提出了基于貝葉斯估計(jì)的遞推濾波方法—卡爾曼濾波[1],用以處理線性系統(tǒng)的狀態(tài)估計(jì)。面對(duì)非線性問(wèn)題,有學(xué)者提出基于線性化的擴(kuò)展卡爾曼濾波EKF(Extended Kalman filter),這是一種用歐拉折線近似替代非線性方程的方法,只能應(yīng)用于線性化誤差比較小的系統(tǒng)方程。后來(lái)又產(chǎn)生了用特征點(diǎn)近似狀態(tài)的概率密度函數(shù)的方法——Sigma點(diǎn)濾波：基于無(wú)跡變換的無(wú)跡卡爾曼濾波UKF(Uncented Kalman Filter)[2]、基于插值法的高斯濾波器[3]和基于重要性采樣的粒子濾波PF(Particle Filter)[4]。它們或數(shù)學(xué)基礎(chǔ)不嚴(yán)密,或計(jì)算量會(huì)隨著系統(tǒng)維度的增加而急劇增大。學(xué)者Arasaratnam提出了基于容積計(jì)算的容積卡爾曼濾波器CKF(Cubature Kalman Filter)[5],此后又研究了連續(xù)系統(tǒng)的容積卡爾曼濾波算法[6],但并未給出帶約束系統(tǒng)的容積卡爾曼濾波算法。對(duì)于約束問(wèn)題,Simon在其專著[7]中給出了4種方法：降維、完美觀測(cè)、約束優(yōu)化、概率重寫,Rawlings將預(yù)測(cè)控制中的滾動(dòng)優(yōu)化方法[8]引入狀態(tài)估計(jì)領(lǐng)域并于推導(dǎo)出遞推滾動(dòng)最優(yōu)估計(jì)算法MHE(Moving Horizion Estimation)[9],通過(guò)解帶約束的最優(yōu)化方程的方法同步解決約束和估計(jì)問(wèn)題。同時(shí)發(fā)展的還有其近親算法——非線性動(dòng)態(tài)數(shù)據(jù)均衡RNDDR(Recursive Nonlinear Dynamic Data Reconciliation)[10],這種方法與N=1的MHE算法很像,這兩種方法有能夠處理多種約束條件的優(yōu)點(diǎn),但最優(yōu)化計(jì)算會(huì)產(chǎn)生很大的運(yùn)算量,同時(shí)RNDDR在遞推過(guò)程中需要用到黎卡提方程,對(duì)于非線性系統(tǒng)則需借助線性化的手段來(lái)實(shí)現(xiàn)[10],或借用Sigma點(diǎn)濾波來(lái)實(shí)現(xiàn)黎卡提方程的參數(shù)更新功能[11],本文將CKF算法和帶約束的非線性動(dòng)態(tài)數(shù)據(jù)均衡算法NDDR(Nonlinear Dynamic Data Reconciliation)相結(jié)合,彌補(bǔ)計(jì)算精度低或UKF數(shù)學(xué)基礎(chǔ)不嚴(yán)密的缺點(diǎn)。完全討論所有的約束會(huì)使問(wèn)題過(guò)于復(fù)雜,本文只討論邊界約束。對(duì)于文獻(xiàn)[12]中所使用的約束方法,其仿真效果在很多情況下并不理想,所以本文在此基礎(chǔ)上改進(jìn),將容積點(diǎn)完全約束在邊界之內(nèi),重新計(jì)算對(duì)應(yīng)的權(quán)重,同時(shí)與數(shù)據(jù)均衡算法結(jié)合,使CKF算法更適用于邊界約束的非線性系統(tǒng),推導(dǎo)出約束容積卡爾曼濾波Constrainted Cubature Kalman filter(CCKF)。

1 容積卡爾曼濾波

對(duì)于非線性系統(tǒng)模型

xk=f(xk-1)+wk-1，

(1)

zk=g(xk)+vk，

(2)

其中：xk∈Rn、zk∈Rm分別表示k時(shí)刻系統(tǒng)狀態(tài)和系統(tǒng)輸出；wk∈Rn、vk∈Rm分別為k時(shí)刻系統(tǒng)噪聲和輸出噪聲；f:Rn→Rn,g:Rn→Rm為非線性函數(shù)。

其中：Q、R分別為正定對(duì)稱矩陣；δij為Kronecker函數(shù)。

Bayes估計(jì)求出狀態(tài)的概率分布之后,通過(guò)求期望來(lái)獲得狀態(tài)估計(jì)的濾波方差,CKF的本質(zhì)是將積分求期望的問(wèn)題用積分插值方法解決,即

對(duì)任意函數(shù)f(x),其容積積分為

(3)

其中點(diǎn)集{ξi}為標(biāo)準(zhǔn)容積點(diǎn)：

CKF的濾波步驟如下:

(1)預(yù)測(cè)更新。計(jì)算方差平方根和容積點(diǎn)

Sk-1/k-1=chol(Pk-1/k-1)，

(4)

(5)

其中：chol(·)表示cholesky分解；Sk-1/k-1為Pk-1/k-1的矩陣平方根；ξi是上文中的標(biāo)準(zhǔn)容積；xk-1/k-1,i為實(shí)際計(jì)算需要的容積點(diǎn)。容積點(diǎn)對(duì)應(yīng)的預(yù)估點(diǎn)為

xk/k-1,i=f(xk-1/k-1,i)，

(6)

預(yù)估狀態(tài)和預(yù)估方差

(7)

(8)

其中：ωi為表示權(quán)重，且ωi=1/2n。

(2)狀態(tài)更新。計(jì)算此時(shí)的容積點(diǎn)

Sk/k-1= chol (Pk/k-1) ，

(9)

(10)

容積點(diǎn)對(duì)應(yīng)的輸出

zk/k-1,i=h(xk/k-1,i)，

(11)

系統(tǒng)輸出的預(yù)估

(12)

相關(guān)協(xié)方差

(13)

(14)

狀態(tài)增益為

(15)

狀態(tài)濾波

(16)

濾波方差

(17)

在噪聲為高斯噪聲的假設(shè)下，Bayes估計(jì)退化為Kalman濾波,因此即使是非線性系統(tǒng)的狀態(tài)濾波也具有了線性Kalman濾波的一般形式,但二者絕不是簡(jiǎn)單類比,而是同源于Bayes估計(jì)。CKF用容積求積規(guī)則逼近求系統(tǒng)狀態(tài)期望的步驟,因而不是最優(yōu)估計(jì)。在濾波算法中引入非線性動(dòng)態(tài)數(shù)據(jù)均衡方法可以一定程度地提高算法精度，這一方法很好地處理了濾波過(guò)程中約束條件的影響。

2 非線性動(dòng)態(tài)數(shù)據(jù)均衡算法

非線性動(dòng)態(tài)數(shù)據(jù)均衡算法是一種盡可能充分利用已知信息進(jìn)行狀態(tài)估測(cè)的方法,其主要思想是對(duì)已知的信息利用懲罰因子的方法予以平衡,獲取可信度相對(duì)高的狀態(tài)估計(jì)，這是一種廣泛應(yīng)用于非線性的化工生產(chǎn)之中的算法[10],同時(shí)可以處理廣泛的非線性約束問(wèn)題,相對(duì)于MHE算法,這是一種很好的平衡算法運(yùn)算量和運(yùn)算精度的方法。

即計(jì)算：

(18)

上式的約束條件為

xL≤xk≤xH，

h(xk)≤0，

e(xk)=0 。

當(dāng)約束條件去除,系統(tǒng)為線性化系統(tǒng)時(shí),以上方法退化為Kalman濾波。

3 約束容積卡爾曼濾波

本文只討論具有邊界約束的狀態(tài)濾波,邊界約束定義如下：xL≤xk≤xU,此時(shí)整個(gè)空間的概率函數(shù)將形成截?cái)嗪瘮?shù),理想假設(shè)中的高斯分布變成了截?cái)喔咚狗植肌４送鈱?duì)于連續(xù)不等式fi(x)≤0,會(huì)形成空間{x|fi(x)≤0}： 將不等式約束化為特殊的邊界約束。容積卡爾曼濾波(CKF)是用容積點(diǎn)近似模擬整個(gè)空間 的概率分布。假如約束空間為凸空間,容積點(diǎn)還可以一定程度近似截?cái)喔咚狗植?。只是其容積點(diǎn)和權(quán)重的取法需要改進(jìn)。

同時(shí)對(duì)于濾波結(jié)果的求取,使用更精確和對(duì)約束處理更直接的非線性動(dòng)態(tài)數(shù)據(jù)均衡算法。

綜合以上假設(shè)和推理,得到

(1)預(yù)估階段。

Sk-1/k-1=chol(Pk-1/k-1) ，

(19)

求出每個(gè)Δxi方向邊界與容積點(diǎn)的相對(duì)關(guān)系

(20)

Δxi=[Sk-1/k-1-Sk-1/k-1]的第i個(gè)縱量,令λi=min{1,θ},

則容積點(diǎn)為

(21)

其對(duì)應(yīng)的權(quán)重為ωk/k-1,i=F(θi)/(F(θi)+F(θi+n))/n

狀態(tài)預(yù)估

(22)

(23)

預(yù)估方差

(24)

(25)

優(yōu)化約束條件為

xL≤xk≤xH，

濾波值為

(26)

濾波方差為

(27)

對(duì)于邊界約束的容積濾波算法,最精確的方式是利用文獻(xiàn)[7]中的方法對(duì)截?cái)嗫臻g的期望x和方差P進(jìn)行重新計(jì)算,這樣容積點(diǎn)就會(huì)全部落入約束區(qū)域,但這種方法處理過(guò)于復(fù)雜,因此本文基于相似的理念給出了一種替代方法。在本方法中,預(yù)估階段的期望、方差和狀態(tài)更新階段的期望、方差,均考慮了約束條件。

4 仿真算例

本文討論的算例來(lái)自化工氣體化合過(guò)程[13]的改寫，其化學(xué)反應(yīng)為：2A → B

(28)

(29)

k1=0.16 ，

P=PA+PB。

(30)

其對(duì)應(yīng)的離散化模型為

式中：PA、PB分別為A和B的分壓；P為總壓強(qiáng),采樣速率為ΔT=0.1s。

(31)

輸出方程為

(32)

各參數(shù)設(shè)置如下：

約束空間為

xL≤x≤xH，

仿真結(jié)果為見(jiàn)圖1和2。

圖1為A和B分壓的真實(shí)狀態(tài)及兩種濾波算法的濾波結(jié)果對(duì)比，圖2為兩種濾波算法的絕對(duì)誤差對(duì)比。

從圖1和2中可以看出CCKF的收斂速度和收斂精度均高于CKF,說(shuō)明約束算法有效。

在仿真算例中發(fā)現(xiàn)一類使大多數(shù)算法失效的情況。即當(dāng)非線性系統(tǒng)的系統(tǒng)方程的一階項(xiàng)系數(shù)—梯度矩陣接近單位陣時(shí),由于計(jì)算的舍入誤差,EKF及其衍生算法隨機(jī)穩(wěn)定,即有時(shí)可以跟蹤真值,有時(shí)不能跟蹤真值,對(duì)于UKF、CKF及其衍生算法中未結(jié)合非線性動(dòng)態(tài)數(shù)據(jù)均衡算法的,跟蹤效果也不穩(wěn)定，本文算法表現(xiàn)穩(wěn)定。

(“——”表示真值;“- - - -”表示CKF的濾波結(jié)果;“……”表示CCKF的濾波結(jié)果?！啊眃enotes the ture state;“- - - -”denotes the state estimated by CKF;“……”denote the state estimated by CCKF)

圖1 CKF和CCKF的濾波效果對(duì)比
Fig.1 The comparation of the estimated states by CKF and by CCKF

(“- - - -”表示CKF的絕度濾波誤差;“……”表示CCKF的絕度濾波誤差?！? - - -”denotes the absolute errors using CKF ;“……”denote the absolute errors using CCKF.)

圖2 兩種算法的絕對(duì)誤差比較

Fig.2 The comparation of the absoluote errors between the double algorithms

5 結(jié)語(yǔ)

本文在容積卡爾曼濾波(CKF)和非線性動(dòng)態(tài)數(shù)據(jù)均衡算法(NDDR)的基礎(chǔ)上提出了處理一類邊界約束的容積卡爾曼濾波,將CKF的容積點(diǎn)約束在求積空間中,提出相應(yīng)的求權(quán)重的方法,同時(shí)將非線性動(dòng)態(tài)數(shù)據(jù)均衡的方法融入到狀態(tài)估計(jì)過(guò)程中,提高濾波精度,并使濾波的每個(gè)過(guò)程都在約束之內(nèi)。仿真表明,本文算法在濾波收斂速度、濾波精度和魯棒性等方面均優(yōu)于原有的容積卡爾曼濾波。

本文提出的算法并非最優(yōu)估計(jì)算法。一個(gè)高斯分布的概率密度函數(shù)在通過(guò)非線性系統(tǒng)之后,其概率密度會(huì)產(chǎn)生畸變,新的概率密度將不再是高斯函數(shù),甚至完全變形。而目前大部分非線性濾波仍假設(shè)是高斯分布,因此本文算法是次優(yōu)的，但大量仿真表明本算法的有效性。

參考文獻(xiàn)：

[1] Kalman R E. A new approach to linear filtering and prediction problems[J]. Journal of Basic Engineering, 1960, 82(1): 35.

[2] Julier S J, Uhlmann J K. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3): 401-422.

[3] Alspach D L, Sorenson H W. Nonlinear bayesian estimation using Gaussian sum approximations[J]. Automatic Control, IEEE Transactions on, 1972, 17(4): 439-448.

[4] Arulampalam M S, Maskell S, Gordon N, et al. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J]. IEEE Transactions on Signal Processing, 2002, 50(2): 174-188.

[5] Arasaratnam I, Haykin S. Cubature Kalman Filters[J]. Automatic Control, IEEE Transactions on, 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] Simon D. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches[M]. New York: Wiley-Interscience, 2006.

[8] Frank P M, Wer F A, Badgwell T A, et al. Nonlinear Predictive Control and Moving Horizon Estimation — An Introductory Overview[M]. London: Springer London, 1999: 391-449.

[9] Robertson D G, Lee J H, Rawlings J B. A moving horizon-based approach for least-squares estimation[J]. AIChE Journal, 1996, 42(8): 2209-2224.

[10] Vachhani P, Narasimhan S, Rengaswamy R. Recursive state estimation in Nonlinear processes[C]. Proceedings of the American Control Conference. New York: IEEE, 2004: 200-204.

[11] Vachhani P, Narasimhan S, Rengaswamy R. Robust and reliable estimation via Unscented Recursive Nonlinear Dynamic Data Reconciliation[J]. Journal of Process Control, 2006, 16(10): 1075-1086.

[12] Zarei J, Shokri E. Nonlinear and Constrained State Estimation Based on the Cubature Kalman Filter[J]. Industrial & Engineering Chemistry Research, 2014, 53(10): 3938-3949.

[13] Haseltine E L, Rawlings J B. Critical evaluation of extended Kalman filtering and moving-horizon estimation[J]. Industrial & Engineering Chemistry Research, 2005, 44(8): 2451-2460.