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

文档序号:19150431发布日期:2019-11-16 00:01阅读:来源:国知局

技术特征:

1.一种自适应无迹卡尔曼粒子滤波方法,其特征在于,具体步骤为:

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

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

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

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

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

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

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

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

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

2.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤1中所述的对系统进行非线性建模,得到状态方程和量测方程,具体为:

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

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

3.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤2中所述的采用比例对称采样方法对每个粒子进行采样,具体为:

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

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

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

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

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

其中,w(m)和w(c)为计算采样点的一步预测和一步预测误差协方差时的权值,β=2。

4.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤3中根据状态方程进行预测方程的时间更新,计算k时刻的一步预测模型,具体为:

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

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

一步预测协方差为:

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

5.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤4中计算k时刻的预测增广样本点及对应的权值,并更新观测预测误差协方差和一步预测互相关协方差,具体为:

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

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

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

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

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

6.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤5中根据新息向量计算自适应渐消因子,并进行规范化处理,具体为:

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

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

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

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

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

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

7.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤6中更新后的量测修正方程具体为:

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

状态估计值为:

状态估计误差协方差为:

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

8.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤7中选取推荐概率密度函数,根据步骤6中得到的状态估计值生成当前时刻的粒子,并计算每个粒子对应的权值,更新粒子集合,具体为:

选取重要性密度函数为:

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

进行权重系数的更新:

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

对权重系数进行归一化:

9.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤8中采用残差二次采样法对步骤7中离散分布的粒子集合进行n次重采样,将步骤7中的非等权粒子样本更新为等权粒子样本,具体为:

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

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

选取新的权重系数

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


技术总结
本发明公开了一种自适应无迹卡尔曼粒子滤波方法,利用Sage滤波开窗法的理论,同时结合渐消的思想,通过采集历元新息向量估计观测量的真实协方差矩阵,与滤波递推模型的协方差阵进行比较,当两种协方差存在偏差时,则将根据差值自适应地调节系统的观测协方差阵。根据此设计自适应渐消因子,进而修正观测噪声,修正后的观测噪声参与增益矩阵的求解,实现对状态估计进行自适应调节。本发明可以有效地对强非线性卫星/惯性组合导航系统进行滤波校正,特别地当外界噪声异常时,可以有效地自适应调节滤波增益以提高系统的鲁棒性和定位精度。

技术研发人员:陈帅;温哲君;刘善武;谭聚豪;王琛;顾得友
受保护的技术使用者:南京理工大学
技术研发日:2019.07.24
技术公布日:2019.11.15
当前第2页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1