改進(jìn)高階容積粒子濾波的系統(tǒng)狀態(tài)估計(jì)算法

金俊平，萬仲保2，杜軍龍，周劍濤

(1.江西省信息中心，南昌 330001；2.華東交通大學(xué) 軟件學(xué)院，南昌 330013)

1 引 言

在系統(tǒng)狀態(tài)估計(jì)問題中，非線性非高斯系統(tǒng)的狀態(tài)估計(jì)廣泛存在于經(jīng)濟(jì)、軍事及工程領(lǐng)域。經(jīng)典濾波算法，如卡爾曼濾波(Kalman Filter,KF)[1]、擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)[2-3]、不敏卡爾曼濾波(Unscented Kalman Filter,UKF)[4]、容積卡爾曼濾波(Cubature Kalman Filter,CKF)[5-6]等，都受限于算法噪聲假設(shè)條件，對(duì)于非線性非高斯系統(tǒng)狀態(tài)估計(jì)無法取得較好的估計(jì)精度。

基于蒙特卡洛與貝葉斯理論的粒子濾波(Particle Filter,PF)采用大量粒子描述概率分布，能夠處理任意噪聲條件下的線性或者非線性系統(tǒng)[7-8]。對(duì)于PF，重要性密度函數(shù)的選取是一個(gè)重要步驟，較為常用的方法是采用狀態(tài)轉(zhuǎn)移先驗(yàn)分布作為重要性密度函數(shù)，但該方法在粒子更新階段沒有包含最新量測(cè)信息。針對(duì)該問題，文獻(xiàn)[9]采用EKF對(duì)粒子進(jìn)行傳遞，將最新量測(cè)信息融入各個(gè)粒子，提高了對(duì)狀態(tài)后驗(yàn)概率密度函數(shù)的近似程度?；谕瑯铀悸?，文獻(xiàn)[10-12]采用UKF對(duì)粒子進(jìn)行傳遞，提出不敏粒子濾波(Unscented Particle Filter,UPF)；文獻(xiàn)[13-15]采用CKF對(duì)粒子進(jìn)行濾波，提出容積粒子濾波(Cubature Particle Filter,CPF)，在先驗(yàn)分布更新階段融入了最新量測(cè)信息，獲得了比經(jīng)典PF更好的狀態(tài)估計(jì)精度；文獻(xiàn)[16]基于七階正交容積準(zhǔn)則提出七階正交容積卡爾曼濾波(Seventh-degree Cubature Quadrature Kalman Filter,7th-CQKF)，能夠獲取比三階和五階算法更高的濾波精度，可應(yīng)用于改進(jìn)PF的估計(jì)效果。

本文針對(duì)經(jīng)典PF采用狀態(tài)轉(zhuǎn)移先驗(yàn)分布作為重要性密度函數(shù)不能包含最新量測(cè)信息的問題，提出了一種改進(jìn)高階CPF的系統(tǒng)狀態(tài)估計(jì)算法。采用7th-CQKF設(shè)計(jì)PF的重要性密度函數(shù)，通過7th-CQKF對(duì)PF粒子更新，提高對(duì)后驗(yàn)概率密度函數(shù)的近似程度，從而提高對(duì)系統(tǒng)狀態(tài)的估計(jì)精度。仿真結(jié)果表明，相比CPF，所提算法具有更好的估計(jì)精度。

2 系統(tǒng)模型

離散形式動(dòng)態(tài)系統(tǒng)為

(1)

式中：Xk∈n為n維狀態(tài)向量，Zk∈m為m維量測(cè)向量,Wk和Vk為相互獨(dú)立的狀態(tài)噪聲和量測(cè)噪聲。

3 7th-CQKF

7th-CQKF采用七階線性積分準(zhǔn)則和七階球面積分準(zhǔn)則實(shí)現(xiàn)對(duì)任意函數(shù)積分的近似，保留更多階的近似項(xiàng)，能夠獲取更好的系統(tǒng)狀態(tài)估計(jì)結(jié)果。對(duì)任意函數(shù)的多維權(quán)重積分：

(2)

該積分值是無法計(jì)算的，可被分解為線性積分和球面積分兩類，即線性積分為

(3)

球面積分形式為

(4)

7th-CQKF對(duì)于線性積分，采用高斯-拉蓋爾正交準(zhǔn)則，應(yīng)用正交點(diǎn)近似為七階：

(5)

式中：λi為正交采樣點(diǎn)，其確定公式為

(6)

式中：a=n/2-1，n為維數(shù)。通過公式(6)可計(jì)算出不同維數(shù)下的7個(gè)正交采樣點(diǎn)。權(quán)重

(7)

式中：Γ(·)表示gamma函數(shù)。公式(5)～(7)即為七階線性積分準(zhǔn)則。

7th-CQKF對(duì)于球面積分，推導(dǎo)七階球面積分：

(8)

式中：f(h1)表示對(duì)函數(shù)進(jìn)行一次h1采樣，包括點(diǎn)集{ej}和{-ej}，ej表示單位矩陣的第j列，有2n個(gè)采樣點(diǎn)；f(h2)表示對(duì)函數(shù)進(jìn)行一次h2采樣，包括點(diǎn)集{m1}、{m2}、{m3}和{m4}，分別表示為

(9)

(10)

(11)

(12)

f(h3)表示對(duì)函數(shù)進(jìn)行一次h3采樣，包括點(diǎn)集{m5}、{m6}、{m7}和{m8}，分別表示為

{m5}={el+ek+ej,l

(13)

{m6}={el+ek-ej,l

(14)

{m7}={el-ek+ej,l

(15)

{m8}={el-ek-ej,l

(16)

公式(8)～(16)為七階球面準(zhǔn)則。進(jìn)而7th-CQKF將公式(2)簡化為

(17)

式中：

(18)

(19)

(20)

公式(17)～(20)的七階正交容積規(guī)則可歸納為

(21)

式中：

(22)

(23)

公式(22)和公式(23)中，i=1,2,…,7。7th-CQKF采用14n(2n2+1)/3個(gè)采樣點(diǎn)計(jì)算高斯權(quán)重積分，計(jì)算出采樣點(diǎn)及其權(quán)重就可通過時(shí)間更新和量測(cè)更新得到7th-CQKF算法。

3.1 時(shí)間更新

Pk|k=Sk|k(Sk|k)T，

(24)

(25)

(26)

Pk+1|k=∑ωiif(Xii,k|k)[f(Xii,k|k)]T-

(27)

3.2 量測(cè)更新

Pk+1|k=Sk+1‖k(Sk+1|k)T，

(28)

(29)

(30)

PXZ,k+1|k=∑ωiif(Xii,k|k)[h(Xii,k+1|k)]T-

(31)

(32)

Κk+1=PXZ,k+1|k(PZZ,k+1|k)-1，

(33)

(34)

Pk+1|k+1=Pk+1|k+Κk+1PZZ,k+1|k(Κk+1)T。

(35)

4 改進(jìn)高階CPF算法

針對(duì)經(jīng)典PF采用狀態(tài)轉(zhuǎn)移概率函數(shù)作為重要性密度函數(shù)不能包含最新量測(cè)信息問題，將7th-CQKF應(yīng)用到PF框架中，通過7th-CQKF設(shè)計(jì)重要性密度函數(shù)，產(chǎn)生新的后驗(yàn)概率密度分布，通過新的后驗(yàn)概率密度產(chǎn)生新的粒子，采用反比例函數(shù)計(jì)算粒子權(quán)重并進(jìn)行歸一化，最后輸出系統(tǒng)狀態(tài)估計(jì)值。本文提出的改進(jìn)高階CPF算法可概括如下：

Step1 初始化。

(36)

(37)

Step2 在估計(jì)時(shí)刻內(nèi)，采用7th-CQKF對(duì)各個(gè)粒子進(jìn)行傳遞。

計(jì)算容積點(diǎn)：

(38)

(39)

時(shí)間更新：

(40)

(41)

(42)

計(jì)算容積點(diǎn)：

(43)

(44)

量測(cè)更新：

(45)

(46)

(47)

(48)

(49)

(50)

Step3 產(chǎn)生新粒子。

(51)

Step4 采用反比例函數(shù)計(jì)算權(quán)重。

傳統(tǒng)PF采用正態(tài)分布函數(shù)作為確定權(quán)值的概率密度函數(shù)，對(duì)大噪聲粒子和小噪聲粒子的權(quán)重區(qū)分不夠明顯，采用反比例函數(shù)計(jì)算可突顯兩者之間權(quán)重差別，對(duì)小噪聲粒子賦予權(quán)值更大，更接近真實(shí)情況。計(jì)算公式為

(52)

權(quán)重歸一化：

(53)

Step5 重采樣。

Step6 狀態(tài)估計(jì)。

(54)

根據(jù)新粒子的狀態(tài)值、協(xié)方差矩陣重復(fù)Step 2～6，直至結(jié)束。

5 仿真分析

為檢驗(yàn)所提算法對(duì)非線性非高斯系統(tǒng)狀態(tài)的估計(jì)能力，采用CPF算法作為對(duì)比，以驗(yàn)證改進(jìn)高階CPF的優(yōu)良估計(jì)能力。仿真的硬件條件為主頻3.20 GHz、內(nèi)存8.00 GHz的計(jì)算機(jī)，軟件為Matlab 2014a。

非線性非高斯系統(tǒng)的狀態(tài)方程為

(55)

(56)

定義量測(cè)向量[L,α]T，量測(cè)方程為

(57)

量測(cè)噪聲為均勻分布，其中斜距離量測(cè)噪聲分布為nL～unif(-30,30)，方位角量測(cè)噪聲分布為nα～unif(-4°,4°)，目標(biāo)初始狀態(tài)[2 000，300,2 000，0,-3×10-3]T，初始協(xié)方差[100，10,100，10,100]T，采樣間隔T=1 s，運(yùn)動(dòng)時(shí)間為50 s，粒子都采樣100個(gè)，運(yùn)行100次蒙特卡洛實(shí)驗(yàn)，對(duì)位置、速度和角速度的RMSE對(duì)比如圖1～3所示。

圖1 位置RMSE對(duì)比Fig.1 The comparison of RMSE for position

圖2 速度RMSE對(duì)比Fig.2 The comparison of RMSE for velocity

圖3 角速度RMSE對(duì)比Fig.3 The comparison of RMSE for angle velocity

從圖1～3的跟蹤結(jié)果可以看出，由于7th-CQKF在非線性系統(tǒng)處理能力上優(yōu)于CKF，故本文提出的改進(jìn)高階CPF算法在粒子傳遞過程中，采用7th-CQKF設(shè)計(jì)重要性密度函數(shù)，能夠獲取比CPF更好的狀態(tài)估計(jì)精度，可有效處理非線性非高斯系統(tǒng)狀態(tài)估計(jì)問題。

在算法運(yùn)算時(shí)間方面，CPF算法一次蒙特卡洛平均耗時(shí)為1.75 s，改進(jìn)高階CPF算法一次蒙特卡洛平均耗時(shí)16.32 s，這是由于改進(jìn)高階CPF算法采用7th-CQKF更新PF的粒子，采樣個(gè)數(shù)大于CKF造成的。

需要指出的是，提出的改進(jìn)高階CPF算法通過具有大量采樣點(diǎn)的7th-CQKF更新PF的粒子，計(jì)算量較大，對(duì)于離線系統(tǒng)或者實(shí)時(shí)性要求不高的系統(tǒng)狀態(tài)估計(jì)問題能夠有較好應(yīng)用。對(duì)于實(shí)時(shí)性要求較高的系統(tǒng)狀態(tài)估計(jì)問題可通過提高硬件條件方式滿足要求，如果仍不能滿足，采用其他狀態(tài)估計(jì)方法。

6 結(jié)束語

針對(duì)非線性非高斯系統(tǒng)狀態(tài)估計(jì)問題，本文提出一種改進(jìn)高階CPF的系統(tǒng)狀態(tài)估計(jì)算法。該算法采用7th-CQKF設(shè)計(jì)PF的重要性密度函數(shù)，使最新量測(cè)信息融入重要性密度函數(shù)，提高了對(duì)狀態(tài)后驗(yàn)概率密度的近似精度；同時(shí)采用反比例函數(shù)計(jì)算粒子權(quán)重，增加了對(duì)大噪聲粒子和小噪聲粒子的區(qū)分。相比已有的CPF算法，本文提出算法具有更高的狀態(tài)估計(jì)精度。本文研究提高了對(duì)非線性非高斯系統(tǒng)狀態(tài)估計(jì)精度，同時(shí)將7th-CQKF擴(kuò)展到了非高斯系統(tǒng)狀態(tài)估計(jì)領(lǐng)域。下一步將研究如何提高算法計(jì)算速度，以使其在實(shí)時(shí)性要求較高的系統(tǒng)狀態(tài)估計(jì)中得到更好應(yīng)用。