一种卡尔曼滤波结合图优化的组合导航后处理方法及系统技术方案

一种卡尔曼滤波结合图优化的组合导航后处理方法及系统。本发明专利技术提供了组合导航后处理方法及系统，方法包括以下步骤：获取惯性导航系统输出的比力和角速度，利用惯性导航积分算法对比力和角速度进行积分，得到位置、速度和姿态航向角；对位置、速度和姿态航向角进行前向卡尔曼滤波，根据卡尔曼滤波过程中的数值或矩阵构建方程组；利用图优化方法求解方程组，获得惯性导航系统所有时刻状态误差；利用图优化方法计算得到的该状态误差对所述惯性导航系统进行导航解算，输出导航数据。该方法既能保障导航精度，又能保障处理速度。又能保障处理速度。又能保障处理速度。

【技术实现步骤摘要】
一种卡尔曼滤波结合图优化的组合导航后处理方法及系统


[0001]本专利技术属于导航
，具体涉及一种卡尔曼滤波结合图优化的组合导航后处理方法及系统。

技术介绍

[0002]现有的组合导航后处理方法大多数是采用正反向卡尔曼滤波，这种方法容易实现，但是工程应用有其局限性。图1为正向卡尔曼滤波的方法。
[0003]卡尔曼滤波由于采用隐式马尔科夫模型，每次迭代必须进行边缘概率密度求解，这样的话既会丢掉很多历史测量信息，又不能用后续的信息对历史的测量进行反向信息传播。因此传统的方法多采用反向卡尔曼滤波加逆向导航解算的方式来提高精度，参见图2，如果不采用这种方式的话，很难把后续的测量信息传递到前方的测量点。但是，由于卡尔曼滤波采用的是不停的求边缘概率密度的方式，由于模型的马尔科夫性质，即假设每个时刻的观测是独立的，如图1所示，Y1的观测独立于Y3，即Y2的观测到来时，就会求第2时刻X2的边缘概率密度，这样一来第3时刻的Y3无法影响X0,X1,X2,即使采用了反向滤波相邻较远的观测也很难相互影响。卡尔曼滤波只是一步线性无偏最优估计，没有反向的消息传播。
[0004]图优化技术是一门新兴的最优估计技术，采用图模型的方式进行建模，解大规模稀疏状态更新方程的方式进行计算，可以处理大规模的传感器数据。图优化进行问题处理的时候，需要重新建模，将状态空间模型转换为图模型。与卡尔曼滤波相反，图优化处理问题的时候，最希望做的是全局优化，不希望用求边缘概率密度求解的方式来去掉以前的观测量。因此，采用图优化做后处理解算，计算时间会非常长，因为图的规模一直在增长。对组合导航后处理来说，如果每个时刻都用图优化计算所有时刻的概率分布，再用最新时刻的概率分布得到反馈量，对系统进行反馈，计算时间会非常长，软件体验会非常不好。而且现有技术中也缺乏用图优化来针对惯性导航系统与其他传感器组合获得更高精度的具体方案。现有技术多是采用惯性导航系统预积分的方案，用惯导的短时精度来对视觉或者激光相邻帧的信息进行平滑处理。现有技术没有用图优化做高精度组合导航后处理提高精度的具体方案。现有技术关于图优化组合导航的方案都是采用状态估计模式，没有考虑状态反馈模式，即估计都是用因子图优化直接估计导航系统的速度位置姿态等状态，而不是估计速度位置姿态的误差。
[0005]因此，现有技术的卡尔曼滤波和图优化组合导航后处理方案，都有其局限性，一个精度不够，一个计算量过大，且没有考虑误差估计方案。

技术实现思路

[0006]针对现有技术中的缺陷，本专利技术提供一种卡尔曼滤波结合图优化的组合导航后处理方法及系统，既能保障导航精度，又能保障处理速度。
[0007]第一方面，一种卡尔曼滤波结合图优化的组合导航后处理方法，包括以下步骤：
[0008]获取惯性导航系统输出的比力和角速度，利用惯性导航积分算法对比力和角速度
进行积分，得到位置、速度和姿态航向角；
[0009]对位置、速度和姿态航向角进行前向卡尔曼滤波，根据卡尔曼滤波过程中的数值或矩阵构建方程组；
[0010]利用图优化方法求解方程组，获得惯性导航系统所有时刻的状态误差；
[0011]利用图优化方法计算得到的状态误差对所述惯性导航系统进行导航解算，输出导航数据。
[0012]优选地，所述方程组为：
[0013][0014]其中，P0为先验方差，Q
k
为k时刻过程噪声协方差，R
k
为k时刻量测协方差，A
k
为k时刻卡尔曼滤波用的系统矩阵，H
k
为k时刻卡尔曼滤波用的观测矩阵，x
k
为k时刻的误差状态，为k
‑
1时刻计算卡尔曼滤波得到的误差状态，z
k
为k时刻的观测量。
[0015]优选地，在所述构建方程组之后，还包括：
[0016]记录每次卡尔曼滤波更新时的导航状态，导航状态包括位置、速度和姿态航向角：
[0017][0018]其中，为第k次更新时的位置，为第k次更新时的速度，为第k次更新时的姿态航向角，NAV
k
为导航状态。
[0019]优选地，所述利用图优化方法和该状态误差对所述惯性导航系统进行导航解算，输出导航数据具体包括：
[0020]利用图优化方法和该状态误差对所述惯性导航系统进行导航解算，根据导航解算结果建立下一次导航解算的反馈量；
[0021]利用上一次导航解算后得到的反馈量对所述惯性导航系统进行导航解算，输出导航数据。
[0022]优选地，所述反馈量为其中，X
gk
为利用所述图优化方法求解得到的卡尔曼滤波积分到k时刻的状态NAV
k
误差，为卡尔曼滤波积分第一次积分到k时刻的导航解算值，N
k
为从k
‑
1时刻到k时刻的采样个数。
[0023]第二方面，一种卡尔曼滤波结合图优化的组合导航后处理系统，包括：
[0024]卡尔曼滤波单元：用于获取惯性导航系统输出的比力和角速度，利用惯性导航积分算法对比力和角速度进行积分，得到位置、速度和姿态航向角；对位置、速度和姿态航向角进行前向卡尔曼滤波，根据卡尔曼滤波过程中的数值或矩阵构建方程组；
[0025]图优化单元：利用图优化方法求解方程组，获得惯性导航系统所有时刻的状态误差；利用图优化方法计算得到的状态误差对所述惯性导航系统进行导航解算，输出导航数据。
[0026]优选地，所述方程组为：
[0027][0028]其中，P0为先验方差，Q
k
为k时刻过程噪声协方差，R
k
k时刻为量测协方差，A
k
为k时刻卡尔曼滤波用的系统矩阵，H
k
为k时刻卡尔曼滤波用的观测矩阵，x
k
为k时刻的误差状态，为k
‑
1时刻卡尔曼滤波计算得到的误差状态，z
k
为k时刻的观测量。
[0029]优选地，所述卡尔曼滤波单元还用于：
[0030]记录每次卡尔曼滤波更新时的导航状态，导航状态包括位置、速度和姿态航向角：
[0031][0032]其中，为第k次更新时的位置，为第k次更新时的速度，为第k次更新时的姿态航向角，NAV
k
为导航状态。
[0033]优选地，所述图优化单元具体用于：
[0034]利用图优化方法计算得到的状态误差对所述惯性导航系统进行导航解算，根据导航解算结果建立下一次导航解算的反馈量；
[0035]利用上一次导航解算后得到的反馈量对所述惯性导航系统进行导航解算，输出导航数据。
[0036]优选地，所述反馈量为其中，X
gk
为利用所述图优化方法求解得到的卡尔曼滤波积分到k时刻的状态NAV
k
误差，为卡尔曼滤波积分第一次积分到k时刻的导航解算值，N
k
为从k
‑
1本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种卡尔曼滤波结合图优化的组合导航后处理方法，其特征在于，包括以下步骤：获取惯性导航系统输出的比力和角速度，利用惯性导航积分算法对比力和角速度进行积分，得到位置、速度和姿态航向角；对位置、速度和姿态航向角进行前向卡尔曼滤波，根据卡尔曼滤波过程中的数值或矩阵构建方程组；利用图优化方法求解方程组，获得惯性导航系统所有时刻的状态误差；利用图优化方法计算得到的该状态误差对所述惯性导航系统进行导航解算，输出导航数据。2.根据权利要求1所述卡尔曼滤波结合图优化的组合导航后处理方法，其特征在于，所述方程组为：其中，P0为先验方差，Q
k
为k时刻过程噪声协方差，R
k
为量测协方差，A
k
为k时刻卡尔曼滤波用的系统矩阵，H
k
为k时刻卡尔曼滤波用的观测矩阵，x
k
为k时刻的误差状态，为k
‑
1时刻计算卡尔曼滤波得到的误差状态，z
k
为k时刻的观测量。3.根据权利要求2所述卡尔曼滤波结合图优化的组合导航后处理方法，其特征在于，在所述构建方程组之后，还包括：记录每次卡尔曼滤波更新时的导航状态，导航状态包括位置、速度和姿态航向角：其中，为第k次更新时的位置，为第k次更新时的速度，为第k次更新时的姿态航向角，NAV
k
为导航状态。4.根据权利要求3所述卡尔曼滤波结合图优化的组合导航后处理方法，其特征在于，所述利用图优化方法和该状态误差对所述惯性导航系统进行导航解算，输出导航数据具体包括：利用图优化方法和该状态误差对所述惯性导航系统进行导航解算，根据导航解算结果建立下一次导航解算的反馈量；利用上一次导航解算后得到的反馈量对所述惯性导航系统进行导航解算，输出导航数据。5.根据权利要求4所述卡尔曼滤波结合图优化的组合导航后处理方法，其特征在于，所述反馈量为其中，X
gk
为利用所述图优化
方法求解得到的卡尔曼滤波积分到k时刻的状态NAV
k
误差，为卡尔曼滤波积分第一次积分到k时刻的导航解算值，N
k
为从k
‑
1时刻到k时刻的...

【专利技术属性】
技术研发人员：吕少麟，葛一弘，
申请(专利权)人：图优化北京科技有限公司，
类型：发明
国别省市：