[发明专利]一种卡尔曼滤波结合图优化的组合导航后处理方法及系统

权利要求书

1.一种卡尔曼滤波结合图优化的组合导航后处理方法，其特征在于，包括以下步骤：

获取惯性导航系统输出的比力和角速度，利用惯性导航积分算法对比力和角速度进行积分，得到位置、速度和姿态航向角；

对位置、速度和姿态航向角进行前向卡尔曼滤波，根据卡尔曼滤波过程中的数值或矩阵构建方程组；

利用图优化方法求解方程组，获得惯性导航系统所有时刻的状态误差；

利用图优化方法计算得到的该状态误差对所述惯性导航系统进行导航解算，输出导航数据。

2.根据权利要求1所述卡尔曼滤波结合图优化的组合导航后处理方法，其特征在于，所述方程组为：

其中，P₀为先验方差，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时刻的采样个数。

6.一种卡尔曼滤波结合图优化的组合导航后处理系统，其特征在于，包括：

卡尔曼滤波单元：用于获取惯性导航系统输出的比力和角速度，利用惯性导航积分算法对比力和角速度进行积分，得到位置、速度和姿态航向角；对位置、速度和姿态航向角进行前向卡尔曼滤波，根据卡尔曼滤波过程中的数值或矩阵构建方程组；

图优化单元：利用图优化方法求解方程组，获得惯性导航系统所有时刻的状态误差；利用图优化方法和该状态误差对所述惯性导航系统进行导航解算，输出导航数据。

7.根据权利要求6所述卡尔曼滤波结合图优化的组合导航后处理系统，其特征在于，所述方程组为：

其中，P₀为先验方差，Q_k为k时刻过程噪声协方差，R_kk时刻为量测协方差，A_k为k时刻卡尔曼滤波用的系统矩阵，H_k为k时刻卡尔曼滤波用的观测矩阵，x_k为k时刻的误差状态，为k-1时刻卡尔曼滤波计算得到的误差状态，z_k为k时刻的观测量。

8.根据权利要求7所述卡尔曼滤波结合图优化的组合导航后处理系统，其特征在于，所述卡尔曼滤波单元还用于：

记录每次卡尔曼滤波更新时的导航状态，导航状态包括位置、速度和姿态航向角：

其中，为第k次更新时的位置，为第k次更新时的速度，为第k次更新时的姿态航向角，NAV_k为导航状态。