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中所述的采用比例对称采样方法对每个粒子进行采样,具体为:
根据先验信息设定系统初始状态为服从高斯分布的随机向量,选取滤波初值为:
其中,
采用比例对称采样方法对每个粒子进行采样,每个粒子得到2n+1个σ采样点,采样公式为:
其中,对于
对应每个采样点的权值为:
其中,w(m)和w(c)为计算采样点的一步预测和一步预测误差协方差时的权值,β=2。
4.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤3中根据状态方程进行预测方程的时间更新,计算k时刻的一步预测模型,具体为:
每个粒子对应的采样点经过系统状态方程后的采样点为:
则每个粒子的一步预测状态为:
一步预测协方差为:
上式中,f()为k到k+1时刻的非线性状态转移函数,
5.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤4中计算k时刻的预测增广样本点及对应的权值,并更新观测预测误差协方差和一步预测互相关协方差,具体为:
利用比例对称采样法更新采样点和权值,并计算观测预测量
上式中,
更新观测预测误差协方差:
更新一步预测互相关协方差:
上式中,
6.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤5中根据新息向量计算自适应渐消因子,并进行规范化处理,具体为:
根据实际观测值zk+1与预测观测值
当系统依据外界观测量的误差统计特性与滤波递推的误差特性一致时,有:
当系统观测异常时,在上式中加入自适应渐消因子,有:
其中,
对自适应渐消因子进行规范化处理得:
其中,
7.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤6中更新后的量测修正方程具体为:
加入自适应矩阵后的滤波增益为:
状态估计值为:
状态估计误差协方差为:
其中,
8.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤7中选取推荐概率密度函数,根据步骤6中得到的状态估计值生成当前时刻的粒子,并计算每个粒子对应的权值,更新粒子集合,具体为:
选取重要性密度函数为:
其中,该条件密度服从正态分布,均值为
进行权重系数的更新:
其中,p表示条件密度,
对权重系数进行归一化:
9.根据权利要求1中所述的自适应无迹卡尔曼粒子滤波方法,其特征在于,步骤8中采用残差二次采样法对步骤7中离散分布的粒子集合进行n次重采样,将步骤7中的非等权粒子样本更新为等权粒子样本,具体为:
对原始粒子进行
[]为向零取整符号,n为粒子总数,
选取新的权重系数
选取剩余的nk2=n-nk1个粒子,将nk1和nk1个粒子合并组成重采样粒子全体,总数仍为n,各粒子权值均为1/n。