CN102645223B - 一种基于比力观测的捷联惯导真空滤波修正方法 - Google Patents

Abstract

一种基于比力观测的捷联惯导真空滤波修正方法，它有八大步骤：一、确定载体的初始位置参数；二、采集捷联惯导系统的输出数据；三、对采集到的输出数据进行处理，完成惯导系统的粗对准；四、粗对准结束后进入精对准阶段；五、载体从水平位置绕俯仰轴在水平面的投影旋转至竖直位置，转动时间、角度视情况而定；六、保持载体在第二个竖直位置上静止不动，作零速修正和航向修正；七、两位置对准结束后立即进入飞行过程，当导航解算高度大于85km，利用加速度计信息进行卡尔曼滤波的真空修正；八、惯组进入中段后期，当导航解算高度低于85km，视载体已出真空段，这时无真空滤波修正，但可延续前期结果继续对器件误差进行修正。它在惯导领域里有实用价值。

Description

一种基于比力观测的捷联惯导真空滤波修正方法

(一)技术领域：

本发明涉及一种基于比力观测的捷联惯导真空滤波修正方法，属于惯性导航技术领域。

(二)背景技术：

捷联惯导系统的静基座对准是保证系统工作精度的前提。静基座对准通常可以采用解析式对准方法、罗经回路对准方法、方位估算方法、两位置卡尔曼滤波对准法等。两位置卡尔曼滤波对准方法虽然能得到水平方向加速度计的零位误差，但天向加速度计的零位误差和标度误差难以分离精确。如果出现零位误差和标度误差极性相反的情况，有可能引入更大的误差。

针对上述问题，并且为了进一步提高惯性导航的精度，可利用载体在真空段飞行过程中的特殊环境：通常无推力，同时由于空气密度非常小，空气动力也可以忽略。此时载体仅受地球重力作用，如果惯导安装在载体的质心，加速度表敏感的信息即比力应为零。因此，可以利用载体真空段飞行时加速度表的输出信息修正惯组误差系数，这种方法一般称为真空修正技术。通常采用的真空修正方法直接利用加速度表的输出信息对加速度计零位误差和标度误差进行修正，从而达到提高惯性导航精度的目的。

杨涛，王明海，曹锐在《弹道导弹加表逐次通电误差分析和修正》一文中提出了利用真空修正技术对加速度计零位误差和标度误差进行分离和补偿的技术。该文在分析加速度计误差传播模型基础上，利用导弹在真空段飞行过程中的特点，对加速度计零位误差和标度误差进行分离和补偿。由于加速度计误差在很大程度上决定了导弹的落点误差，对加速度计标定系数进行误差分离和补偿可以有效提高导航精度。此方案通过仿真计算表明，这种方法可以有效提高导弹的落点精度。(战术导弹技术，2010(6)：72～74)但该方法只对加速度计零位误差和标度误差进行分离和补偿，无法同时对系统姿态误差进行校正，精度提高有限。

在实际工程应用中，如果能够通过建立合理的观测模型，应用真空段加速度的输出信息进一步修正系统的姿态等误差，对于提高捷联惯导的实际导航性能将具有非常重要的应用价值。

(三)发明内容：

1、目的：本发明的目的在于提供一种基于比力观测的捷联惯导真空滤波修正方法，它充分利用真空段加速度表的信息，在分离器件误差的基础上，实现捷联惯导姿态误差的滤波修正，提高捷联惯导系统的实际导航性能。

2、技术方案：本发明提供一种基于比力观测的捷联惯导真空滤波修正方法，该方法具体步骤如下：

步骤1、通过外部设备(例如GPS接收机)可确定载体的初始位置参数(包括初始的经度、纬度)并将其装订至导航计算机。

步骤2、捷联惯导系统进行预热，然后采集陀螺仪和加速度计的输出数据。

步骤3、对采集到的陀螺仪和加速度计的输出数据进行处理，根据捷联惯导系统的误差传播特性和古典控制理论，采用二阶调平法和方位估算法来完成系统的粗对准，初步确定载体的姿态。粗对准时间不少于30秒。

步骤4、粗对准结束后进入精对准阶段。保持载体在水平位置上静止不动，作零速修正，对准时间不少于250秒。此时惯组初始俯仰角和滚转角分别为0度和-45度，航向角为45度(实例)。对准过程包括建立精对准过程零速修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计。

步骤5、载体从水平位置绕俯仰轴在水平面的投影旋转至竖直位置，转动时间、角度根据具体情况确定。这里绕俯仰轴在水平面的投影旋转角度设为90度，转速为1度/秒(实例)。

步骤6、保持载体在第二个竖直位置上静止不动，作零速修正和航向修正，对准时间不少于250秒。对准过程中包括建立精对准过程零速修正和航向修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计。

步骤7、两位置对准结束后立即进入飞行过程。飞行轨迹经历主动段大过载、中段前期，期间当导航解算高度大于85km时，视为载体已进入真空段，这时可利用观测到的加速度计信息进行基于卡尔曼滤波的真空修正。真空修正过程包括建立真空修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计。

步骤8、惯组进入中段后期，当导航解算高度低于85km时，视为载体已出真空段，这时无真空滤波修正，但可延续前期基于真空修正的结果继续对器件误差进行修正。

其中，步骤4中所述的“对准过程包括建立精对准过程零速修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计”，其具体实现过程如下：

1)导航坐标系取为游动自由方位坐标系，建立步骤4中用到的系统状态方程和量测方程如下：

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}{\mathrm{\δ\θ}}_x\\ {\mathrm{\δ\θ}}_y\\ \mathrm{\δh}\\ {\mathrm{\δv}}_x\\ {\mathrm{\δv}}_y\\ {\mathrm{\δv}}_z\\ {\mathrm{\ψ}}_x\\ {\mathrm{\ψ}}_y\\ {\mathrm{\ψ}}_z\end{array}\right]=\left[\begin{array}{ccccccccc}-\frac{v_z}{R}& 0& \frac{v_y}{R^2}& 0& -\frac{1}{R}& 0& 0& 0& 0\\ 0& -\frac{v_z}{R}& -\frac{v_x}{R^2}& \frac{1}{R}& 0& 0& 0& 0& 0\\ -v_y& v_x& 0& 0& 0& 1& 0& 0& 0\\ 0& -g& 0& 0& 2{\mathrm{\Ω}}_z& -{(\mathrm{\ρ}+2\mathrm{\Ω})}_y& 0& -f_z& f_y\\ g& 0& 0& -2{\mathrm{\Ω}}_z& 0& {(\mathrm{\ρ}+2\mathrm{\Ω})}_x& f_z& 0& -f_x\\ 0& 0& 2\frac{g}{R}& {(\mathrm{\ω}+\mathrm{\Ω})}_y& -{(\mathrm{\ω}+\mathrm{\Ω})}_x& 0& -f_y& f_x& 0\\ 0& 0& 0& 0& 0& 0& 0& {\mathrm{\Ω}}_z& -{\mathrm{\ω}}_y\\ 0& 0& 0& 0& 0& 0& -{\mathrm{\Ω}}_z& 0& {\mathrm{\ω}}_x\\ 0& 0& 0& 0& 0& 0& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ\θ}}_x\\ {\mathrm{\δ\θ}}_y\\ \mathrm{\δh}\\ {\mathrm{\δv}}_x\\ {\mathrm{\δv}}_y\\ {\mathrm{\δv}}_z\\ {\mathrm{\ψ}}_x\\ {\mathrm{\ψ}}_y\\ {\mathrm{\ψ}}_z\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ {\mathrm{\δf}}_x\\ {\mathrm{\δf}}_y\\ \mathrm{\δ}{f}_z\\ 0\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\\ -{\mathrm{\ϵ}}_x\\ -{\mathrm{\ϵ}}_y\\ -{\mathrm{\ϵ}}_z\end{array}\right]$

式中，δθ——角位置误差矢量；δh——高度误差；δv——速度误差矢量；ψ——ψ角姿态误差；v——载体运动速度矢量；ρ——载体运动角速率矢量；Ω——地球自转角速率矢量；ω——ρ+Ω；g——地球重力加速度；R——地球半径；f——载体感受的比力矢量；δf——加速度计输出误差；ε——陀螺输出误差。

滤波器中陀螺仪的误差模型为：

${\left[\begin{array}{c}{\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]}^n=C_b^n{\left[\begin{array}{c}{\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]}^b=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\left(\begin{array}{c}\left[\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right]\end{array}+\left[\begin{array}{ccc}{S}_{\mathrm{gx}}& S_{\mathrm{gxy}}& S_{\mathrm{gxz}}\\ S_{\mathrm{gyx}}& S_{\mathrm{gy}}& S_{\mathrm{gyz}}\\ S_{\mathrm{gzx}}& S_{\mathrm{gzy}}& S_{\mathrm{gz}}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib}}^x\\ {\mathrm{\ω}}_{\mathrm{ib}}^y\\ {\mathrm{\ω}}_{\mathrm{ib}}^z\end{array}\right]\right)$

式中，B_x、B_y、B_z为陀螺的常值漂移误差项；S_gx、S_gy、S_gz为陀螺的标度误差项；

S_gxy、S_gxz、S_gyx、S_gyz、S_gzx、S_gzy为陀螺的安装误差；

ω_ib为陀螺仪敏感的相对惯性空间的角速度矢量。

为捷联惯导的位置矩阵

$C_b^n=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ \sin \mathrm{\θ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}\end{array}\right]$

上式中γ、θ、ψ分别表示载体的横滚角，俯仰角和航向角。

滤波器中加速度计的误差模型为：

${\left[\begin{array}{c}{\mathrm{\δf}}_x\\ \mathrm{\δ}{f}_y\\ {\mathrm{\δf}}_z\end{array}\right]}^n=C_b^n{\left[\begin{array}{c}\mathrm{\δ}{f}_x\\ {\mathrm{\δf}}_y\\ \mathrm{\δ}{f}_z\end{array}\right]}^b=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\left(\begin{array}{c}\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]\end{array}+\left[\begin{array}{ccc}{S}_{\mathrm{ax}}& S_{\mathrm{axy}}& S_{\mathrm{axz}}\\ S_{\mathrm{ayx}}& S_{\mathrm{ay}}& S_{\mathrm{ayz}}\\ S_{\mathrm{azx}}& S_{\mathrm{azy}}& S_{\mathrm{az}}\end{array}\right]\left[\begin{array}{c}{f}_b^x\\ f_b^y\\ f_b^z\end{array}\right]\right)$

式中，A_x、A_y、A_z为加速度计的常值漂移误差项；

S_ax、S_ay、S_az为加速度计的标度误差项；

S_axy、S_axz、S_ayx、S_ayz、S_azx、S_azy为加速度计的安装误差；

f_b为加速度计感受的比力矢量。

上述陀螺仪、加速度计的常值误差项可根据实际情况需要选用。这里选取的主要误差状态变量包括：光纤陀螺的常值漂移误差(3个，B_x、B_y、B_z)，加速度计的常值漂移误差(3个，A_x、A_y、A_z)，光纤陀螺仪的标度误差(1个，S_gx)，加速度计的标度误差(3个，S_ax、S_ay、S_az)，陀螺仪的安装误差(2个，S_gyx、S_gzx)，以及加速度计的安装误差(1个，S_axy)。将上述误差状态变量作为增广状态变量添加入上述系统状态方程，共计22阶的滤波器状态变量为

X(t)＝[δθ_x δθ_y δh δv_x δv_y δv_z ψ_x ψ_y ψ_z B_x B_y B_z S_gx S_gyx  S_gzx A_z A_z A_z S_ax S_ay S_az S_axy]在静基座对准时，首先使用的外部信息是零速信息，其量测模型为

Z₁(t)＝H₁X(t)+η₁(t)＝[0_3×3|I_3×3|0_3×16]X(t)+η₁(t)        (1)

式中，η₁为零速量测噪声矢量。

2)步骤4中，对系统的状态变量进行估计，需要对捷联惯组的系统方程进行离散化。离散化采用泰勒级数展开：

$\mathrm{\Φ}(k+1,k)=I+\mathrm{TA}\left(k\right)+\frac{T^2}{2!}{A}^2\left(k\right)+\frac{T^3}{3!}{A}^3\left(k\right)+...$

式中T为滤波周期。

系统模型噪声的方差为

$Q\left(k\right)=\mathrm{QT}+[\mathrm{FQ}+{\left(\mathrm{FQ}\right)}^T]\frac{T^2}{2!}+\left\{F\right[\mathrm{FQ}+{\left(\mathrm{FQ}\right)}^T]+F{\left[(\mathrm{FQ}+{\mathrm{QF}}^T)\right]}^T\}\frac{T^3}{3!}+...$

3)步骤4中卡尔曼滤波器的迭代，第k+1步的量测值为Z_k+1，则x(k+1)的卡尔曼滤波估计值按下述方程求解：

$P_{k+1/k}={\mathrm{\Φ}}_{k+1,k}{P}_{k|k}{\mathrm{\Φ}}_{k+1,k}^T+{\mathrm{\Γ}}_k{Q}_k{\mathrm{\Γ}}_k^T$

$K_{k+1}=P_{k+1/k}{H}_{k+1}^T{(H_{k+1}{P}_{K+1/k}{H}_{k+1}^T+R_{k+1})}^{-1}$

$P_{k+1/k+1}=(I-K_{k+1}{H}_{k+1})P_{k+1/k}{(I-K_{k+1}{H}_{k+1})}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T$

其中，步骤6用到的系统状态方程和零速修正量测方程同步骤4。其次使用的是光学瞄准提供的航向信息，航向修正的量测模型为

Z₂(t)＝H₂X(t)+η₂(t)＝[0_1×8|1|0_1×13]X(t)+η₂(t)           (2)

式中，η₂为航向量测噪声矢量。

其中，步骤7中用到的系统状态方程同步骤4。使用三轴向加速度计输出比力信息作为真空修正观测量，真空修正量测模型为

Z₃(t)＝H₃X(t)+η₃(t)＝[0_3×15|I_3×3|0_3×4]X(t)+η₃(t)    (3)

式中

X(t)＝[δθ_x δθ_y δh δ_vx δ_vy δ_vz ψ_x ψ_y ψ_z B_x B_y B_z  S_gx S_gyx S_zx  A_x A_y A_z S_ax S_ay S_az S_axy]是卡尔曼滤波器的状态矢量，Z₃(t)为加速度计输出比力信息的观测矢量，η₃为加速度计比力量测噪声矢量；

依据上述卡尔曼滤波器系统状态方程和量测方程(3)，应用卡尔曼滤波器的迭代公式对采集到的惯组数据和加速度计比力观测数据进行滤波计算，可在对器件误差状态变量进行估计并补偿的基础上，进一步完成对系统姿态误差的修正。上述对器件误差和系统姿态误差进行估计并补偿的过程，即为本发明的真空滤波修正过程。

3、优点及功效：本发明一种基于比力观测的捷联惯导真空滤波修正方法的优点是：本发明可以利用真空段的特点，应用加速度计输出的比力信息作为观测量对系统姿态误差进行修正，同时可实现主要器件误差的估计及补偿，从而提高捷联惯导的实际导航性能。

(四)附图说明：

图1为本发明的应用比力观测的捷联惯导真空滤波修正方法的流程图。

图2a为本发明与常规方案东向姿态角漂移比对曲线示意图

图2b为本发明与常规方案北向姿态角漂移比对曲线示意图

图2c为本发明与常规方案航向姿态角漂移比对曲线示意图

图2d为本发明与常规方案航向姿态角漂移比对曲线局部放大图示意图

图3a为本发明的加速度计零偏误差估计值收敛曲线示意图

图3b为本发明的加速度计标度误差估计值收敛曲线示意图

图中符号说明如下：

M1代表本发明方案结果，M2代表常规发明方案结果，A_x、A_y、A_z为加速度计x，y，z方向的常值漂移误差项，S_ax、S_ay、S_az为加速度计x，y，z向的标度误差项。

(五)具体实施方式：

见图1，本发明提供惯导真空滤波修正方法，该方法具体步骤如下：

步骤1、通过外部设备(例如GPS接收机)可确定载体的初始位置参数(包括初始的经度、纬度)并将其装订至导航计算机。

步骤2、捷联惯导系统进行预热，然后采集陀螺仪和加速度计的输出数据。

步骤3、对采集到的陀螺仪和加速度计的输出数据进行处理，根据捷联惯导系统的误差传播特性和古典控制理论，采用二阶调平法和方位估算法来完成系统的粗对准，初步确定载体的姿态。粗对准时间不少于30秒。

步骤4、粗对准结束后进入精对准阶段。保持载体在水平位置上静止不动，作零速修正，对准时间不少于250秒。此时惯组初始俯仰角和滚转角分别设为0度和-45度，航向角为45度。对准过程包括建立精对准过程零速修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计。

步骤5、载体从水平位置绕俯仰轴在水平面的投影旋转至竖直位置，转动时间、角度根据具体情况确定。这里绕俯仰轴在水平面投影旋转角度设定为90度，转速为1度/秒(实例)。

步骤6、保持载体在第二个竖直位置上静止不动，作零速修正和航向修正，对准时间不少于250秒。对准过程中包括建立精对准过程零速修正和航向修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计。

步骤7、两位置对准结束后立即进入飞行过程。飞行轨迹经历主动段大过载、中段前期，期间当导航解算高度大于85km时，视为载体已进入真空段，这时可利用观测到的加速度计信息进行基于卡尔曼滤波的真空修正。真空修正过程包括建立真空修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计。

步骤8、惯组进入中段后期，当导航解算高度低于85km时，视为载体已出真空段，这时无真空滤波修正，但可延续前期基于真空修正的结果继续对器件误差进行修正。

实施例说明(1)：

a.仿真飞行主要动态描述；

1、初始化条件：

初始航向角：220度；

初始俯仰角：0度；

初始滚转角：45度；

2、俯仰角速率变化范围：

0.0°/s～4.2°/s

3、纵向加速度变化范围：

2～8g

4、仿真时间

初始精对准时段：0～700s；

飞行时段：700s～1126s；

真空滤波修正有效时段：800s～1070s

b.系统误差分配如表1；

c.滤波器主要参数如表2。

表1捷联惯导系统误差源分配

表2真空修正滤波器参数

说明(2)：图2a-图2d中共有4个仿真曲线，分别为捷联惯导全程(从初始对准、主动段、真空修正段、再入段)三个方向姿态角误差漂移曲线以及航向姿态角在真空修正段的局部漂移曲线；其中实线表示使用本发明真空滤波修正方案的姿态角误差漂移过程，虚线表示使用常规真空修正方案的姿态角误差漂移过程。图2a为东向姿态角漂移比对曲线；图2b为北向姿态角漂移比对曲线；图2c为航向姿态角漂移比对曲线；图2d为航向姿态角漂移比对曲线即图2c的局部放大图。

图3a-图3b中共两个仿真曲线，表示加速度计零偏误差以及标度误差估计值随时间变化的仿真曲线。图3a为加速度计零偏误差估计值收敛曲线，图3b为加速度计标度误差估计值收敛曲线。

可以看出，与现有技术一中的方案相比，本发明除了考虑加速度计零位、标度误差的补偿外，还增加了系统姿态误差的修正过程，进一步提高了捷联惯组的实际使用精度。从图2各比对曲线以及图3各曲线中可以看出，通过本发明设计可以在修正加表零位、标度误差的同时，完成系统姿态误差的修正。

Claims (1)

1.一种基于比力观测的捷联惯导真空滤波修正方法，其特征在于：该方法具体步骤如下：

步骤1、通过外部设备确定载体的初始位置参数并将其装订至导航计算机；

步骤2、捷联惯导系统进行预热，然后采集陀螺仪和加速度计的输出数据；

步骤3、对采集到的陀螺仪和加速度计的输出数据进行处理，根据捷联惯导系统的误差传播特性和古典控制理论，采用二阶调平法和方位估算法来完成系统的粗对准，初步确定载体的姿态，粗对准时间不少于30秒；

步骤4、粗对准结束后进入精对准阶段，保持载体在水平位置上静止不动，作零速修正，对准时间不少于250秒；此时惯组初始俯仰角和滚转角分别为0度和－45度，航向角为45度，对准过程包括建立精对准过程零速修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计；

步骤5、载体从水平位置绕俯仰轴在水平面的投影旋转至竖直位置，转动时间、角度根据具体情况确定；

步骤6、保持载体在第二个竖直位置上静止不动，作零速修正和航向修正，对准时间不少于250秒；对准过程中包括建立精对准过程零速修正和航向修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计；

步骤7、两位置对准结束后立即进入飞行过程；飞行轨迹经历主动段大过载、中段前期，期间当导航解算高度大于85km时，视为载体已进入真空段，这时利用观测到的加速度计信息进行基于卡尔曼滤波的真空修正；真空修正过程包括建立真空修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计；

步骤8、惯组进入中段后期，当导航解算高度低于85km时，视为载体已出真空段，这时无真空滤波修正，但延续前期基于真空修正的结果继续对器件误差进行修正；

其中，步骤4中所述的“对准过程包括建立精对准过程零速修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计”，其具体实现过程如下：

1）导航坐标系取为游动自由方位坐标系，建立步骤4中用到的系统状态方程和量测方程如下：

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}{\mathrm{\δ\θ}}_x\\ {\mathrm{\δ\θ}}_y\\ \mathrm{\δh}\\ {\mathrm{\δv}}_x\\ {\mathrm{\δv}}_y\\ {\mathrm{\δv}}_z\\ {\mathrm{\ψ}}_x\\ {\mathrm{\ψ}}_y\\ {\mathrm{\ψ}}_z\end{array}\right]=\left[\begin{array}{ccccccccc}-\frac{v_z}{R}& 0& \frac{\mathrm{vy}}{R^2}& 0& -\frac{1}{R}& 0& 0& 0& 0\\ 0& -\frac{v_z}{R}& -\frac{v_x}{R^2}& \frac{1}{R}& 0& 0& 0& 0& 0\\ {-v}_y& v_x& 0& 0& 0& 1& 0& 0& 0\\ 0& -g& 0& 0& 2{\mathrm{\Ω}}_z& -{(\mathrm{\ρ}+2\mathrm{\Ω})}_y& 0& -f_z& f_y\\ g& 0& 0& -2{\mathrm{\Ω}}_z& 0& {(\mathrm{\ρ}+2\mathrm{\Ω})}_x& f_z& 0& -f_x\\ 0& 0& 2\frac{g}{R}& {(\mathrm{\ω}+\mathrm{\Ω})}_x& -{(\mathrm{\ω}+\mathrm{\Ω})}_x& 0& {-f}_y& f_x& 0\\ 0& 0& 0& 0& 0& 0& 0& {\mathrm{\Ω}}_z& -{\mathrm{\ω}}_y\\ 0& 0& 0& 0& 0& 0& -{\mathrm{\Ω}}_z& 0& {\mathrm{\ω}}_x\\ 0& 0& 0& 0& 0& 0& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ\θ}}_x\\ {\mathrm{\δ\θ}}_y\\ \mathrm{\δh}\\ {\mathrm{\δv}}_x\\ {\mathrm{\δv}}_y\\ {\mathrm{\δv}}_z\\ {\mathrm{\ψ}}_x\\ {\mathrm{\ψ}}_y\\ {\mathrm{\ψ}}_z\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ {\mathrm{\δf}}_x\\ {\mathrm{\δf}}_y\\ {\mathrm{\δf}}_z\\ 0\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\\ {-\mathrm{\ϵ}}_x\\ -{\mathrm{\ϵ}}_y\\ -{\mathrm{\ϵ}}_z\end{array}\right]$

式中，δθ——角位置误差矢量；δh——高度误差；δv——速度误差矢量；

ψ角姿态误差矢量；v——载体运动速度矢量；ρ——载体运动角速率矢量；

Ω——地球自转角速率矢量；ω——ρ+Ω；g——地球重力加速度；

R——地球半径；f——载体感受的比力矢量；δf——加速度计输出误差；

ε——陀螺输出误差；

滤波器中陀螺仪的误差模型为：

${\left[\begin{array}{c}{\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]}^n=C_b^n{\left[\begin{array}{c}{\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]}^b=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\left(\left[\begin{array}{c}{B}_x\\ B_y\\ B_z\end{array}\right]+\left[\begin{array}{ccc}{S}_{\mathrm{gx}}& S_{\mathrm{gxy}}& S_{\mathrm{gxz}}\\ S_{\mathrm{gyx}}& S_{\mathrm{gy}}& S_{\mathrm{gyz}}\\ S_{\mathrm{gzx}}& S_{\mathrm{gzy}}& S_{\mathrm{gz}}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib}}^x\\ {\mathrm{\ω}}_{\mathrm{ib}}^y\\ {\mathrm{\ω}}_{\mathrm{ib}}^z\end{array}\right]\right)$

式中，B_x、B_y、B_z为陀螺的常值漂移误差项；S_gx、S_gy、S_gz为陀螺的标度误差项；

S_gxy、S_gxz、S_gyx、S_gyz、S_gzx、S_gzy为陀螺的安装误差；

ω_ib为陀螺仪敏感的相对惯性空间的角速度矢量；

为捷联惯导的位置矩阵

$C_b^n=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\γ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}\\ \sin \mathrm{\θ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}\end{array}\right]$

上式中γ、θ、ψ分别表示载体的横滚角，俯仰角和航向角；

滤波器中加速度计的误差模型为：

${\left[\begin{array}{c}\mathrm{\δ}{f}_x\\ \mathrm{\δ}{f}_y\\ \mathrm{\δ}{f}_z\end{array}\right]}^n=C_b^n{\left[\begin{array}{c}\mathrm{\δ}{f}_x\\ \mathrm{\δ}{f}_y\\ \mathrm{\δ}{f}_z\end{array}\right]}^b=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right](\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]+\left[\begin{array}{ccc}{S}_{\mathrm{ax}}& S_{\mathrm{axy}}& S_{\mathrm{axz}}\\ S_{\mathrm{ayx}}& S_{\mathrm{ay}}& S_{\mathrm{ayz}}\\ S_{\mathrm{azx}}& S_{\mathrm{azy}}& S_{\mathrm{az}}\end{array}\right]\left[\begin{array}{c}{f_b}^x\\ {f_b}^y\\ {f_b}^z\end{array}\right])$

式中，A_x、A_y、A_z为加速度计的常值漂移误差项；

S_ax、S_ay、S_az为加速度计的标度误差项；

S_axy、S_axz、S_ayx、S_ayz、S_azx、S_azy为加速度计的安装误差；

f_b为加速度计感受的比力矢量；

上述陀螺仪、加速度计的常值误差项根据实际情况需要选用，这里选取的误差状态变量包括：光纤陀螺的3个常值漂移误差B_x、B_y、B_z，加速度计的3个常值漂移误差A_x、A_y、A_z，光纤陀螺仪的1个标度误差S_gx，加速度计的3个标度误差S_ax、S_ay、S_az，陀螺仪的2个安装误差S_gyx、S_gzx，以及加速度计的1个安装误差S_axy；将上述误差状态变量作为增广状态变量添加入上述系统状态方程，共计22阶的滤波器状态变量为X(t)=[δθ_x δθ_y δh δv_x δv_y δv_z ψ_x ψ_y ψ_z B_x B_y B_z S_gx S_gyx S_gzx A_x A_y A_z S_ax S_ay S_az S_axy]

在静基座对准时，首先使用的外部信息是零速信息，其量测模型为

Z₁(t)=H₁X(t)+η₁(t)=[0_3×3|I_3×3|0_3×16]X(t)+η₁(t)    （1）

式中，η₁为零速量测噪声矢量；

2）步骤4中，对系统的状态变量进行估计，需要对捷联惯组的系统方程进行离散化；离散化采用泰勒级数展开：

$\mathrm{\Φ}(k+1,k)=I+\mathrm{TA}\left(k\right)+\frac{T^2}{2!}{A}^2\left(k\right)+\frac{T^3}{3!}{A}^3\left(k\right)+...$

式中T为滤波周期；

系统模型噪声的方差为

$Q\left(k\right)=\mathrm{QT}+[\mathrm{FQ}+{\left(\mathrm{FQ}\right)}^T]\frac{T^2}{2!}+\left\{F\right[\mathrm{FQ}+{\left(\mathrm{FQ}\right)}^T]+F{\left[(\mathrm{FQ}+Q{F}^T)\right]}^T\}\frac{T^3}{3!}+...$

3）步骤4中卡尔曼滤波器的迭代，第k+1步的量测值为Z_k+1，则x(k+1)的卡尔曼滤波估计值按下述方程求解：

$P_{k+1/k}={\mathrm{\Φ}}_{k+1,k}{P}_{k/k}{\mathrm{\Φ}}_{k+1,k}^T+{\mathrm{\Γ}}_k{Q}_k{\mathrm{\Γ}}_k^T$

$K_{k+1}=P_{k+1/k}{H}_{k+1}^T{(H_{k+1}{P}_{K+1/k}{H}_{k+1}^T+R_{k+1})}^{-1}$

$P_{k+1/k+1}=(I-K_{k+1}{H}_{k+1})P_{k+1/k}{(I-K_{k+1}{H}_{k+1})}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T;$

其中，步骤6用到的系统状态方程和零速修正量测方程同步骤4；其次使用的是光学瞄准提供的航向信息，航向修正的量测模型为

Z₂(t)=H₂X(t)+η₂(t)=[0_1×8|1|0_1×13]X(t)+η₂(t)   （2）

式中，η₂为航向量测噪声矢量；

其中，步骤7中用到的系统状态方程同步骤4；使用三轴向加速度计输出比力信息作为真空修正观测量，真空修正量测模型为

Z₃(t)=H₃X(t)+η₃(t)=[0_3×15|I_3×3|0_3×4]X(t)+η₃(t)   （3）

式中

X(t)=[δθ_x δθ_y δh δv_x δv_y δv_z ψ_x ψ_y ψ_z B_x B_y B_z S_gx S_gyx S_gzx A_x A_y A_z S_ax S_ay S_az S_axy]是卡尔曼滤波器的状态矢量，Z₃(t)为加速度计输出比力信息的观测矢量，η₃为加速度计比力量测噪声矢量；

依据上述卡尔曼滤波器系统状态方程和量测方程（3），应用卡尔曼滤波器的迭代公式对采集到的惯组数据和加速度计比力观测数据进行滤波计算，在对器件误差状态变量进行估计并补偿的基础上，进一步完成对系统姿态误差的修正。