改进高阶容积粒子滤波的系统状态估计算法

金俊平，万仲保2，杜军龙，周剑涛

(1.江西省信息中心，南昌 330001；2.华东交通大学 软件学院，南昌 330013)

1 引 言

在系统状态估计问题中，非线性非高斯系统的状态估计广泛存在于经济、军事及工程领域。经典滤波算法，如卡尔曼滤波(Kalman Filter,KF)[1]、扩展卡尔曼滤波(Extended Kalman Filter,EKF)[2-3]、不敏卡尔曼滤波(Unscented Kalman Filter,UKF)[4]、容积卡尔曼滤波(Cubature Kalman Filter,CKF)[5-6]等，都受限于算法噪声假设条件，对于非线性非高斯系统状态估计无法取得较好的估计精度。

基于蒙特卡洛与贝叶斯理论的粒子滤波(Particle Filter,PF)采用大量粒子描述概率分布，能够处理任意噪声条件下的线性或者非线性系统[7-8]。对于PF，重要性密度函数的选取是一个重要步骤，较为常用的方法是采用状态转移先验分布作为重要性密度函数，但该方法在粒子更新阶段没有包含最新量测信息。针对该问题，文献[9]采用EKF对粒子进行传递，将最新量测信息融入各个粒子，提高了对状态后验概率密度函数的近似程度。基于同样思路，文献[10-12]采用UKF对粒子进行传递，提出不敏粒子滤波(Unscented Particle Filter,UPF)；文献[13-15]采用CKF对粒子进行滤波，提出容积粒子滤波(Cubature Particle Filter,CPF)，在先验分布更新阶段融入了最新量测信息，获得了比经典PF更好的状态估计精度；文献[16]基于七阶正交容积准则提出七阶正交容积卡尔曼滤波(Seventh-degree Cubature Quadrature Kalman Filter,7th-CQKF)，能够获取比三阶和五阶算法更高的滤波精度，可应用于改进PF的估计效果。

本文针对经典PF采用状态转移先验分布作为重要性密度函数不能包含最新量测信息的问题，提出了一种改进高阶CPF的系统状态估计算法。采用7th-CQKF设计PF的重要性密度函数，通过7th-CQKF对PF粒子更新，提高对后验概率密度函数的近似程度，从而提高对系统状态的估计精度。仿真结果表明，相比CPF，所提算法具有更好的估计精度。

2 系统模型

离散形式动态系统为

(1)

式中：Xk∈n为n维状态向量，Zk∈m为m维量测向量,Wk和Vk为相互独立的状态噪声和量测噪声。

3 7th-CQKF

7th-CQKF采用七阶线性积分准则和七阶球面积分准则实现对任意函数积分的近似，保留更多阶的近似项，能够获取更好的系统状态估计结果。对任意函数的多维权重积分：

(2)

该积分值是无法计算的，可被分解为线性积分和球面积分两类，即线性积分为

(3)

球面积分形式为

(4)

7th-CQKF对于线性积分，采用高斯-拉盖尔正交准则，应用正交点近似为七阶：

(5)

式中：λi为正交采样点，其确定公式为

(6)

式中：a=n/2-1，n为维数。通过公式(6)可计算出不同维数下的7个正交采样点。权重

(7)

式中：Γ(·)表示gamma函数。公式(5)～(7)即为七阶线性积分准则。

7th-CQKF对于球面积分，推导七阶球面积分：

(8)

式中：f(h1)表示对函数进行一次h1采样，包括点集{ej}和{-ej}，ej表示单位矩阵的第j列，有2n个采样点；f(h2)表示对函数进行一次h2采样，包括点集{m1}、{m2}、{m3}和{m4}，分别表示为

(9)

(10)

(11)

(12)

f(h3)表示对函数进行一次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)为七阶球面准则。进而7th-CQKF将公式(2)简化为

(17)

式中：

(18)

(19)

(20)

公式(17)～(20)的七阶正交容积规则可归纳为

(21)

式中：

(22)

(23)

公式(22)和公式(23)中，i=1,2,…,7。7th-CQKF采用14n(2n2+1)/3个采样点计算高斯权重积分，计算出采样点及其权重就可通过时间更新和量测更新得到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 改进高阶CPF算法

针对经典PF采用状态转移概率函数作为重要性密度函数不能包含最新量测信息问题，将7th-CQKF应用到PF框架中，通过7th-CQKF设计重要性密度函数，产生新的后验概率密度分布，通过新的后验概率密度产生新的粒子，采用反比例函数计算粒子权重并进行归一化，最后输出系统状态估计值。本文提出的改进高阶CPF算法可概括如下：

Step1 初始化。

(36)

(37)

Step2 在估计时刻内，采用7th-CQKF对各个粒子进行传递。

计算容积点：

(38)

(39)

时间更新：

(40)

(41)

(42)

计算容积点：

(43)

(44)

量测更新：

(45)

(46)

(47)

(48)

(49)

(50)

Step3 产生新粒子。

(51)

Step4 采用反比例函数计算权重。

传统PF采用正态分布函数作为确定权值的概率密度函数，对大噪声粒子和小噪声粒子的权重区分不够明显，采用反比例函数计算可突显两者之间权重差别，对小噪声粒子赋予权值更大，更接近真实情况。计算公式为

(52)

权重归一化：

(53)

Step5 重采样。

Step6 状态估计。

(54)

根据新粒子的状态值、协方差矩阵重复Step 2～6，直至结束。

5 仿真分析

为检验所提算法对非线性非高斯系统状态的估计能力，采用CPF算法作为对比，以验证改进高阶CPF的优良估计能力。仿真的硬件条件为主频3.20 GHz、内存8.00 GHz的计算机，软件为Matlab 2014a。

非线性非高斯系统的状态方程为

(55)

(56)

定义量测向量[L,α]T，量测方程为

(57)

量测噪声为均匀分布，其中斜距离量测噪声分布为nL～unif(-30,30)，方位角量测噪声分布为nα～unif(-4°,4°)，目标初始状态[2 000，300,2 000，0,-3×10-3]T，初始协方差[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的跟踪结果可以看出，由于7th-CQKF在非线性系统处理能力上优于CKF，故本文提出的改进高阶CPF算法在粒子传递过程中，采用7th-CQKF设计重要性密度函数，能够获取比CPF更好的状态估计精度，可有效处理非线性非高斯系统状态估计问题。

在算法运算时间方面，CPF算法一次蒙特卡洛平均耗时为1.75 s，改进高阶CPF算法一次蒙特卡洛平均耗时16.32 s，这是由于改进高阶CPF算法采用7th-CQKF更新PF的粒子，采样个数大于CKF造成的。

需要指出的是，提出的改进高阶CPF算法通过具有大量采样点的7th-CQKF更新PF的粒子，计算量较大，对于离线系统或者实时性要求不高的系统状态估计问题能够有较好应用。对于实时性要求较高的系统状态估计问题可通过提高硬件条件方式满足要求，如果仍不能满足，采用其他状态估计方法。

6 结束语

针对非线性非高斯系统状态估计问题，本文提出一种改进高阶CPF的系统状态估计算法。该算法采用7th-CQKF设计PF的重要性密度函数，使最新量测信息融入重要性密度函数，提高了对状态后验概率密度的近似精度；同时采用反比例函数计算粒子权重，增加了对大噪声粒子和小噪声粒子的区分。相比已有的CPF算法，本文提出算法具有更高的状态估计精度。本文研究提高了对非线性非高斯系统状态估计精度，同时将7th-CQKF扩展到了非高斯系统状态估计领域。下一步将研究如何提高算法计算速度，以使其在实时性要求较高的系统状态估计中得到更好应用。