一种基于边缘化CKF重力辅助导航的方法

摘要

本发明公开了一种基于边缘化CKF重力辅助导航的方法，包括：实时获取惯性导航系统输出的位置信息及重力仪测得的重力异常值；建立惯性器件输出误差模型及惯导系统误差方程，确定状态量和量测量；将迭代过程分为状态更新和量测更新；滤波过程中各个时刻的观测量为重力仪实时测量的重力异常值，估计观测预测值、自相关协方差阵、互相关协方差阵及卡尔曼增益；根据得到的状态和观测预测值，计算状态估计值和状态误差协方差估计值；根据得到的惯导系统的导航参数误差，通过输出校正，对惯导系统进行修正。本发明有效地快速地对实现状态估计，借助重力异常基准图以采样点形式进行滤波，避免模型不准确带来的误差，可以精确地、快速地实现重力辅助导航。

主权项

1.一种基于边缘化CKF重力辅助导航的方法，其特征在于，该基于边缘化CKF重力辅助导航的方法包括以下步骤：步骤一，实时获取惯性导航系统输出位置信息的经度和纬度及重力仪测得的重力异常值，并保证惯导系统指示位置信息和重力异常信息在时间上同步；步骤二，建立惯性器件输出误差模型及惯导系统误差方程，确定状态量和量测量，量测量为重力异常观测值；取经纬度误差、速度误差、平台姿态角误差、加速度计误差及陀螺误差为状态量，即：从而组合导航系统的状态方程可表示如下：$\stackrel{·}{x}\left(t\right)=\mathrm{Fx}\left(t\right)+\mathrm{Gw}\left(t\right)$其中，G为12×12维的单位阵I_12×12，w(t)为系统噪声，均为零均值高斯白噪声，w(t)＝[0 0 w_ax w_ay w_gx w_gy w_gz 0 0 0 0 0]^T，F为系统状态转移矩阵，表达式如下：$A=\left[\begin{array}{ccc}{F}_{2\times 4}& 0_{2\times 3}& 0_{2\times 5}\\ F_{5\times 4}& F_{5\times 3}& I_{5\times 5}\\ 0_{5\times 4}& 0_{5\times 3}& 0_{5\times 5}\end{array}\right]$其中，0_5×4表示全零矩阵，F_2×4，F_5×4，F_5×3分别表示如下：直接将重力异常作为该组合导航系统的观测量，为组合导航系统提供校正信息，重力异常观测值与载体所在位置组合导航系统观测模型可表示如下：其中，z为重力异常观测值，为载体真实位置处的位置坐标，为真实位置处的重力异常值，ν为观测噪声；步骤三，将迭代过程分为状态更新和量测更新，其中状态线性部分进行更新时，一步状态预测和协方差阵更新与标准卡尔曼方程一致，而卡尔曼增益通过Cubature点集进行更新，从而得到线性状态预测值；步骤四，量测的非线性部分更新时，通过Cholesky分解协方差阵计算Cubature点；步骤五，载体真实位置可通过惯导系统的指示位置以及状态更新阶段的位置误差预测值近似，用重力异常基准图代替滤波过程中组合导航系统的观测方程以采样点形式来传播Cubature点；步骤六，滤波过程中各个时刻的观测量为重力仪实时测量的重力异常值，估计观测预测值、自相关协方差阵、互相关协方差阵及卡尔曼增益；步骤七，根据得到的状态和观测预测值，计算状态估计值和状态误差协方差估计值；步骤八，根据得到的惯导系统的导航参数误差，通过输出校正，对惯导系统进行修正。