自适应无迹卡尔曼粒子滤波方法与流程

本发明属于非线性组合导航领域，具体为一种自适应无迹卡尔曼粒子滤波方法。

背景技术：

常规卡尔曼滤波是一种基于模型的线性最小方差估计，但是实际工程实践中，系统往往是非线性的。拓展卡尔曼滤波(ekf，extendedkalmanfilter)虽然可以解决非线性系统状态估计的问题，但是其对系统方程做泰勒展开，忽略了高阶项对系统的影响，存在一定的截断误差。无迹卡尔曼滤波ukf(unscentedkalmanfilter)根据被估计量和量测量的协方差来确定最佳增益矩阵，协方差阵又根据复现的一倍σ样本点来计算，而样本点则根据系统的非线性模型计算。所以即可以用于线性系统也可以用于非线性系统。ekf和ukf都是递推型的滤波方法，二者的核心思想都是通过参数化的解析形式对非线性系统进行一定的近似化。

粒子滤波pf(particlefilter)则直接根据概率密度来计算条件均值，k时刻的滤波估计值由众多不同分布的粒子加权确定。其核心是选择合适的推荐概率密度，推荐概率密度与实际越接近，滤波效果越明显。滤波时，后验概率无法得知，故目前还没有一种普遍的推荐概率密度函数，不同的推荐概率密度函数效果也不尽相同。

将pf与ukf相结合，推荐密度由ukf确定，可以使粒子更新时获得量测量的最新后验信息，有利于粒子向概率大的方向移动，估计效果好于普通的粒子滤波。

大多数滤波方法对强非线性系统状态估计效果较差，且当外界噪声变化时，系统往往无法感知，造成滤波结果变差，严重时甚至导致滤波发散。

技术实现要素：

本发明的目的在于提供一种自适应无迹卡尔曼粒子滤波方法。

实现本发明目的的技术解决方案为：一种自适应无迹卡尔曼粒子滤波方法，包括以下步骤：

步骤1、对卫星/惯性紧密组合导航系统进行非线性建模，得到状态方程和量测方程；

步骤2、采用比例对称采样方法对卫星/惯性组合导航系统每个粒子进行采样，并计算每个采样点对应的权值；

步骤3、根据状态方程进行预测方程的时间更新，计算k时刻的一步预测模型，所述一步预测模型包括一步预测状态和一步预测协方差；

步骤4、计算k时刻的预测增广样本点及相应的权值，并根据预测增广样本点及相应的权值计算观测预测误差协方差和一步预测互相关协方差；

步骤5、根据新息向量计算自适应渐消因子，并做规范化处理；

步骤6、利用自适应渐消因子进行量测修正方程的更新，所述量测修正方程包括滤波增益、状态估计值和状态估计误差协方差；

步骤7、选取推荐概率密度函数，利用步骤6中得到的状态估计值生成当前时刻的粒子，并计算每个粒子对应的权值，更新粒子集合；

步骤8、采用残差二次采样法对步骤7中离散分布的粒子集合进行n次重采样，将步骤7中的非等权粒子样本更新为等权粒子样本；

步骤9、根据二次采样的粒子加权计算滤波值，对系统状态进行校正。

优选地，步骤1中所述的对系统进行非线性建模，得到状态方程和量测方程，具体为：

选取捷联惯导系统误差量和gps误差量作为卫星/惯性紧密组合导航系统的状态量，并对其进行离散化，得到系统状态方程和量测方程：

其中，x和z为系统的状态量和量测量，f(xk,k)为状态量的非线性转移函数，h(xk+1,k)为量测非线性函数，wk和vk+1为互不相关的高斯噪声，均值为零。

优选地，步骤2中所述的采用比例对称采样方法对每个粒子进行采样，具体为：

根据先验信息设定系统初始状态为服从高斯分布的随机向量，选取滤波初值为：

其中，为初始粒子，为粒子对应的状态误差协方差，初始粒子总数为n，i为粒子编号，i＝1,2,3…n；

采用比例对称采样方法对每个粒子进行采样，每个粒子得到2n+1个σ采样点，采样公式为：

其中，对于(i)为粒子数编号，j为采样点编号，k为当前时刻；为第i个粒子k时刻所对应的状态误差协方差阵；n为状态量维数，λ＝α²(n+κ)-n，α为尺度参数，κ为比例因子；

对应每个采样点的权值为：

其中，w^(m)和w^(c)为计算采样点的一步预测和一步预测误差协方差时的权值，β取值与状态量分布有关，当状态量为正态分布时，取β＝2。

优选地，步骤3中根据状态方程进行预测方程的时间更新，计算k时刻的一步预测模型，具体为：

每个粒子对应的采样点经过系统状态方程后的采样点为:

则每个粒子的一步预测状态为：

一步预测协方差为：

上式中，f()为k到k+1时刻的非线性状态转移函数，表示由k时刻递推到k+1时刻的第i个粒子的第j个采样点，为经过采样点加权和的粒子，和分别为采样点的一步预测和一步预测误差协方差时的权值，qk为系统噪声。

优选地，步骤4中计算k时刻的预测增广样本点及对应的权值，并更新观测预测误差协方差和一步预测互相关协方差，具体为：

利用比例对称采样法更新采样点和权值，并计算观测预测量：

上式中，为粒子所对应的采样点的观测预测量，为经过采样点加权后的粒子的观测预测量；

更新观测预测误差协方差：

更新一步预测互相关协方差：

上式中，和分别为采样点的一步预测和一步预测误差协方差时的权值,为第i个粒子对应的第j个采样点的量测量，为第i个粒子的观测预测量，rk+1为系统量测噪声。

优选地，步骤5中根据新息向量计算自适应渐消因子，并进行规范化处理，具体为：

根据实际观测值zk+1与预测观测值计算得到每个粒子对应的新息向量即：

当系统依据外界观测量的误差统计特性与滤波递推的误差特性一致时，有：

当系统观测异常时，在上式中加入自适应渐消因子，有：

其中，为步骤4中的观测预测误差协方差，λ为历元残差的采集个数，得到得自适应渐消因子为：

对自适应渐消因子进行规范化处理得：

其中，为的第j个主对角元素值，k为观测噪声的维数。

优选地，步骤6中更新后的量测修正方程具体为：

加入自适应矩阵后的滤波增益为：

状态估计值为：

状态估计误差协方差为：

其中，和为一步预测互相关协方差和观测预测误差协方差，为自适应渐消因子，zk+1为实际观测值，为预测观测值，为粒子的一步预测状态，rk+1为量测噪声。

优选地，步骤7中选取推荐概率密度函数，根据步骤6中得到的状态估计值生成当前时刻的粒子，并计算每个粒子对应的权值，更新粒子集合，具体为：

选取重要性密度函数为：

其中，该条件密度服从正态分布，均值为方差阵为

进行权重系数的更新：

其中，p表示条件密度，为k-1即上一时刻粒子对应的权重。

对权重系数进行归一化：

优选地，步骤8中采用残差二次采样法对步骤7中离散分布的粒子集合进行n次重采样，将步骤7中的非等权粒子样本更新为等权粒子样本，具体为：

对原始粒子进行采样，假定

[]为向零取整符号，n为粒子总数，为归一化权值，则粒子个数为

选取新的权重系数

选取剩余的nk2＝n-nk1个粒子，将nk1和nk1个粒子合并组成重采样粒子全体，总数仍为n，各粒子权值均为1/n。

本发明与现有技术相比，其显著优点为：

(1)本发明对紧密组合导航系统进行建模，采用伪距、伪距率对惯性/卫星进行组合，不引入接收机导航解算误差，精度更高，且收星数少于4颗时仍能进行组合滤波；

(2)本发明中无迹卡尔曼粒子滤波利用条件概率密度计算条件均值，可对强非线性系统进行状态估计，且精度较高；

(3)本发明利用sage滤波开窗法的理论，并结合渐消的思想，设计了多重次优渐消自适应因子，以在线实时自适应矩阵修正观测噪声，修正后的观测噪声参与增益矩阵的求解，实现对状态估计进行自适应调节的效果。

附图说明

图1是本发明自适应无迹卡尔曼粒子滤波方法的流程示意图。

图2是本发明的三维测试轨迹图。

图3是本发明的纬度误差估计对比图。

图4是本发明的经度误差估计对比图。

图5是本发明的高度误差估计对比图。

图6是本发明的东向速度误差估计对比图。

图7是本发明的北向速度误差估计对比图。

图8是本发明的天向速度误差估计对比图。

具体实施方式

下面对本发明做进一步的详细说明。

一种自适应无迹卡尔曼粒子滤波方法，根据无迹卡尔曼滤波ukf确定推荐概率密度函数，采用粒子滤波对强非线性系统进行状态估计，并对外界噪声做出实时估计，大大降低噪声异常时的导航结果。

如图1所示，一种自适应无迹卡尔曼粒子滤波方法，步骤如下：

步骤1、对惯性/卫星紧密组合导航系统进行非线性系统建模，其中选取捷联惯导系统误差量xi和gps误差量xg作为整个系统状态量，即：

其中，分别为东北天三个方向的姿态误差角，δve、δvn、δvu分别为东北天三个方向的速度误差，δl、δλ、δh分别为纬经高三个方向的位置误差，εx、εy、εz分别为载体坐标系下陀螺仪三轴常值漂移，分别为载体坐标系下加速度计三轴常值偏置，δtu为钟差的等效距离误差，δtru为钟漂的等效距离率误差。

sins系统状态方程为：

其中，fi(t)为sins系统状态转移矩阵，gi(t)为sins系统噪声驱动矩阵，wi(t)为sins系统噪声矩阵。

gps系统状态方程为：

其中，fg(t)为gps系统状态转移矩阵，gg(t)为gps系统噪声驱动矩阵，wg(t)为gps系统噪声矩阵

联立可得到系统状态方程：

系统观测方程为：

其中，sins解算的伪距、伪距率为ρi和gps解算的伪距、伪距率为ρg和观测量为二者的差值。

伪距非线性观测方程为：

其中，[xiyizi]^t为sins解算得到的载体的位置，为第i颗卫星在导航坐标系中的位置，sins解算得到的位置与真实位置之间的距离误差为[δxδyδz]^t，δlu为等效时钟误差引起的距离误差，vρ为伪距观测噪声。

伪距率非线性观测方程为：

其中，为sins解算得到的载体的速度，为第i颗卫星在导航坐标系中的速度，sins解算得到的速度与真实速度之间的速度误差为δlru为等效钟漂误差引起的距离率误差，为伪距率观测噪声。

对以上建模的系统进行离散化得：

其中，x和z为系统的状态量和量测量，f(xk,k)为状态量的非线性转移函数，h(xk+1,k)为量测非线性函数，wk和vk+1为互不相关的高斯噪声，均值为零，对应的方差矩阵为qk和rk。

步骤2、根据先验信息，设定系统初始状态为服从高斯分布的随机向量，选取滤波初值为：

其中，为初始粒子，为粒子对应的状态误差协方差，初始粒子总数为n，i为粒子编号，i＝1,2,3…n；

采用比例对称采样方法对每个粒子进行采样，每个粒子可以得到2n+1个σ采样点：

其中，对于(i)为粒子数编号，j为采样点编号，k为当前时刻；为第i个粒子k时刻所对应的状态误差协方差阵；n为状态量维数，根据步骤1中的系统模型取17，λ＝α²(n+κ)-n，α为尺度参数，一般取10^-4≤α≤1；κ为比例因子，取κ＝3-n。

对应每个采样点的权值为：

其中，w^(m)和w^(c)为计算采样点的一步预测和一步预测误差协方差时的权值，β取值与状态量分布有关，当状态量为正态分布时，取β＝2最优。

步骤3、进行预测方程的时间更新，每个粒子对应的采样点经过系统状态方程后的采样点为：

则每个粒子的状态一步预测为：

一步预测协方差为：

上式中，f()为k到k+1时刻的非线性状态转移函数，表示由k时刻递推到k+1时刻的第i个粒子的第j个采样点，为经过采样点加权和的粒子，和为步骤2中计算出的权值，qk为系统噪声。

步骤4、再次利用比例对称采样法更新采样点和权值：

其中，表示由k时刻递推k+1时刻的粒子一步预测状态的采样点，w^(m)和w^(c)为计算粒子观测预测量和观测预测误差协方差、一步预测互相关协方差时的权值，其余各参数的选择同步骤2。

计算观测预测量：

上式中，为粒子所对应的采样点的观测预测量，为经过采样点加权后的粒子的观测预测量。

计算观测预测误差协方差：

计算一步预测互相关协方差：

上式中，和为粒子所对应的权值，为第i个粒子对应的第j个采样点的量测量，为第i个粒子的观测预测量，rk+1为系统量测噪声。

步骤5、根据新息向量计算自适应渐消因子，每个粒子对应的新息向量可由实际观测值zk+1与预测观测值计算得到：

当系统依据外界观测量的误差统计特性与滤波递推的误差特性一致时，有：

当系统观测异常时，在上式中加入自适应因子，有：

其中，为步骤4中的观测预测误差协方差，λ为历元残差的采集个数，即滑动窗口宽度，一般小于50，运算后可得自适应因子为：

自适应因子的对角线元素可能会因为测量次数和计算误差等因素的影响不再大于等于1，需要对做规范化处理：

其中，为的第j个主对角元素值，k为观测噪声的维数。

步骤6、加入自适应矩阵后的滤波增益为：

计算状态估计值：

状态估计误差协方差：

其中，和为步骤4中的一步预测互相关协方差和观测预测误差协方差，为步骤5中的自适应因子，zk+1为实际观测值，为预测观测值，为粒子的一步预测状态，rk+1为量测噪声。

步骤7、利用步骤6中得到的状态估计值生成当前时刻的粒子，选取重要性密度函数为：

其中，该条件密度服从正态分布，均值为方差阵为

进行权重系数的更新：

其中，p表示条件密度，为k-1即上一时刻粒子对应的权重。

对权重系数进行归一化:

步骤8、为避免粒子退化，采用残差二次采样法从离散分布的中进行n次重采样，将之前的非等权粒子样本更新为等权粒子样本。

首先对原始粒子进行次数的重复采样，假定：

[]为向零取整符号，n为粒子总数，为归一化权值，粒子个数为其次选取新的权重系数为：

选取剩余的nk2＝n-nk1个粒子。将nk1和nk1个粒子合并组成重采样粒子全体，总数仍为n，所有粒子构成等权样本，权值均为1/n。

步骤9、根据二次采样的粒子加权计算滤波值：

本发明可对强非线性系统进行估计，当系统发散摄动时，可以通过调整滤波增益进一步的调整粒子滤波所需的推荐概率密度函数，减小其对系统的精度和稳定性的干扰。

为进一步验证该导航方法的可行性与效果，利用数字仿真将本发明所述的自适应无迹卡尔曼粒子滤波方法和普通的无迹卡尔曼滤波相比较。如图2所示，利用轨迹发生器生成一条载体运动的实际轨迹，同时生成卫星的实际位置。仿真中设置有效的卫星为4颗，gps刷新率为1hz，伪距观测噪声为10m，伪距率观测噪声为0.1m/s，惯组刷新率为200hz，陀螺仪的零偏和零偏稳定性均为1°/h，加速度计的零偏和零偏稳定性为0.5mg和0.15mg，仿真时间为230s。在载体运动的某一时间段(50s-70s)外界量测噪声增大为正常情况的10倍。

如图3、图4、图5所示，本发明所述的自适应无迹卡尔曼粒子滤波方法较于普通无迹卡尔曼滤波位置误差更小，曲线波动较小。如图6、图7、图8所示，本发明所述的自适应无迹卡尔曼粒子滤波方法较于普通无迹卡尔曼滤波速度误差更小，误差曲线更平稳。整体来看，当外界噪声出现异常时，自适应upf方法的速度、位置误差与噪声正常时相差不大，且收敛速度更快，具有较强的抗干扰能力。

综上所述，本发明对紧密组合导航系统进行建模，采用伪距、伪距率对惯性/卫星导航进行组合。利用无迹卡尔曼滤波的条件概率密度计算条件均值，可对强非线性系统进行状态估计。同时，利用sage滤波开窗法的理论，结合渐消的思想，设计了多重次优渐消自适应因子，以在线实时自适应矩阵修正观测噪声，修正后的观测噪声参与增益矩阵的求解，实现对状态估计进行自适应调节的效果。可大大提高噪声异常时的导航精度，从而提升导航系统的鲁棒性和稳定性。