[发明专利]基于李群非线性状态误差的惯性基组合导航滤波方法

权利要求书

1.一种基于李群非线性状态误差的惯性基组合导航滤波方法，其特征在于，包括：

惯性/卫星组合导航系统中的卫星接收机和捷联惯导分别接收各自的数据信息；

根据卫星接收机和捷联惯导接收的数据信息，建立惯性/卫星初始对准模型，完成基于李群非线性状态误差的惯性基组合导航滤波方法的初始对准；

建立惯性/卫星组合导航模型，完成基于李群非线性状态误差的惯性基组合导航滤波方法的组合导航，将组合导航的姿态、速度、位置信息输出；

其中惯性/卫星组合导航模型与惯性/卫星初始对准模型相同，均包括基于李群非线性状态误差的惯性/卫星组合导航系统模型和惯性/卫星组合导航系统观测模型，基于李群非线性状态误差的惯性/卫星组合导航系统模型的建立方法为：

(1)惯性导航状态方程建立；

姿态表达采用即载体系到地心地固坐标系ECEF的方向余弦矩阵，速度表达和位置的表达均采用载体相对于惯性系ECI的速度和位置在ECEF坐标系的投影；

方向余弦矩阵对时间的导数为

其中，为载体相对于ECI系的角速度在ECI系下的投影，为地球自转角速度在ECEF系下的投影；

载体相对ECI坐标系的速度在ECI坐标系的投影对时间的导数为：

其中，为在ECI系下投影的引力加速度，其为ECI坐标系中笛卡尔位置的函数，f^b为比力值，为载体系到ECI坐标系的方向余弦矩阵；

将公式(2)定义的速度在ECEF系下投影，得到：

其中，公式(3)的推导中用到了为ECEF坐标系到ECI坐标系的方向余弦矩阵；为引力加速度在ECEF坐标系下的投影；为载体相对ECI坐标系的速度在ECEF坐标系下的投影；

ECEF系下定义的载体位置随时间的导数为：

其中，为ECI坐标系到ECEF坐标系的方向余弦矩阵，为载体相对ECI坐标系的位置在ECEF坐标系的投影，为载体相对ECI坐标系的位置在ECI坐标系的投影；

(2)状态误差定义及其微分方程；

定义李群下的与姿态、速度、位置对应的状态误差如下：

其中，为ECEF坐标系到载体系的方向余弦矩阵的估计值，exp(·)代表矩阵的指数，φ^e为姿态误差；为载体相对ECI坐标系的速度在载体系下投影的估计值，为载体相对ECI坐标系的速度在ECEF坐标系下投影的估计值；为载体相对ECI坐标系的位置在载体系下投影的估计值，为载体相对ECI坐标系的位置在ECEF坐标系下投影的估计值；I为3乘3的单位矩阵；

设带有测量噪声的三轴陀螺仪测量的角速率和三轴加速度计测量的比力为：

其中，为三轴陀螺仪测量误差，δf^b为三轴加速度计测量误差，ε^b为三轴陀螺仪零偏，为三轴加速度计零偏，w_g＝[w_gx w_gy w_gz]^T和w_a＝[w_ax w_ay w_az]^T均为白噪声过程；

利用一阶近似得到李群下的非线性姿态误差φ^e的微分方程为：

李群下的非线性速度误差的微分方程为:

其中，μ为引力常数；

李群下的非线性位置误差的微分方程为：

(3)构建基于李群非线性状态误差的惯性/卫星组合导航系统模型；

三轴陀螺仪测量误差和三轴加速度计测量误差可以建模为随机游走过程:

根据公式(7)-(10)，得到基于李群非线性状态误差的惯性/卫星组合导航系统模型为：

其各变量的定义如下：

w＝[w_gx w_gy w_gz w_ax w_ay w_az]^T (13)

构建基于李群非线性状态误差的惯性/卫星组合导航系统观测模型方法为：

将惯导与卫星的速度、位置求差作为观测，需要用到如下的ECEF和ECI坐标系之间状态转换公式：

其中，为载体相对ECEF坐标系的位置在ECEF坐标系下投影的估计值，为载体相对ECI坐标系的位置在ECI坐标系下投影的估计值，为载体相对ECEF坐标系的位置在ECI坐标系下投影的估计值；和分别为载体相对ECI系的线性位置误差估计值和载体相对ECEF系的线性位置误差估计值在ECEF系的投影，二者相同，因为ECI和ECEF的坐标原点相同；为载体相对ECI坐标系的速度在ECI坐标系下投影的估计值，为载体相对ECEF坐标系的速度在ECI坐标系下投影的估计值；为地球自转角速度在ECI坐标系下的投影；为载体相对ECEF坐标系的速度在ECEF坐标系下投影的估计值；和分别为载体相对ECI系的线性速度误差估计值和载体相对ECEF系的线性速度误差估计值在ECEF系的投影；

基于李群非线性状态误差的惯性/卫星松组合观测模型为：

δz＝Hx_{LGEKF-INS-ECEF}+υ (17)

其中：

δz_v和δz_r分别为速度观测误差向量和位置观测误差向量，υ_v,3×1和υ_r,3×1分别为速度和位置观测白噪声；

公式(18)中的H矩阵是根据公式(19)得到：