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

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

1 引 言

在系統(tǒng)狀態(tài)估計問題中，非線性非高斯系統(tǒng)的狀態(tài)估計廣泛存在于經(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è)條件，對于非線性非高斯系統(tǒng)狀態(tài)估計無法取得較好的估計精度。

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

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

2 系統(tǒng)模型

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

(1)

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

3 7th-CQKF

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

(2)

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

(3)

球面積分形式為

(4)

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

(5)

式中：λi為正交采樣點，其確定公式為

(6)

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

(7)

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

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

(8)

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

(9)

(10)

(11)

(12)

f(h3)表示對函數(shù)進(jìn)行一次h3采樣，包括點集{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個采樣點計算高斯權(quán)重積分，計算出采樣點及其權(quán)重就可通過時間更新和量測更新得到7th-CQKF算法。

3.1 時間更新

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 量測更新

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算法

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

Step1 初始化。

(36)

(37)

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

計算容積點：

(38)

(39)

時間更新：

(40)

(41)

(42)

計算容積點：

(43)

(44)

量測更新：

(45)

(46)

(47)

(48)

(49)

(50)

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

(51)

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

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

(52)

權(quán)重歸一化：

(53)

Step5 重采樣。

Step6 狀態(tài)估計。

(54)

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

5 仿真分析

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

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

(55)

(56)

定義量測向量[L,α]T，量測方程為

(57)

量測噪聲為均勻分布，其中斜距離量測噪聲分布為nL～unif(-30,30)，方位角量測噪聲分布為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，運動時間為50 s，粒子都采樣100個，運行100次蒙特卡洛實驗，對位置、速度和角速度的RMSE對比如圖1～3所示。

圖1 位置RMSE對比Fig.1 The comparison of RMSE for position

圖2 速度RMSE對比Fig.2 The comparison of RMSE for velocity

圖3 角速度RMSE對比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è)計重要性密度函數(shù)，能夠獲取比CPF更好的狀態(tài)估計精度，可有效處理非線性非高斯系統(tǒng)狀態(tài)估計問題。

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

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

6 結(jié)束語

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