一种基于数字高通滤波的旋转捷联系统现场标定方法

摘要

本发明提供的是一种基于数字高通滤波的旋转捷联系统现场标定方法。(1)通过GPS确定载体的初始位置参数；(2)采集光纤陀螺仪输出和加速度计输出的数据并对数据进行处理；(3)惯性测量单元单轴四位置转停；(4)利用谱条件数法分析惯性器件偏差的可观测度；(5)采用IIR高通数字滤波器滤除导航系下的速度信息中包含的舒勒周期；(6)以滤波后的速度信息作为观测量，采用卡尔曼滤波技术估计惯性器件的偏差。当载体处于系泊状态下，采用本发明可以获得较高现场标定精度。

说明书

技术领域

本发明涉及的是一种测量方法，尤其涉及的是一种基于数字高通滤波的旋转捷联系统现场标定方法。

背景技术

捷联惯性导航系统(SINS)是将陀螺仪、加速度计等惯性元件固连在载体上，根据牛顿力学定律，通过对这些惯性元件采集的信息进行处理，得到载体的姿态、速度、位置、加速度、角速度和角加速度等全导航信息的完全自主导航设备。由于SINS完全依靠自身的惯性元件，不依靠任何外界信息测量导航参数，因此，它具有隐蔽性好，不受气候条件限制，不受干扰等优点，是一种完全自主式、全天候的导航系统，已广泛应用于航空、航天、航海等领域。根据SINS的基本原理，SINS在导航过程中惯性器件常值偏差的存在是导致惯导系统导航精度难以提高的主要因素。如何有效限制惯性导航误差发散、提高惯性导航系统精度是惯性导航领域一项非常重要的课题。

惯导系统的误差抑制，不是依赖于外部辅助对误差状态进行估计，而是研究惯性导航误差在特定运动条件下的传播规律，并依据此规律限制误差发散，提高导航精度的方法。转动抑制是最典型的误差抑制方法：通过绕一个轴或多个轴转动惯性测量单元(IMU)，对导航误差进行调制，达到控制导航误差发散、提高导航精度的目的。

标定技术就是一种从软件方面来提高惯导实际使用精度的方法。标定技术本质上也是一种误差补偿技术。所谓误差补偿技术就是建立惯性元件和惯导系统的误差数学模型，通过一定的试验来确定模型系数，进而通过软件算法来消除误差。惯性元件和惯导系统在出厂之前，必须通过标定来确定基本的误差数学模型参数，以保证元件和系统的正常工作。而且惯性元件高阶误差项的研究、惯导系统恶劣动态环境下的误差补偿都是在标定的基础上进行的，可以说标定工作是整个误差补偿技术的基础。惯性测量单元逐次开机误差(惯性元件的漂移与刻度因数误差)是不同的，且随着时间增加IMU输出误差随时间发生漂移，实现现场在线标定对于提高系统精度具有重大意义。

在CNKI库中与本发明相关的公开报道有：1、《水平初始对准误差对旋转IMU导航系统的精度影响》，该文章主要讲述了水平不对准对旋转捷联惯导系统的影响，其中文中讲述了IMU单轴单向旋转，但是并没有提及现场标定的内容。2、《旋转IMU在光纤捷联航姿系统中的应用》，该文章主要介绍了单轴、双轴旋转方式，并在理论上给予证明。3、《光纤陀螺IMU的六位置旋转现场标定新方法》，该文章主要介绍了在使用现场将光纤陀螺IMU在六个位置上进行十二次旋转，然后根据光纤陀螺IMU的误差模型建立42个非线性输入输出方程，通过旋转积分和对称位置误差相消，消除方程中的非线性项，最终求解出陀螺标度因数、陀螺常值漂移、陀螺安装误差和加速度计常值偏置等15个误差系数。

发明内容

本发明的目的在于提供一种当载体处于系泊状态下，可以获得较高现场标定精度的一种基于数字高通滤波的旋转捷联系统现场标定方法.

本发明的目的是这样实现的：包括以下步骤：

(1)利用全球定位系统GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(2)光纤陀螺捷联惯性导航系统进行预热后采集光纤陀螺仪和石英加速度计输出的数据；

(3)IMU采用8个转停次序为一个旋转周期的转位方案；

(4)利用谱条件数法求取IMU四位置转停过程中惯性器件偏差的可观测度；

(5)设计无限冲击响应数字高通滤波器，将导航系下解算出的载体水平速度进行高通滤波处理，滤除导航系下载体速度中的舒勒周期，保留载体由于摇摆和荡运动产生的速度偏差；

(6)根据惯导系统动基座误差方程建立载体系泊状态时的估漂模型，以高通滤波后得到的速度直接作为观测量，利用卡尔曼滤波技术实现旋转捷联惯导系统的现场标定。

本发明还可以包括：

1、所述IMU采用8个转停次序为一个旋转周期的转位方案为：

次序1，IMU从A点出发顺时针转动180°到达位置C，停止时间T_t；次序2，IMU从C点出发顺时针转动90°到达位置D，停止时间T_t；次序3，IMU从D点出发逆时针转动180°到达位置B，停止时间T_t；次序4，IMU从B点出发逆时针转动90°到达位置A，停止时间T_t；次序5，IMU从A点出发逆时针转动180°到达位置C，停止时间T_t；次序6，IMU从C点出发逆时针转动90°到达位置B，停止时间T_t；次序7，IMU从B点出发顺时针转动180°到达位置D，停止时间T_t；次序8，IMU从D点出发顺时针转动90°到达位置A，停止时间T_t；IMU按照此转动顺序循环进行；水平东向轴上的IMU停顿点p3、p8与p4、p7对称于转轴中心；北向轴上的停顿点p1、p5与p2、p6对称于转轴中心；四位置转停方案仍然是转动角度为180°或90°间隔进行。

2、所述利用谱条件数法求取IMU四位置转停过程中惯性器件偏差的可观测度的方法为：

求解线性方程组

AX＝b，b∈Cⁿ

设A∈C^n×n，||·||是一种算子范数，

$\mathrm{cond}\left(A\right)=\left(\begin{array}{cc}\left|\right|A\left|\right|\left|\right|A^{-1}\left|\right|,& \det A\ne 0\\ \infty ,& \det A=0\end{array}\right)$

称cond(A)为矩阵A的关于算子范数||·||的条件数，常用的是关于p-范数||·||_p的条件数，记作cond_p(A)，cond₂(A)为谱条件数，

针对离散时变系统：

$\left(\begin{array}{c}{X}_{k+1}=F_k{X}_k\\ Z_k=H{X}_k\end{array}\right)$

将系统状态方程带入观测方程得到一组方程：

$\left(\begin{array}{c}{Z}_0=H{X}_0\\ Z_1=H{F}_0{X}_0\\ .\\ .\\ .\\ Z_k=H\underset{i=0}{\overset{k}{\Pi }}{F}_i{X}_0\end{array}\right)$

记

$O_k={\left(\begin{array}{cc}H& {\mathrm{HF}}_0...H\underset{i=0}{\overset{k}{\Pi }}{F}_i\end{array}\right)}^T$

则

O_kX₀＝Z

对于定常系统F_k为常数，O_k就是可观性矩阵，时变系统在采样点上进行观测得到离散时变系统，F_k就是采样周期内的状态转移矩阵Φ(t_k+T，t_k)，

O_k＝[HHΦ(t₁，t₀)…HΦ(t_k，t₀)]^T

状态是n维的，一次观测量Z_k是r维的(r＜n)，观测阵H的秩为r，至少进行k次观测(kr≥n)，求出X₀，根据最小二乘法求解状态X₀，

$X_0={\left(O_k^T{O}_k\right)}^{-1}{O}_k^{T}Z$

为观测阵，由于是正规矩阵，通过计算谱条件数cond₂(M)，来分析解的稳定性，而

${\mathrm{cond}}_2\left(M\right)=\frac{\underset{\lambda \in \lambda \left(M\right)}{\max }\left|\lambda \right|}{\underset{\lambda \in \lambda \left(M\right)}{\min }\left|\lambda \right|}$

式中，λ为矩阵M的特征值，进一步分析矩阵M的特征值和特征向量，以便确定究竟哪些状态的可观测度较好，哪些状态的可观测度差，将M可酉对角化，记U^TMU＝Λ，其中Λ＝diag(λ₁，λ₂，...λ_n)，则状态X的可观测度S：

S＝abs(U[λ₁，λ₂，...，λ_n]^T)

计算出系统可观测性矩阵M的特征值和特征向量，确定出各个状态的可观测度。

3、所述利用卡尔曼滤波技术实现旋转捷联惯导系统的现场标定的方法为：

1)建立卡尔曼滤波的状态方程：

用一阶线性微分方程来描述旋转捷联惯导系统的状态误差：

$\stackrel{·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)$

其中X(t)为t时刻系统的状态向量；A(t)和B(t)分别为系统的状态矩阵和噪声矩阵；W(t)为系统噪声向量；

系统的状态向量为：

系统的白噪声向量为：

W＝[a_x a_y ω_x ω_y ω_z 0 0 0 0 0 0 0 0]^T

其中δV_e、δV_n分别表示东向、北向的速度误差；分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；δK_gx、δK_gy、δK_gz分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的标度因数误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差；

系统的状态转移矩阵为：

$A=\left(\begin{array}{cccc}{F}_{2\times 2}^1& f_{2\times 3}& {\tilde{T}}_{2\times 2}& O_{2\times 6}\\ F_{3\times 2}^2& F_{3\times 3}^3& O_{3\times 2}& T_{3\times 6}\\ O_{8\times 2}& O_{8\times 3}& O_{8\times 2}& O_{8\times 6}\end{array}\right)$

$F_{2\times 2}^1=\left(\begin{array}{cc}\frac{V_N}{R_n}\tan L& 2{\omega }_{\mathrm{ie}}\sin L+\frac{V_E}{R_n}\tan L\\ -({2\omega }_{\mathrm{ie}}\sin L+\frac{{2V}_E}{R_n}\tan L)& 0\end{array}\right)$

$F_{3\times 2}^2=\left(\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right)$

$F_{3\times 3}^3=\left(\begin{array}{ccc}0& {\omega }_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n}& -({\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_n})\\ -({\omega }_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n})& 0& -\frac{V_N}{R_m}\\ {\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_n}& \frac{V_N}{R_m}& 0\end{array}\right)$

$f_{2\times 3}=\left(\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& f_E\end{array}\right)$

${\tilde{T}}_{2\times 2}=\left(\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right)$

$T_{3\times 6}=\left(\begin{array}{cccccc}-T_{11}& -T_{12}& -T_{13}& -T_{11}{\omega }_x& -T_{12}{\omega }_y& -T_{13}{\omega }_z\\ -T_{21}& -T_{22}& -T_{23}& -T_{21}{\omega }_x& -T_{22}{\omega }_y& -T_{23}{\omega }_z\\ -T_{31}& -T_{32}& -T_{33}& -T_{31}{\omega }_x& -T_{32}{\omega }_y& -T_{33}{\omega }_z\end{array}\right)$

V_E、V_N分别表示东向、北向的速度；ω_x、ω_y、ω_z分别表示陀螺的三个输入角速度；ω_ie表示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；f_E、f_N、f_U分别表示为导航坐标系下东向、北向、天向的比力；

2)建立卡尔曼滤波的量测方程：

用一阶线性微分方程来描述旋转捷联惯导系统的量测方程如下：

Z(t)＝H(t)X(t)+V(t)

其中：Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；V(t)表示系统的量测噪声；

系统量测矩阵为：

$H=\left(\begin{array}{ccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right)$

量测量为高通滤波后得到的速度：

$Z={\left(\begin{array}{cc}{\tilde{V}}_E& {\tilde{V}}_N\end{array}\right)}^T.$

本发明将惯性测量单元相对载体方位轴固定的四个位置转停，利用IMU的转停运动提高系统参数的可观测度，设计卡尔曼滤波器引入高精度外部信息源分别激励IMU的各个标定误差项，估计并补偿IMU输出误差，完成系统的现场标定。

本发明与现有技术相比的优点在于：本发明打破了传统标定方法不适用于系统的现场标定，提出一种利用IMU转停提高系统参数可观测度并采用卡尔曼滤波技术对惯性器件偏差进行在线标定的方案，该方法可以将惯性器件常值偏差和陀螺仪标度因数误差进行现场估计并补偿，可以有效地提高系统对准及导航精度。

对本发明有益的效果说明如下：

在Visual C++仿真条件下，对该方法进行仿真实验：

设定陀螺仪常值漂移为0.01°/h，加速度计零位偏差为10^-4g；系统初始对准误差为0.1°、0.1°、0.5°；载体以正弦规律绕纵摇轴、横摇轴和航向轴作三轴摇摆运动，其数学模型为：

$\left(\begin{array}{c}\theta ={\theta }_m\sin ({\omega }_{\theta }t+{\phi }_{\theta })\\ \gamma ={\gamma }_m\sin ({\omega }_{\gamma }t+{\phi }_{\gamma })\\ \psi ={\psi }_m\sin ({\omega }_{\psi }t+{\phi }_{\psi })+k\end{array}\right)$

其中：θ、γ、ψ分别表示纵摇角、横摇角和航向角的摇摆角度变量；θ_m、γ_m、ψ_m分别表示相应的摇摆角度幅值；ω_θ、ω_γ、ω_ψ分别表示相应的摇摆角频率；φ_θ、φ_γ、φ_ψ分别表示相应的初始相位；ω_i＝2τ/T_i，i＝θ、γ、ψ，T_i表示相应的摇摆周期，k为初始航向角。仿真时取：θ_m＝6°，γ_m＝12°，ψ_m＝10°，T_θ＝8s，T_γ＝10s，T_ψ＝6s，k＝0°。

载体的横荡、纵荡和垂荡引起的加速度为：

式中，i＝x，y，z为地理坐标系的东向、北向、天向。为[0，2π]上服从均匀分布的随机相位。

载体初始位置：北纬45.7796°，东经126.6705°；

初始姿态误差角：三个初始姿态误差角均为零；

赤道半径：R_e＝6378393.0m；

椭球度：e＝3.367e-3；

由万有引力可得的地球表面重力加速度：g₀＝9.78049；

地球自转角速度(弧度/秒)：7.2921158e-5；

陀螺仪常值漂移：0.01度/小时；

陀螺仪随机游走：0.001度/

陀螺仪标度因数误差：1000ppm；

加速度计零偏：10^-4g₀；

加速度计噪声：10^-6g₀；

常数：π＝3.1415926；

IMU四位置转停方案的数学模型参数：

四个位置的停顿时间：T_t＝5min；

转动180°和90°时消耗的时间：T_z＝12s；

转动180°和90°的过程中，每一个转位中的加减速时间各为4s。

附图说明

图1为本发明的一种基于数字高通滤波的旋转捷联系统现场标定方法流程图；

图2为本发明的IMU四位置转停过程中，IMU坐标系与载体坐标系的相对位置关系；

图3为本发明的估计的陀螺仪常值漂移；

图4为本发明的估计的水平方向上的加速度计零偏；

图5为本发明的估计的陀螺仪标度因数误差。

具体实施方式

下面结合附图举例对本发明做更详细地描述：

(1)利用全球定位系统GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(2)光纤陀螺捷联惯性导航系统进行预热后采集光纤陀螺仪和石英加速度计输出的数据；

(3)IMU采用8个转停次序为一个旋转周期的转位方案；

次序1，IMU从A点出发顺时针转动180°到达位置C，停止时间T_t；次序2，IMU从C点出发顺时针转动90°到达位置D，停止时间T_t；次序3，IMU从D点出发逆时针转动180°到达位置B，停止时间T_t；次序4，IMU从B点出发逆时针转动90°到达位置A，停止时间T_t；次序5，IMU从A点出发逆时针转动180°到达位置C，停止时间T_t；次序6，IMU从C点出发逆时针转动90°到达位置B，停止时间T_t；次序7，IMU从B点出发顺时针转动180°到达位置D，停止时间T_t；次序8，IMU从D点出发顺时针转动90°到达位置A，停止时间T_t；IMU按照此转动顺序循环进行。为了有效地对水平方向上的惯性器件偏差在对称位置上进行正负平均，定义水平东向轴上的IMU停顿点p3、p8与p4、p7对称于转轴中心；北向轴上的停顿点p1、p5与p2、p6对称于转轴中心。改进的四位置转停方案仍然是转动角度为180°或90°间隔进行。

(4)利用谱条件数法求取IMU四位置转停过程中惯性器件偏差的可观测度；

考虑求解线性方程组

AX＝b，b∈Cⁿ            (1)

设A∈C^n×n，||·||是一种算子范数，

$\mathrm{cond}\left(A\right)=\left(\begin{array}{cc}\left|\right|A\left|\right|\left|\right|A^{-1}\left|\right|,& \det A\ne 0\\ \infty ,& \det A=0\end{array}\right)---\left(2\right)$

称cond(A)为矩阵A(关于求逆或求解线性方程组)的关于算子范数||·||的条件数。常用的是关于p-范数||·||_p的条件数，可记作cond_p(A)。特别，称cond₂(A)为谱条件数。

针对离散时变系统：

$\left(\begin{array}{c}{X}_{k+1}=F_k{X}_k\\ Z_k=H{X}_k\end{array}\right)---\left(3\right)$

能否由一组观测值Z＝[Z₀，Z₁，....Z_k]^T求出初始状态X₀是系统可观的实质。将系统状态方程带入观测方程得到一组方程：

$\left(\begin{array}{c}{Z}_0=H{X}_0\\ Z_1=H{F}_0{X}_0\\ .\\ .\\ .\\ Z_k=H\underset{i=0}{\overset{k}{\Pi }}{F}_i{X}_0\end{array}\right)---\left(4\right)$

记

$O_k={\left(\begin{array}{cc}H& {\mathrm{HF}}_0...H\underset{i=0}{\overset{k}{\Pi }}{F}_i\end{array}\right)}^T---\left(5\right)$

则有

O_kX₀＝Z            (6)

对于定常系统F_k为常数，O_k就是可观性矩阵。时变系统在采样点上进行观测得到离散时变系统，F_k就是采样周期内的状态转移矩阵Φ(t_k+T，t_k)，因此有

O_k＝[H HΦ(t₁，t₀)…HΦ(t_k，t₀)]^T                (7)

设状态是n维的，一次观测量Z_k是r维的(r＜n)，观测阵H的秩为r，至少进行k次观测(kr≥n)，才可以求出X₀。根据最小二乘法求解状态X₀。

$X_0={\left(O_k^T{O}_k\right)}^{-1}{O}_k^{T}Z---\left(8\right)$

记为观测阵。由于是正规矩阵，则可以通过计算谱条件数cond₂(M)，来分析解的稳定性，而

${\mathrm{cond}}_2\left(M\right)=\frac{\underset{\lambda \in \lambda \left(M\right)}{\max }\left|\lambda \right|}{\underset{\lambda \in \lambda \left(M\right)}{\min }\left|\lambda \right|}---\left(9\right)$

式中，λ为矩阵M的特征值。进一步分析矩阵M的特征值和特征向量，以便确定究竟哪些状态的可观测度较好，哪些状态的可观测度差。将M可酉对角化，记U^TMU＝Λ，其中Λ＝diag(λ₁，λ₂，...λ_n)，则状态X的可观测度S：

S＝abs(U[λ₁，λ₂，...，λ_n]^T)            (10)

计算出系统可观测性矩阵M的特征值和特征向量，就可以确定出各个状态的可观测度。

(5)设计无限冲击响应数字高通滤波器(IIR)，将导航系下解算出的载体水平速度进行高通滤波处理，滤除导航系下载体速度中的舒勒周期，保留载体由于摇摆和荡运动产生的速度偏差；

(6)根据惯导系统动基座误差方程建立载体系泊状态时的估漂模型，以高通滤波后得到的速度直接作为观测量。利用卡尔曼滤波技术实现旋转捷联惯导系统的现场标定；

建立以经过高通滤波后的导航系下的水平速度为观测量的卡尔曼滤波模型；

1)建立卡尔曼滤波的状态方程：

用一阶线性微分方程来描述旋转捷联惯导系统的状态误差：

$\stackrel{·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(11\right)$

其中X(t)为t时刻系统的状态向量；A(t)和B(t)分别为系统的状态矩阵和噪声矩阵；W(t)为系统噪声向量；

系统的状态向量为：

系统的白噪声向量为：

W＝[a_x a_y ω_x ω_y ω_z 0 0 0 0 0 0 0 0]^T        (13)

其中δV_e、δV_n分别表示东向、北向的速度误差；分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；δK_gx、δK_gy、δK_gz分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的标度因数误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差；

系统的状态转移矩阵为：

$A=\left(\begin{array}{cccc}{F}_{2\times 2}^1& f_{2\times 3}& {\tilde{T}}_{2\times 2}& O_{2\times 6}\\ F_{3\times 2}^2& F_{3\times 3}^3& O_{3\times 2}& T_{3\times 6}\\ O_{8\times 2}& O_{8\times 3}& O_{8\times 2}& O_{8\times 6}\end{array}\right)---\left(14\right)$

$F_{2\times 2}^1=\left(\begin{array}{cc}\frac{V_N}{R_n}\tan L& 2{\omega }_{\mathrm{ie}}\sin L+\frac{V_E}{R_n}\tan L\\ -({2\omega }_{\mathrm{ie}}\sin L+\frac{{2V}_E}{R_n}\tan L)& 0\end{array}\right)---\left(15\right)$

$F_{3\times 2}^2=\left(\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right)---\left(16\right)$

$F_{3\times 3}^3=\left(\begin{array}{ccc}0& {\omega }_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n}& -({\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_n})\\ -({\omega }_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n})& 0& -\frac{V_N}{R_m}\\ {\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_n}& \frac{V_N}{R_m}& 0\end{array}\right)---\left(17\right)$

$f_{2\times 3}=\left(\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& f_E\end{array}\right)---\left(18\right)$

${\tilde{T}}_{2\times 2}=\left(\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right)---\left(19\right)$

$T_{3\times 6}=\left(\begin{array}{cccccc}-T_{11}& -T_{12}& -T_{13}& -T_{11}{\omega }_x& -T_{12}{\omega }_y& -T_{13}{\omega }_z\\ -T_{21}& -T_{22}& -T_{23}& -T_{21}{\omega }_x& -T_{22}{\omega }_y& -T_{23}{\omega }_z\\ -T_{31}& -T_{32}& -T_{33}& -T_{31}{\omega }_x& -T_{32}{\omega }_y& -T_{33}{\omega }_z\end{array}\right)---\left(20\right)$

V_E、V_N分别表示东向、北向的速度；ω_x、ω_y、ω_z分别表示陀螺的三个输入角速度；ω_ie表示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；f_E、f_N、f_U分别表示为导航坐标系下东向、北向、天向的比力。

2)建立卡尔曼滤波的量测方程：

用一阶线性微分方程来描述旋转捷联惯导系统的量测方程如下：

Z(t)＝H(t)X(t)+V(t)                (21)

其中：Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；V(t)表示系统的量测噪声；

系统量测矩阵为：

$H=\left(\begin{array}{ccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right)---\left(22\right)$

量测量为高通滤波后得到的速度：

$Z={\left(\begin{array}{cc}{\tilde{V}}_E& {\tilde{V}}_N\end{array}\right)}^T---\left(23\right)$