CN103389094B - 一种改进的粒子滤波方法 - Google Patents

Abstract

本发明提供的是一种改进的粒子滤波方法。包括：（1）运载体启动后，选取运载体状态函数及量测函数；（2）选取渐消因子和弱化因子；（3）采用STSRCKF设计重要性密度函数；（4）重新产生粒子；（5）计算粒子重要性权值并归一化；（6）重采样；（7）状态估计；（8）时刻迭代更新，判断当前时刻k是否为迭代终止时刻，若当前时刻k不是迭代终止时刻，则由当前时刻k更新到下一时刻k+1，重复执行步骤（2）；若当前时刻k是迭代终止时刻，则结束，控制运载体停止运动。

Description

一种改进的粒子滤波方法

技术领域

本发明涉及的是一种粒子滤波方法。

背景技术

非线性非高斯系统状态估计问题广泛存在于运载体（移动机器人、水下航行器等）导航领域，备受许多学者关注。本发明涉及的是一种基于强跟踪平方根容积粒子滤波（Strong Tracking Square Root Cubature Particle Filter，即STSRCPF）技术。

基于贝叶斯理论与Monte Carlo方法的粒子滤波（particle filter，PF）是一种新的处理非线性非高斯系统的有效方法，这种方法利用大量的随机样本描述概率分布，并在测量的基础上调节各粒子权值的大小和样本位置来近似实际后验概率分布，因此该方法在任何非线性非高斯系统中都可以进行估计。由于粒子需要从重要性密度函数（或称为建议分布）中抽取，因此重要性密度函数的选择就影响着粒子滤波性能的好坏。而传统的粒子滤波方法采用未含当前最新测量数据的状态转移先验分布作为重要性密度函数，引入了较大的权重方差，无法很好地逼近后验概率，尤其当量测数据出现在转移概率分布的尾部或似然函数同转移概率分布相比过于集中时（如呈尖峰型），粒子滤波可能失败。

为了很好地解决粒子滤波出现的上述问题，文献《Cubature粒子滤波》（系统工程与电子技术,2011,33(11):2554-2557）提出容积粒子滤波，将CKF引入到粒子滤波框架中，在先验分布更新阶段融入了最新的量测数据，采用CKF设计粒子滤波的重要性密度函数，使重要性密度函数更加接近系统状态后验概率密度，提高了滤波估计精度。

上述容积粒子滤波方法在精度和稳定性方面仍有提高的余地。

发明内容

本发明的目的在于提供一种能改善标准粒子滤波的估计性能，精度高、稳定性强的运用于运载体导航过程的改进的粒子滤波方法。

本发明的目的是这样实现的：

（1）运载体启动后，

首先，选取运载体状态函数及量测函数如下：

其中，x_k为状态向量，z_k为量测向量，f(x_k-1)和h(x_k)分别为运载体状态函数和量测函数，w_k-1为随机系统噪声，且w_k-1～N(0,Q)，v_k为随机量测噪声，且v_k～N(0,R)；

在初始时刻，即k＝0时刻，设定运载体的初始状态，初始状态协方差P₀＝[0]_8×8，系统噪声Q，量测噪声R，并设定粒子数N，重采样阈值N_threshold，用于求取渐消因子C_0,k的初始值C_0,0和弱化因子ρ；

从先验分布P(x₀)中选取粒子，其中i＝1,2,…,N：

式中，P(x₀)为先验分布，为所选取的粒子，为所选取的第i个粒子的均值，为所选取的第i个粒子的协方差；

（2）选取渐消因子和弱化因子

选取渐消因子λ_k和弱化因子β如下：

其中，λ_k为渐消因子，当模型较准确时，λ_k＝1，STSRCKF退化为普通SRCKF；tr(·)为求矩阵的迹；β≥1为弱化因子；S_zz,k|k-1为新息协方差矩阵的平方根因子；γ_k是残差；0＜ρ≤1为遗忘因子，能提高滤波器的快速跟踪能力，通常取；ρ＝0.95；

（3）采用STSRCKF设计重要性密度函数

采用STSRCKF方法的时间更新和量测更新更新每个粒子：

①时间更新

若k-1时刻的后验概率为，分解因式得：

S_k-1|k-1＝Chol{P_k-1|k-1}

计算容积点，i＝1,2,…,m；m＝2n：

通过状态方程传播容积点：

估计k时刻的状态预测值：

计算k时刻状态协方差预测值的平方根因子：

②量测更新，融入最新量测数据：

计算容积点，i＝1,2,…,m；m＝2n：

通过量测方程传播容积点：

z_i,k|k-1＝h(X_i,k|k-1)

估计k时刻的量测预测值：

计算新息协方差矩阵的平方根因子：

S_zz,k|k-1＝Tria([ξ_k|k-1 S_R,k])

计算互相关协方差矩阵：

引入渐消因子λ_k实时估计滤波增益：

计算k时刻的状态估计值：

计算k时刻状态协方差估计值的平方根因子：

S_k|k＝Tria([η_k|k-1-K_kζ_k|k-1 K_kS_R,k])

其中，、P_k-1|k-1分别为k-1时刻的状态估计值和协方差估计值；S_k-1|k-1为P_k-1|k-1的乔里斯基因子；[1]_i表示集合[1]的第i列，[1]=[enye()n-，；m＝2n为容积点个数，n＝8为UUV状态向量的维数；X_i,k-1|k-1、X_i,k|k-1分别为k-1时刻和k时刻的容积点；f(·)、h(·)分别为状态方程和量测方程；z_i,k|k-1分别为通过状态方程和量测方程传播后的容积点；S_Q,k-1、S_R,k分别为Q_k-1和R_k的平方根因子； 分别为k时刻的状态预测值和量测预测值；S_k|k-1、S_k|k分别为k时刻状态协方差预测值和估计值的平方根因子；S_zz,k|k-1为信息协方差矩阵的平方根因子；P_xz,k|k-1为互相关协方差矩阵；K_k为滤波增益；z_k为量测向量；

（4）重新产生粒子

重新产生粒子：

其中，表示采用重要性密度函数重新抽取粒子 表示新产生的粒子所服从的分布；

（5）计算粒子重要性权值并归一化

对于i＝1,2,…,N，计算重要性权值并归一化

计算重要性权值：

归一化：

其中，为k时刻第i个粒子未经过归一化的重要性权值，为k时刻第i个粒子归一化后的重要性权值，为采用STSRCKF设计的重要性密度函数，为似然函数，为预测密度函数；

（6）重采样

计算有效粒子数N_eff，并将有效粒子数与设定的重采样阈值进行比较，若N_eff＜N_threshold，则进行重采样过程，得到新的粒子集j＝1,2,…N，N个粒子的权值相等，均为

其中，N_eff为有效粒子数，N_threshold为重采样阈值，为重采样后获得的粒子；

（7）状态估计

计算状态估计值：

计算协方差估计值：

其中，为k时刻的状态估计值，P_k|k为协方差估计值；

（8）时刻迭代更新

判断当前时刻k是否为迭代终止时刻，若当前时刻k不是迭代终止时刻，则由当前时刻k更新到下一时刻k+1，重复执行步骤（2）；若当前时刻k是迭代终止时刻，则结束，控制运载体停止运动。

本发明的有益效果在于：

STSRCPF采用具有强跟踪性能的SRCKF设计重要性密度函数，STSRCKF通过引入时变渐消因子和弱化因子，实时修正滤波增益矩阵，实现残差序列正交，确保了滤波器的准确跟踪能力；并通过传播目标状态的均值和协方差的平方根，确保协方差矩阵的对称性和半正定性，从而改进SRCKF和CPF的估计精度和稳定性。

附图说明

图1一种改进的粒子滤波方法结构框图；

图2无人水下航行器的全局坐标系与船体坐标系示意图；

图3三种滤波方法的估计轨迹与GPS真实轨迹对比曲线示意图；

图4图3中矩形区域的局部放大示意图；

图5东向误差比较示意图；

图6北向误差比较示意图。

具体实施方式

下面给出本发明的优选实施方式，并结合附图和无人水下航行器湖试例证加以说明。

如附图1所示，本发明的目的是通过如下步骤实现的：

（1）启动运载体

首先，选取运载体状态函数及量测函数如下：

其中，x_k为状态向量，z_k为量测向量，f(x_k-1)和h(x_k)分别为运载体状态函数和量测函数，w_k-1为随机系统噪声，且w_k-1～N(0,Q)，v_k为随机量测噪声，且v_k～N(0,R)。

在初始时刻，即k＝0时刻，设定运载体的初始状态，初始状态协方差P₀＝[0]_8×8，系统噪声Q，量测噪声R，并设定粒子数N，重采样阈值N_threshold，用于求取渐消因子C_0,k的初始值C_0,0和弱化因子ρ。

从先验分布P(x₀)中选取粒子

式中，P(x₀)为先验分布，为所选取的粒子，为所选取的第i个粒子的均值，为所选取的第i个粒子的协方差。

（2）选取渐消因子和弱化因子

CKF基于容积准则，计算出一组偶数个具有同等权值的容积点，它们能完全捕获高斯分布变量的均值和方差，且经过非线性系统方程转换后，其后验均值和方差能够精确到非线性系统泰勒级数展开的三阶项或更高阶项，无需对非线性模型进行线性化，不依赖于具体系统模型的非线性方程，算法相对独立，适用于任何形式的非线性模型。但CKF仍存在对模型参数变化的鲁棒性差、收敛速度慢及对突变状态的跟踪能力低等缺陷。

SRCKF是CKF的平方根实现，其通过传播状态协方差的平方根，确保了协方差矩阵的对称性和半正定性，改进了数值精度和稳定性。因此，本发明将SRCKF和STF相结合，设计了一种STSRCKF方法，并将STSRCKF引入到粒子滤波框架中，提出STSRCPF滤波方法。

标准SRCKF成为STSRCKF的充分条件是在线选择k时刻的时变滤波增益K_k，使得：

式中，γ_k是k时刻的残差，k＝0,1,2…，j＝1,2…，x_k为k时刻的真实状态值，为k时刻的状态估计值。条件(4)是SRCKF状态估计残差最小方差性能指标；条件(5)要求不同时刻的输出残差序列处处保持正交。

实际应用中，系统模型不确定性造成滤波器的状态估计值偏离系统真实状态时，会导致滤波输出残差序列不正交，建立在前述性能指标(4)和(5)基础上的STSRCKF通过引入渐消因子强行使输出残差序列保持正交，具有类似高斯白噪声的性质，最大程度地提取输出残差序列中一切有效信息，使STSRCKF在模型不确定时仍能保持对系统状态的强跟踪能力。

STSRCKF满足引理1所述的性质。

引理1：对于系统模型(1)，令，其中为采用STSRCKF得到的状态估计值。当STSRCKF可以比较准确地估计出系统状态时，即下式成立：

式中，j＝1,2,…，k＝0,1,2…，γ_k是残差，H(·)和F(·)分别是f(x_k)和h(x_k)关于x_k的雅克比矩阵，K_k是增益矩阵，P_xz,k|k-1为互相关协方差阵，

引理1反映了残差自协方差的一个重要性质。当滤波器的增益最优时，C_j,k＝0，表明残差是不相关的，而当模型参数和噪声方差存在误差时，C_j,k≠0。如果能选择渐消矩阵使得C_j,k的最后一项，对于所有的j＝1,2,3,…，都有式(7)成立，可以认为K_k仍将是最优的。SRCKF满足式(7)时成为STSRCKF。

P_xz,k|k-1-K_kC_0,k＝0 (7)

根据SRCKF方法，，为了削弱老数据对当前滤波值的影响，引入时变渐消因子λ_k在线选择增益矩阵K_k，记为：

其中，S_zz,k|k-1为新息协方差矩阵的平方根因子。

由引理1可知，当等式P_xz,k|k-1-K_kC_0,k＝0成立时，即：

式(9)成立的一个充分条件是：

即：

对式(11)求迹得：

为削弱λ_k的调节作用，避免过调节，使状态估计更平滑，引入弱化因子β，式(12)转化为：

其中，β≥1为弱化因子；0＜ρ≤1为遗忘因子，能提高滤波器的快速跟踪能力，通常取ρ＝0.95，ρ值越大，k时刻以前的信息所占的比例越小，就越突出当前残差向量的影响。该方法具有很强的关于突变状态的跟踪能力，且在滤波达到稳态时，仍保持对缓变状态及突变状态的跟踪能力。当模型较准确时，λ_k＝1，STSRCKF退化为普通SRCKF。

（3）采用STSRCKF设计重要性密度函数

用STSRCKF设计重要性密度函数可以表示为式(14)：

其中，为采用STSRCKF设计的重要性密度函数；为采用STSRCKF更新后的粒子所服从的分布。

重要性采样：对于，k＝1,2,…，i＝1,2,…,N，采用STSRCKF方法的时间更新和量测更新更新每个粒子。

①时间更新

若k-1时刻的后验概率为，分解因式可得：

S_k-1|k-1＝Chol{P_k-1|k-1} (15)

计算容积点（i＝1,2,…,m；m＝2n）：

通过状态方程传播容积点：

估计七时刻的状态预测值：

计算七时刻状态协方差预测值的平方根因子：

②量测更新，融入最新量测数据：

计算容积点(i=1，2，…，m；m=2n)：

通过量测方程传播容积点：

z_i，k|k-1=h(x_i，k|k-1) (23)估计七时刻的量测预测值：

计算新息协方差矩阵的平方根因子：

计算互相关协方差矩阵：

引入渐消因子λ_k实时估计滤波增益：

计算k时刻的状态估计值：

计算k时刻状态协方差估计值的平方根因子：

S_k|k＝Tria([η_k|k-1-K_kζ_k|k-1 K_kS_R,k]) (32)

其中，P_k-1|k-1分别为k-1时刻的状态估计值和协方差估计值；S_k-1|k-1为P_k-1|k-1的乔里斯基因子；，[1]_i表示集合[1]的第i列，[1]=[enye()n-，；m＝2n为容积点个数（n＝8为UUV状态向量的维数）；X_i,k-1|k-1、X_i,k|k-1分别为k-1时刻和k时刻的容积点；f(·)、h(·)分别为状态方程和量测方程；z_i,k|k-1分别为通过状态方程和量测方程传播后的容积点；S_Q,k-1、S_R,k分别为Q_k-1和R_k的平方根因子； 分别为k时刻的状态预测值和量测预测值；S_k|k-1、S_k|k分别为k时刻状态协方差预测值和估计值的平方根因子；S_zz,k|k-1为信息协方差矩阵的平方根因子；P_xz,k|k-1为互相关协方差矩阵；K_k为滤波增益；z_k为量测向量。

（4）重新产生粒子

重新产生粒子：

其中，表示采用重要性密度函数重新抽取粒子 表示新产生的粒子所服从的分布。

（5）计算粒子重要性权值并归一化

对于i＝1,2,…,N，计算重要性权值并归一化。

计算重要性权值：

归一化：

其中，为k时刻第i个粒子未经过归一化的重要性权值，为k时刻第i个粒子归一化后的重要性权值，为采用STSRCKF设计的重要性密度函数，为似然函数，为预测密度函数。

（6）重采样

采用公式(44)计算有效粒子数N_eff，并将有效粒子数与设定的重采样阈值进行比较，若N_eff＜N_threshold，则进行重采样过程，得到新的粒子集N个粒子的权值相等，均为

其中，N_eff为有效粒子数，N_threshold为重采样阈值，为重采样后获得的粒子。

（7）状态估计

计算状态估计值：

计算协方差估计值：

其中，为k时刻的状态估计值，P_k|k为协方差估计值。

（8）时刻迭代更新

判断当前时刻k是否为迭代终止时刻。若当前时刻k不是迭代终止时刻，则由当前时刻k更新到下一时刻k+1，重复执行步骤（2）～（8）；若当前时刻k是迭代终止时刻，则结束计算。

（9）控制运载体停止运动

下面描述本发明的具体实施例。

通过引用申请人自研无人水下航行器（Unmanned Underwater Vehicle，UUV）于2010年3月在杭州千岛湖完成的湖试试验数据，进一步说明本发明对提高导航精度和稳定性上所带来的有益效果。

（1）湖试条件

UUV采集的传感器数据包括深度计、运动传感器OCTANS、多普勒计程仪（DopplerVelocity Log，DVL）等测量数据，UUV近水面航行且采用全球定位定位系统（GlobalPositioning System，GPS）记录其航迹。湖试中UUV的运动轨迹大致为五边形，选取其中的1圈试验航段，历时12分30秒。根据上述实测湖试数据，基于强跟踪平方根容积粒子滤波、容积粒子滤波、粒子滤波三种方法进行UUV导航定位试验。

（2）构建UUV离散时间非线性动态系统

如附图2所示，以UUV初始位置和初始艏向角建立全局坐标系L；V是UUV船体坐标系；E为北东坐标系，North方向为地磁北向。x、y为UUV在L中的位置；ψ为UUV在L中的艏向角，显然，其中z_ψ为采用运动传感器OCTANS测得的UUV艏向角。

①UUV运动模型：

选取一个简单的四自由度、常速动力学模型x_k＝f(x_k-1)+w_k-1对UUV的运动过程进行建模：

式中，[x,y,z,ψ]表示UUV在L中的位置和艏向；[u,v,w,r]表示UUV在V中相应的线速度和角速度；k表示任意采样时刻；T为航位推算传感器的采样时间间隔；w_k-1为随机系统噪声。

②UUV传感器测量模型：

湖试中，UUV配置了深度计、运动传感器OCTANS和多普勒计程仪DVL。深度计即压力传感器，通过测量水柱压力提供UUV的深度数据；UUV通过OCTANS实时测量其艏向角，即UUV艏艉向与地磁北之间的夹角；DVL可以测量海流速度、对底跟踪速度等，UUV在湖试中使用DVL进行对底跟踪速度的测量。它们提供状态向量中深度、艏向和对底速度的直接测量值，因而量测模型为线性的。本发明选取式(40)的测量模型对UUV的传感器测量进行建模：

z_k＝Hx_k+v_k (40)

式中，z_k是量测向量，v_k是量测噪声，测量矩阵H为：

③通过UUV运动模型和传感器测量模型构建UUV离散时间非线性动态系统：

式中，z_k是量测向量；随机系统噪声w_k～N(0,Q_k)，随机量测噪声v_k～N(0,R_k)，系统初始状态为x₀，x₀与w_k，v_k统计独立；非线性函数f(x_k-1)和h(x_k)为关于状态的一阶连续偏导数：

（3）参数设置

试验参数设置如表1所示：

表1试验参数设置

（4）试验结果及分析

附图3为采用强跟踪平方根容积粒子滤波、容积粒子滤波和粒子滤波三种方法的UUV运行轨迹与GPS真实轨迹的对比曲线。附图4为附图3中矩形区域的局部放大示意图。附图5、附图6分别对应三种滤波方法东向和北向的估计误差。

对比附图3、附图4、附图5和附图6的试验结果，有以下分析结果：

从附图3和附图4试验结果来看，PF的运行轨迹与GPS真实轨迹偏离最大，吻合度最低。STSRCPF的运行轨迹与GPS真实轨迹最接近，吻合度最高。理论上来说，由于考虑了最新量测值，STSRCPF及CPF的估计精度均高于标准PF。同时，对比STSRCPF和CPF，两者都是通过对后验概率密度进行高斯近似而实现系统的状态估计的，而STSRCPF采用具有强跟踪性能的SRCKF设计重要性密度函数，STSRCKF通过引入时变渐消因子和弱化因子，实时修正滤波增益矩阵，实现残差序列正交，确保了滤波器的准确跟踪能力；并通过传播目标状态的均值和协方差的平方根，确保协方差矩阵的对称性和半正定性，从而改进CPF的估计精度和稳定性。因此，理论与试验结果吻合。

附图5和附图6中k为试验迭代步数，试验总迭代过程共1464步。东向/北向的估计误差计算公式：ε＝E-T，其中，ε、E和T分别为东向/北向的估计误差、估计值和真实值。显然，东向/北向估计误差绝对值|ε|越小，滤波方法精度越高。根据附图5和附图6，三种方法东向/北向的估计误差曲线呈现相同趋势，但STSRCPF东向/北向估计误差绝对值|ε|一直是最小的，估计精度明显优于CPF和PF。

定义系统导航定位误差模型为：

式中，k是任意时刻，k_max是总的运行步数，(tx_k,ty_k)与分别是UUV在k时刻的真实位置和滤波方法估计的位置。

表2给出UUV总航时为12分30秒时，STSRCPF、CPF和PF的导航定位误差和方法平均运行时间。导航定位精度方面，STSRCPF导航定位误差最小，其导航定位精度优于CPF和PF。在运行时间方面，PF的运行时间最短，STSRCPF运行时间较短，而CPF耗时最长。在兼顾方法精度和运行时间的情况下，相对于CPF和PF，STSRCPF是粒子滤波改进方法的一种较好的权衡选择。

表2导航定位误差和平均运行时间比较

由前述基于UUV湖试数据集的验证结果可见，STSRCPF的导航定位估计精度优于CPF和PF。

Claims (1)

1.一种用于运载体导航过程的改进的粒子滤波方法，其特征是：

(1)运载体启动后，选取运载体状态函数及量测函数；

所述选取运载体状态函数及量测函数包括：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.$

其中，x_k为状态向量，z_k为量测向量，f(x_k-1)和h(x_k)分别为运载体状态函数和量测函数，w_k-1为随机系统噪声，且w_k-1～N(0,Q)，v_k为随机量测噪声，且v_k～N(0,R)；

在初始时刻，即k＝0时刻，设定运载体的初始状态x₀＝[0 0 1 0 -0.1 1.5 0.03 0]^T，初始状态协方差P₀＝[0]_8×8，系统噪声Q，量测噪声R，并设定粒子数N，重采样阈值N_threshold，用于求取渐消因子中间参数C_0,k的初始值C_0,0和弱化因子ρ；

从先验分布P(x₀)中选取粒子其中i＝1,2,…,N：

${\stackrel{\‾}{x}}_0^{\left(i\right)}=E\[x_0^{\left(i\right)}\]$

$P_0^{\left(i\right)}=E\[(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)}){(x_0^{\left(i\right)}-{\stackrel{\‾}{x}}_0^{\left(i\right)})}^T\]$

式中，P(x₀)为先验分布，为所选取的粒子，为所选取的第i个粒子的均值，为所选取的第i个粒子的协方差；

(2)选取渐消因子和弱化因子；

所述选取渐消因子和弱化因子包括：

选取渐消因子λ_k和弱化因子β如下：

${\mathrm{\λ}}_k=\frac{tr(C_{0,k}-{\mathrm{\βR}}_k)}{tr\left(S_{zz,k|k-1}{S}_{zz,k|k-1}^T\right)},C_{0,k}=\left\{\begin{array}{cc}{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T& k=1\\ \frac{{\mathrm{\ρC}}_{0,k-1}+{\mathrm{\γ}}_k{\mathrm{\γ}}_k^T}{1+\mathrm{\ρ}}& k>1\end{array}\right.$

其中，λ_k为渐消因子，当模型较准确时，λ_k＝1，STSRCKF退化为普通SRCKF；tr(·)为求矩阵的迹；β≥1为弱化因子；S_zz,k|k-1为新息协方差矩阵的平方根因子；γ_k是残差；0＜ρ≤1为遗忘因子；

(3)采用STSRCKF设计重要性密度函数；

所述采用STSRCKF设计重要性密度函数包括：

采用STSRCKF方法的时间更新和量测更新更新每个粒子：

①时间更新

若k-1时刻的后验概率为分解因式得：

S_k-1|k-1＝Chol{P_k-1|k-1}

计算容积点，i＝1,2,…,m；m＝2n：

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

通过状态方程传播容积点：

$X_{i,k-1|k-1}^{*}=f\left(X_{i,k-1|k-1}\right)$

估计k时刻的状态预测值：

${\hat{x}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}{X}_{i,k|k-1}^{*}$

计算k时刻状态协方差预测值的平方根因子：

$S_{k|k-1}=Tria(\[\begin{array}{cc}{\mathrm{\ξ}}_{k|k-1}^{*}& S_{Q,k-1}\end{array}\])$

$Q_{k-1}=S_{Q,k-1}{S}_{Q,k-1}^T$

${\mathrm{\ξ}}_{k|k-1}^{*}=\frac{1}{\sqrt{m}}\[\begin{array}{cccc}{X}_{1,k|k-1}^{*}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}^{*}-{\hat{x}}_{k|k-1}& \mathrm{...}& X_{m,k|k-1}^{*}-{\hat{x}}_{k|k-1}\end{array}\];$

②量测更新，融入最新量测数据：

计算容积点，i＝1,2,…,m；m＝2n：

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}$

通过量测方程传播容积点：

z_i,k|k-1＝h(X_i,k|k-1)

估计k时刻的量测预测值：

${\hat{z}}_{k|k-1}=\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}{z}_{i,k|k-1}$

计算新息协方差矩阵的平方根因子：

S_zz,k|k-1＝Tria([ζ_k|k-1 S_R,k])

$R_k=S_{R,k}{S}_{R,k}^T$

${\mathrm{\ζ}}_{k|k-1}=\frac{1}{\sqrt{m}}\[\begin{array}{cccc}{z}_{1,k|k-1}-{\hat{z}}_{k|k-1}& z_{2,k|k-1}-{\hat{z}}_{k|k-1}& \mathrm{...}& z_{m,k|k-1}-{\hat{z}}_{k|k-1}\end{array}\]$

计算互相关协方差矩阵：

$P_{xz,k|k-1}={\mathrm{\η}}_{k|k-1}{\mathrm{\ζ}}_{k|k-1}^T$

${\mathrm{\η}}_{k|k-1}=\frac{1}{\sqrt{m}}\[\begin{array}{cccc}{X}_{1,k|k-1}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}-{\hat{x}}_{k|k-1}& \mathrm{...}& X_{m,k|k-1}-{\hat{x}}_{k|k-1}\end{array}\]$

引入渐消因子λ_k实时估计滤波增益：

$K_k=P_{xz,k|k-1}{\left({\mathrm{\λ}}_k{S}_{zz,k|k-1}{S}_{zz,k|k-1}^T\right)}^{-1}$

计算k时刻的状态估计值：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

计算k时刻状态协方差估计值的平方根因子：

S_k|k＝Tria([η_k|k-1-K_kζ_k|k-1 K_kS_R,k])

其中，P_k-1|k-1分别为k-1时刻的状态估计值和协方差估计值；S_k-1|k-1为P_k-1|k-1的乔里斯基因子；[1]_i表示集合[1]的第i列，[1]＝[eye(n) -eye(n)]，eye(n)＝diag([1 1 1 1 1 1 1 1])；m＝2n为容积点个数，n＝8为UUV状态向量的维数；X_i,k-1|k-1、X_i,k|k-1分别为k-1时刻和k时刻的容积点；f(·)、h(·)分别为状态方程和量测方程；z_i,k|k-1分别为通过状态方程和量测方程传播后的容积点；S_Q,k-1、S_R,k分别为Q_k-1和R_k的平方根因子；分别为k时刻的状态预测值和量测预测值；S_k|k-1、S_k|k分别为k时刻状态协方差预测值和估计值的平方根因子；S_zz,k|k-1为信息协方差矩阵的平方根因子；P_xz,k|k-1为互相关协方差矩阵；K_k为滤波增益；z_k为量测向量；

(4)重新产生粒子；

所述重新产生粒子包括：

重新产生粒子：

$x_k^{\left(i\right)}~q\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)},z_k)=N({\hat{x}}_k^{\left(i\right)},P_k^{\left(i\right)})$

其中，表示采用重要性密度函数重新抽取粒子 表示新产生的粒子所服从的分布；

(5)计算粒子重要性权值并归一化；

所述计算粒子重要性权值并归一化包括：

对于i＝1,2,…,N，计算重要性权值并归一化

计算重要性权值：

$w_k^{\left(i\right)}=\frac{P\left(z_k\right|{\hat{x}}_k^{\left(i\right)})P\left({\hat{x}}_k^{\left(i\right)}\right|z_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)},z_k)}$

归一化：

${\tilde{w}}_k^{\left(i\right)}=w_k^{\left(i\right)}/\underset{j=1}{\overset{N}{\Σ}}{w}_k^{\left(j\right)}$

其中，为k时刻第i个粒子未经过归一化的重要性权值，为k时刻第i个粒子归一化后的重要性权值，为采用STSRCKF设计的重要性密度函数，为似然函数，为预测密度函数；

(6)重采样；

所述重采样的方法为：

计算有效粒子数N_eff，并将有效粒子数与设定的重采样阈值进行比较，若N_eff＜N_threshold，则进行重采样过程，得到新的粒子集j＝1,2,…N，N个粒子的权值相等，均为

$N_{eff}=1/\underset{i=1}{\overset{N}{\Σ}}{\left({\tilde{w}}_k^{\left(i\right)}\right)}^2$

其中，N_eff为有效粒子数，N_threshold为重采样阈值，为重采样后获得的粒子；

(7)状态估计；

所述状态估计包括：

计算状态估计值：

${\hat{x}}_{k|k}=\frac{1}{N}\underset{j=1}{\overset{N}{\Σ}}{x}_k^{\left(j\right)}$

计算协方差估计值：

$P_{k|k}=\frac{1}{N}\underset{j=1}{\overset{N}{\Σ}}(x_k^{\left(j\right)}-{\hat{x}}_{k|k}){(x_k^{\left(j\right)}-{\hat{x}}_{k|k})}^T$

其中，为k时刻的状态估计值，P_k|k为协方差估计值；

(8)时刻迭代更新，

判断当前时刻k是否为迭代终止时刻，若当前时刻k不是迭代终止时刻，则由当前时刻k更新到下一时刻k+1，重复执行步骤(2)；若当前时刻k是迭代终止时刻，则结束，控制运载体停止运动。