CN104374401A - 一种捷联惯导初始对准中重力扰动的补偿方法 - Google Patents

Abstract

本发明属于初始对准中误差补偿领域，具体涉及一种捷联惯导初始对准中重力扰动的补偿方法。本发明包括：采集光纤陀螺仪输出的角速度数据和石英加速度计输出的加速度数据；通过重力扰动位计算对准地点的重力扰动值，对石英加速度计的输出加速度数据进行补偿；采用解析法来完成粗对准，选取滤波初始值；估计出平台失准角；完成精确的初始对准。发明中捷联惯导初始对准中重力扰动的补偿方法是，根据已知的经纬度值并通过EGM2008模型计算出对准点的重力扰动值，然后算出的重力扰动值从加速度计中剔除掉，就得到重力扰动补偿后加速度计的输出，仿真结果表明重力扰动补偿后可提高初始对准精度。

Description

一种捷联惯导初始对准中重力扰动的补偿方法

技术领域

本发明属于初始对准中误差补偿领域，具体涉及一种捷联惯导初始对准中重力扰动的补偿方法。 

背景技术

初始对准是捷联惯性导航***关键技术之一。初始对准精度直接影响捷联惯性导航***的工作精度。捷联惯性导航***初始对准的主要目的是建立姿态矩阵的初值，初始对准中通过初始对准状态空间模型，利用卡尔曼滤波将初始失准角状态估计出来并用以校正姿态矩阵。传统的对准过程包括粗对准和精对准两个阶段，首先用粗对准模型粗略估计出失准角的大小，然后再利用精对准模型估计出失准角的大小而实现精对准。捷联惯性导航***的严格数学误差模型是一组非线性微分方程，而实际粗对准的失准角在很多情况下为大角度，因此采用非线性模型更能真实的反映误差传播特性。 

惯性导航解算时通常用的是正常重力值，正常重力值是将地球近似为一个质量均匀的旋转椭球模型而得到的重力值；而实际由于地球内部质量分布不均匀，重力矢量包括正常重力和重力扰动。由于不同地区地球内部质量分布不同，这些重力扰动在地表或者海平面上是变化的。惯性加速度计不能区分地球重力的切线方向分量和机体的水平加速度，因此这些重力扰动会引起惯导***解算误差。在惯性导航中，使用加速度计测量载***置的比力矢量，比力矢量是测量点绝对加速度与重力加速度之差为：从测得的比力中补偿掉引力加速度，就可以得到载体的绝对加速度，再根据绝对加速度和相对加速度的关系，惯导***可进一步求得载体的相对速度、位置。显然，如果航迹上的重力加速度不能真实的反映实际重力加速度，那么将会导致测量点加速度的误差，进一步导致导航解算误差。随着惯性器件精度不断提高与高精度惯性导航***需求的发展,重力扰动成为高精度惯导初始对准的一项主要误差源。初始对准的精度的高低直接影响到惯性导航的精度，要实现高精度的惯性导航有必要补偿初始对准中的重力扰动。 

发明内容

本发明的目的是为了提高初始对准的精度，补偿初始对准时存在的重力扰动，提出用地球重力场模型EGM2008计算重力扰动并加以补偿的捷联惯导初始对准中重力扰动的补偿方法。 

本发明的目的是这样实现的： 

(1)采集光纤陀螺仪输出的角速度数据和石英加速度计输出的加速度数据； 

(2)通过重力扰动位计算对准地点的重力扰动值，对石英加速度计的输出加速度数据进 行补偿； 

(3)采用解析法来完成粗对准，通过初始矩阵确定载体的姿态信息即纵摇角、横摇角、航向角，其中b代表载体坐标系，n′代表计算导航坐标系； 

(4)选取滤波初始值 

${\hat{x}}_0=E\left[x_0\right],P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right],$

其中x₀为状态变量的初值,P₀为状态变量的初始误差协方差矩阵； 

(5)利用无迹卡尔曼滤波方法进行滤波，估计出平台失准角； 

(6)利用估计出来的平台失准角修正***的捷联初始矩阵得到精确的初始捷联矩阵即从而完成精确的初始对准。 

重力扰动位T(ρ,L,λ)， 

$T(\mathrm{\ρ},L,\mathrm{\λ})=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{N}{\mathrm{\Σ}}}{\left(\frac{R}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[C_{\mathrm{nm}}\cos \mathrm{m\λ}+S_{\mathrm{nm}}\sin \mathrm{m\λ}]P_{\mathrm{nm}}\left(\cos \mathrm{\θ}\right),$

地球平均半径为R，θ＝90-L是地心余纬度，位系数C_nm,S_nm是已知的重力扰动的系数，可通过EGM2008重力场模型获得，L表示地球纬度，λ为地球经度，ρ为对准地点处的地心向径，GM为地球常系数，P_nm(·)为缔合勒让德多项式： 

$P_{\mathrm{nm}}\left(x\right)={(1-x^2)}^{m/2}\underset{k=0}{\overset{N}{\mathrm{\Σ}}}\frac{{(-1)}^k(2n-2k)!}{2^{n}k!(n-k)!(n-m-2k)!}{x}^{n-m-2k},$

x为变量，|x|＜1；n称为阶，m称为次，N＝[(n-m)2]； 

重力扰动位的梯度即为重力扰动矢量δgⁿ： 

$\mathrm{\δ}{g}^n=\mathrm{grad}\mathrm{dT}(\mathrm{\ρ},L,\mathrm{\λ})={(\frac{\∂T}{\∂\mathrm{\ρ}},\frac{\∂T}{\∂L},\frac{\∂T}{\∂\mathrm{\λ}})}^T,$

通过重力扰动矢量得到极坐标系下的重力扰动值： 

$T_{\mathrm{\ρ}}=\frac{\∂T}{\∂\mathrm{\ρ}}=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}(n+1){\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[{\stackrel{\‾}{C}}_{\mathrm{nm}}\cos \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\sin \mathrm{m\λ}]{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)$ $T_L=\frac{\∂T}{\∂L}=-\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}{\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[{\stackrel{\‾}{C}}_{\mathrm{nm}}\cos \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\sin \mathrm{m\λ}]\frac{d{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)}{\mathrm{dL}}$

$T_{\mathrm{\λ}}=\frac{\∂T}{\∂\mathrm{\λ}}=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}{\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[-{\stackrel{\‾}{C}}_{\mathrm{nm}}\sin \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\cos \mathrm{m\λ}]{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)$

在东北天-ENU地理坐标系上，重力扰动矢量3个轴向的分量为转换到地理坐标系为： 

${\mathrm{\δg}}_E^n=\frac{T_{\mathrm{\λ}}}{\mathrm{\ρ}\cos L},{\mathrm{\δg}}_N^n=\frac{T_L}{\mathrm{\ρ}},\mathrm{\δ}{g}_U^n=T_{\mathrm{\ρ}},$

然后对加速度计的输出进行补偿。 

初始矩阵： 

θ₀是纵摇角，γ₀是横摇角，是航向角。 

状态变量是平台失准角、水平速度误差。 

步骤(5)包括： 

(5.1)构造2n+1维的Sigma点 

$\left\{\begin{array}{c}{\mathrm{\ξ}}_0={\hat{x}}_{k-1}\\ {\mathrm{\ξ}}_i={\hat{x}}_{k-1}+\sqrt{(m+\mathrm{\κ})P_{k-1}}\\ {\mathrm{\ξ}}_{i+n}={\hat{x}}_{k-1}-\sqrt{(m+\mathrm{\κ})P_{k-1}}\end{array}\right.,i=\mathrm{1,2},...,m$

其中为k-1时刻的状态估计，P_k-1为k-1时刻的状态误差协方差矩阵，m为***状态变量的维数，κ为比例系数，可用于调节Sigma点和的距离； 

(5.2)确定权值 

$w_i^m=w_i^c=\left\{\begin{array}{cc}\mathrm{\κ}/(m+\mathrm{\κ})& (i=0)\\ 1/\left[2(m+\mathrm{\κ})\right]& (i\≠0)\end{array}\right.$

(5.3)通过滤波方程进行时间更新 

γ_k/k-1＝f_k-1(ξ_k-1) 

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i{\mathrm{\γ}}_{i,k/k-1}$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i({\mathrm{\γ}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\γ}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1}$

由和P_k-1计算Sigma点ξ_k-1，通过非线性状态函数f_k-1(·)传播为γ_k/k-1，由γ_k/k-1可得状态预测和预测误差协方差阵P_k/k-1，Q_k-1为***噪声； 

(5.4)通过滤波方程进行量测更新 

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H{\hat{x}}_{k/k-1})$

P_k＝(I-K_kH)P_k/k-1

其中R_k为量测噪声，K_k为滤波增益，P_k为估计误差协方差矩阵，为状态估计值其中 包括平台失准角。 

滤波方程包括状态方程 

***噪声向量 $W={\left[\begin{array}{ccccc}{w}_{\mathrm{gx}}^b& w_{\mathrm{gy}}^b& w_{\mathrm{gz}}^b& w_{\mathrm{ax}}^b& w_{\mathrm{ay}}^b\end{array}\right]}^T$

系数阵 $G=\left[\begin{array}{cc}{G}_1& 0_{3\×2}\\ 0_{2\×3}& G_2\end{array}\right],G_1=\left[\begin{array}{ccc}{G}_{11}^{\′}& G_{12}^{\′}& C_{13}^{\′}\\ G_{21}^{\′}& C_{22}^{\′}& C_{23}^{\′}\\ C_{31}^{\′}& C_{32}^{\′}& C_{33}^{\′}\end{array}\right]G_2=\left[\begin{array}{cc}{C}_{11}^{\′}& C_{12}^{\′}\\ C_{21}^{\′}& C_{22}^{\′}\end{array}\right],$

f(X)为 

${\stackrel{\·}{\mathrm{\φ}}}_x=-\left(\sin {\mathrm{\φ}}_z\right){\mathrm{\ω}}_{\mathrm{ie}}\cos L+{\mathrm{\φ}}_y{\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\δ}{v}_y}{R_M}+C_{11}^{\′}({\mathrm{\ϵ}}_x^b+w_{\mathrm{gx}}^b)+C_{12}^{\′}({\mathrm{\ϵ}}_y^b+w_{\mathrm{gy}}^b)+C_{13}^{\′}({\mathrm{\ϵ}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{\·}{\mathrm{\φ}}}_y=(1-\cos {\mathrm{\φ}}_z){\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\φ}}_x{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{\mathrm{\δ}{v}_x}{R_N}+C_{21}^{\′}({\mathrm{\ϵ}}_x^b+w_{\mathrm{gx}}^b)+C_{22}^{\′}({\mathrm{\ϵ}}_y^b+w_{\mathrm{gy}}^b)+C_{23}^{\′}({\mathrm{\ϵ}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{\·}{\mathrm{\φ}}}_z=(-{\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z){\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\δ}{v}_x\tan L}{R_N}+C_{31}^{\′}({\mathrm{\ϵ}}_x^b+w_{\mathrm{gx}}^b)+C_{32}^{\′}({\mathrm{\ϵ}}_y^b+w_{\mathrm{gy}}^b)+C_{33}^{\′}({\mathrm{\ϵ}}_z^b+w_{\mathrm{gz}}^b)$

$\begin{array}{c}\mathrm{\δ}{\stackrel{.}{v}}_x=-f_x(\cos {\mathrm{\φ}}_z-1)+f_y\sin {\mathrm{\φ}}_z-f_z({\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z)+2{\mathrm{\ω}}_{\mathrm{ie}}{\mathrm{\δv}}_y\sin L+\\ C_{11}^{\′}({\▿}_x^b+w_{\mathrm{ax}}^b)+C_{12}^{\′}({\▿}_y^b+w_{\mathrm{ay}}^b)\end{array}$

$\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_y=-f_x\sin {\mathrm{\φ}}_z-f_y(\cos {\mathrm{\φ}}_z-1)-f_z({\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z)-2{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\δ}{v}_x\sin L+\\ C_{21}^{\′}({\▿}_x^b+w_{\mathrm{ax}}^b)+C_{22}^{\′}({\▿}_y^b+w_{\mathrm{ay}}^b)\end{array}$

其中，R_M,R_N分别为地球子午、卯酉曲率半径，φ_x,φ_y,φ_z为三个方向的平台失准角；δv_x,δv_y为速度误差；L为当地纬度；w_ie为地球自转角速度；为三个轴向的陀螺漂移； 为陀螺零均值高斯白噪声；为三个轴向的加速度零偏；为加速度计零均值高斯白噪声；f_x,f_y,f_z为加速度输出比力在计算地理坐标系上的值；C′_ij是载体系到计算地理系矩阵中的元素； 

量测方程为： 

Z＝HX+V 

其中，量测量Z为惯导水平速度误差δv_x,δv_y；H＝[I_2×2 0_2×3]，I_2×2为单位二阶矩阵，0_2×3为2行3列零矩阵；V为量测噪声。 

本发明的有益效果在于：本发明建立的***状态空间模型过程中，失准角和速度误差模型均采用非线性形式，这样可以准确的描述实际的捷联惯导***误差传播特性；传统的惯性导航解算时通常用的是正常重力值，而实际由于地球内部质量分布不均匀，重力矢量包括正常重力和重力扰动。由于不同地区地球内部质量分布不同，这些重力扰动在地表或者海平面上是变化的。惯性加速度计不能区分重力扰动和机体的加速度，重力扰动的存在会导致一定 的初始姿态误差，此误差会通过姿态解算、速度解算及位置解算影响***各项指标的精度，这是一个不利的因素。随着惯性器件精度不断提高与高精度惯性导航***需求的发展,重力扰动成为高精度惯导初始对准的一项主要误差源。本发明中捷联惯导初始对准中重力扰动的补偿方法是，根据已知的经纬度值并通过EGM2008模型计算出对准点的重力扰动值，然后算出的重力扰动值从加速度计中剔除掉，就得到重力扰动补偿后加速度计的输出，仿真结果表明重力扰动补偿后可提高初始对准精度。 

附图说明

图1表示本发明的流程图。 

图2表示存在重力扰动时失准角误差曲线图 

图3表示补偿重力扰动后失准角误差曲线图。 

具体实施方式

下面结合附图对本发明做进一步描述。 

1、采集光纤陀螺仪和石英加速度计输出的数据； 

2、计算对准地点的重力扰动值，然后对加速度计的输出进行补偿。地球重力场模型就是用一个截断到N阶的引力位球谐函数级数式来表示地球引力场，T(ρ,L,λ)为扰动位，地球平均半径为R，θ＝90-L是地心余纬度。 

$T(\mathrm{\ρ},L,\mathrm{\λ})=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{N}{\mathrm{\Σ}}}{\left(\frac{R}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[C_{\mathrm{nm}}\cos \mathrm{m\λ}+S_{\mathrm{nm}}\sin \mathrm{m\λ}]P_{\mathrm{nm}}\left(\cos \mathrm{\θ}\right)---\left(1\right)$

式中：位系数C_nm,S_nm是已知的重力扰动的系数，可通过EGM2008重力场模型获得，L表示地球纬度，λ为地球经度，ρ为计算点处的地心向径，GM为地球常系数，P_nm(·)为缔合勒让德多项式，定义式如下： 

$P_{\mathrm{nm}}\left(x\right)={(1-x^2)}^{m/2}\underset{k=0}{\overset{N}{\mathrm{\Σ}}}\frac{{(-1)}^k(2n-2k)!}{2^{n}k!(n-k)!(n-m-2k)!}{x}^{n-m-2k}---\left(2\right)$

式中：式中：x为变量，|x|＜1；n称为阶，m称为次，N＝[(n-m)2]。 

结合缔合勒让德多项式，根据式(1)可获得地球重力扰动位模型T(ρ,L,λ)，该模型的梯度即为重力扰动矢量δgⁿ： 

$\mathrm{\δ}{g}^n=\mathrm{grad}\mathrm{dT}(\mathrm{\ρ},L,\mathrm{\λ})={(\frac{\∂T}{\∂\mathrm{\ρ}},\frac{\∂T}{\∂L},\frac{\∂T}{\∂\mathrm{\λ}})}^T---\left(3\right)$

由(3)式得极坐标系下的重力扰动值： 

$T_{\mathrm{\ρ}}=\frac{\∂T}{\∂\mathrm{\ρ}}=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}(n+1){\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[{\stackrel{\‾}{C}}_{\mathrm{nm}}\cos \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\sin \mathrm{m\λ}]{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)$

$T_L=\frac{\∂T}{\∂L}=-\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}{\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[{\stackrel{\‾}{C}}_{\mathrm{nm}}\cos \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\sin \mathrm{m\λ}]\frac{d{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)}{\mathrm{dL}}---\left(4\right)$

$T_{\mathrm{\λ}}=\frac{\∂T}{\∂\mathrm{\λ}}=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}{\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[-{\stackrel{\‾}{C}}_{\mathrm{nm}}\sin \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\cos \mathrm{m\λ}]{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)$

在东、北、天地理坐标系上，重力扰动矢量3个轴向的分量为由于式(4)为极坐标变换表示形式，将其转换到地理坐标系，有如下关系式： 

${\mathrm{\δg}}_E^n=\frac{T_{\mathrm{\λ}}}{\mathrm{\ρ}\cos L},{\mathrm{\δg}}_N^n=\frac{T_L}{\mathrm{\ρ}},\mathrm{\δ}{g}_U^n=T_{\mathrm{\ρ}}---\left(5\right)$

根据已知的经纬度值并利用式(4)、(5)可计算出对准地点的重力扰动值，然后对加速度计的输出进行补偿； 

3、采用解析法来完成粗对准，初步确定载体的姿态信息

式中分别是纵摇角，横摇角，航向角； 

4、粗对准结束后通常水平失准角是小失准角，方位失准角是大失准角，因此建立大方位失准角非线性滤波方程； 

5、滤波初始化； 

6、利用UKF滤波方法进行滤波估计出失准角； 

7、利用估计出来的平台失准角修正***的捷联初始矩阵得到精确的初始捷联矩阵即从而完成精确的初始对准。 

实施例1 

1、采集光纤陀螺仪和石英加速度计输出的数据； 

2、计算对准地点的重力扰动值，然后对加速度计的输出进行补偿。地球重力场模型就是用一个截断到N阶的引力位球谐函数级数式来表示地球引力场，T(ρ,L,λ)为扰动位，地球平均半径为R，θ＝90-L是地心余纬度。 

$T(\mathrm{\ρ},L,\mathrm{\λ})=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{N}{\mathrm{\Σ}}}{\left(\frac{R}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[C_{\mathrm{nm}}\cos \mathrm{m\λ}+S_{\mathrm{nm}}\sin \mathrm{m\λ}]P_{\mathrm{nm}}\left(\cos \mathrm{\θ}\right)---\left(1\right)$

式中：位系数C_nm,S_nm是已知的重力扰动的系数，可通过EGM2008重力场模型获得，L表示地球纬度，λ为地球经度，ρ为计算点处的地心向径，GM为地球常系数，P_nm(·)为缔合勒 让德多项式，定义式如下： 

$P_{\mathrm{nm}}\left(x\right)={(1-x^2)}^{m/2}\underset{k=0}{\overset{N}{\mathrm{\Σ}}}\frac{{(-1)}^k(2n-2k)!}{2^{n}k!(n-k)!(n-m-2k)!}{x}^{n-m-2k}---\left(2\right)$

式中：式中：x为变量，|x|＜1；n称为阶，m称为次，N＝[(n-m)2]。 

结合缔合勒让德多项式，根据式(1)可获得地球重力扰动位模型T(ρ,L,λ)，该模型的梯度即为重力扰动矢量δgⁿ： 

$\mathrm{\δ}{g}^n=\mathrm{grad}\mathrm{dT}(\mathrm{\ρ},L,\mathrm{\λ})={(\frac{\∂T}{\∂\mathrm{\ρ}},\frac{\∂T}{\∂L},\frac{\∂T}{\∂\mathrm{\λ}})}^T---\left(3\right)$

由(3)式得极坐标系下的重力扰动值： 

$T_{\mathrm{\ρ}}=\frac{\∂T}{\∂\mathrm{\ρ}}=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}(n+1){\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[{\stackrel{\‾}{C}}_{\mathrm{nm}}\cos \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\sin \mathrm{m\λ}]{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)$

$T_L=\frac{\∂T}{\∂L}=-\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}{\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[{\stackrel{\‾}{C}}_{\mathrm{nm}}\cos \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\sin \mathrm{m\λ}]\frac{d{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)}{\mathrm{dL}}---\left(4\right)$

$T_{\mathrm{\λ}}=\frac{\∂T}{\∂\mathrm{\λ}}=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}{\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[-{\stackrel{\‾}{C}}_{\mathrm{nm}}\sin \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\cos \mathrm{m\λ}]{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)$

在东、北、天地理坐标系上，重力扰动矢量3个轴向的分量为由于式(4)为极坐标变换表示形式，将其转换到地理坐标系，有如下关系式： 

${\mathrm{\δg}}_E^n=\frac{T_{\mathrm{\λ}}}{\mathrm{\ρ}\cos L},{\mathrm{\δg}}_N^n=\frac{T_L}{\mathrm{\ρ}},\mathrm{\δ}{g}_U^n=T_{\mathrm{\ρ}}---\left(5\right)$

根据已知的经纬度值并利用式(4)、(5)可计算出对准地点的重力扰动值，然后对加速度计的输出进行补偿； 

3、采用解析法来完成***的粗对准，初步确定载体的姿态信息

式中分别是纵摇角，横摇角，航向角； 

4、粗对准结束后通常水平失准角是小失准角，方位失准角是大失准角，因此建立捷联惯性导航***初始对准非线性状态方程***噪声向量  $W={\left[\begin{array}{ccccc}{w}_{\mathrm{gx}}^b& w_{\mathrm{gy}}^b& w_{\mathrm{gz}}^b& w_{\mathrm{ax}}^b& w_{\mathrm{ay}}^b\end{array}\right]}^T$

系数阵 $G=\left[\begin{array}{cc}{G}_1& 0_{3\×2}\\ 0_{2\×3}& G_2\end{array}\right],G_1=\left[\begin{array}{ccc}{G}_{11}^{\′}& G_{12}^{\′}& C_{13}^{\′}\\ G_{21}^{\′}& C_{22}^{\′}& C_{23}^{\′}\\ C_{31}^{\′}& C_{32}^{\′}& C_{33}^{\′}\end{array}\right]G_2=\left[\begin{array}{cc}{C}_{11}^{\′}& C_{12}^{\′}\\ C_{21}^{\′}& C_{22}^{\′}\end{array}\right],$ f(X)为 

${\stackrel{\·}{\mathrm{\φ}}}_x=-\left(\sin {\mathrm{\φ}}_z\right){\mathrm{\ω}}_{\mathrm{ie}}\cos L+{\mathrm{\φ}}_y{\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\δ}{v}_y}{R_M}+C_{11}^{\′}({\mathrm{\ϵ}}_x^b+w_{\mathrm{gx}}^b)+C_{12}^{\′}({\mathrm{\ϵ}}_y^b+w_{\mathrm{gy}}^b)+C_{13}^{\′}({\mathrm{\ϵ}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{\·}{\mathrm{\φ}}}_y=(1-\cos {\mathrm{\φ}}_z){\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\φ}}_x{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{\mathrm{\δ}{v}_x}{R_N}+C_{21}^{\′}({\mathrm{\ϵ}}_x^b+w_{\mathrm{gx}}^b)+C_{22}^{\′}({\mathrm{\ϵ}}_y^b+w_{\mathrm{gy}}^b)+C_{23}^{\′}({\mathrm{\ϵ}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{\·}{\mathrm{\φ}}}_z=(-{\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z){\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\δ}{v}_x\tan L}{R_N}+C_{31}^{\′}({\mathrm{\ϵ}}_x^b+w_{\mathrm{gx}}^b)+C_{32}^{\′}({\mathrm{\ϵ}}_y^b+w_{\mathrm{gy}}^b)+C_{33}^{\′}({\mathrm{\ϵ}}_z^b+w_{\mathrm{gz}}^b)$

$\begin{array}{c}\mathrm{\δ}{\stackrel{.}{v}}_x=-f_x(\cos {\mathrm{\φ}}_z-1)+f_y\sin {\mathrm{\φ}}_z-f_z({\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z)+2{\mathrm{\ω}}_{\mathrm{ie}}{\mathrm{\δv}}_y\sin L+\\ C_{11}^{\′}({\▿}_x^b+w_{\mathrm{ax}}^b)+C_{12}^{\′}({\▿}_y^b+w_{\mathrm{ay}}^b)\end{array}$

$\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_y=-f_x\sin {\mathrm{\φ}}_z-f_y(\cos {\mathrm{\φ}}_z-1)-f_z({\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z)-2{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\δ}{v}_x\sin L+\\ C_{21}^{\′}({\▿}_x^b+w_{\mathrm{ax}}^b)+C_{22}^{\′}({\▿}_y^b+w_{\mathrm{ay}}^b)\end{array}$

其中，R_M,R_N分别为地球子午、卯酉曲率半径，φ_x,φ_y,φ_z为三个方向的失准角(状态量)；δv_x,δv_y为速度误差(状态量)；L为当地纬度；w_ie为地球自转角速度；为三个轴向的陀螺漂移；为陀螺零均值高斯白噪声；为三个轴向的加速度零偏； 为加速度计零均值高斯白噪声；f_x,f_y,f_z为加速度输出比力在计算地理坐标系上的值；C′_ij是载体系到计算地理系矩阵中的元素； 

初始对准量测方程为： 

Z＝HX+V 

其中，量测量Z为惯导水平速度误差δv_x,δv_y；H＝[I_2×2 0_2×3](I_2×2为单位二位矩阵，0_2×3为2行3列零矩阵)；V为量测噪声； 

5、选取滤波初始值 

${\hat{x}}_0=E\left[x_0\right],P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]$

其中x₀为状态变量的初值,P₀为状态变量的初始误差协方差矩阵。 

6、利用UKF方法进行滤波估计出失准角，具体如下； 

(1)构造2n+1维的Sigma点 

$\left\{\begin{array}{c}{\mathrm{\ξ}}_0={\hat{x}}_{k-1}\\ {\mathrm{\ξ}}_i={\hat{x}}_{k-1}+\sqrt{(m+\mathrm{\κ})P_{k-1}},\\ {\mathrm{\ξ}}_{i+n}={\hat{x}}_{k-1}-\sqrt{(m+\mathrm{\κ})P_{k-1}}\end{array}\right.,i=\mathrm{1,2},...,n$

其中为k-1时刻的状态估计，P_k-1为k-1时刻的状态误差协方差矩阵，n为***状态变量的维数，κ为比例系数，可用于调节Sigma点和的距离。 

(2)确定权值 

$w_i^m=w_i^c=\left\{\begin{array}{cc}\mathrm{\κ}/(m+\mathrm{\κ})& (i=0)\\ 1/\left[2(m+\mathrm{\κ})\right]& (i\≠0)\end{array}\right.$

(3)时间更新 

γ_k/k-1＝f_k-1(ξ_k-1) 

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i{\mathrm{\γ}}_{i,k/k-1}$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i({\mathrm{\γ}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\γ}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1}$

按照第(2)步所选择的Sigma采样策略，由和P_k-1计算Sigma点ξ_k-1，通过非线性状态函数f_k-1(·)传播为γ_k/k-1，由γ_k/k-1可得状态预测和预测误差协方差阵P_k/k-1，Q_k-1为***噪声。 

(4)量测更新 

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H{\hat{x}}_{k/k-1})$

P_k＝(I-K_kH)P_k/k-1

其中R_k为量测噪声，K_k为滤波增益，P_k为估计误差协方差矩阵 

7、利用估计出来的平台失准角修正***的捷联初始矩阵得到精确的初始捷联矩阵即从而完成精确的初始对准。 

8、对本发明中的方法进行matlab仿真验证： 

载体初始位置：北纬45.7996°，东经126.6705°，地球半径R＝6378393m，陀螺常值漂移0.001°/h，随机游走系数为0.0002°/h；初始失准φ_x＝1°,φ_y＝1°,φ_z＝10°，加速度计常值偏置10μg，零偏白噪声为5μg，惯性传感器数据产生周期为0.01s，仿真中利用提供的2.5'×2.5'分辨率的重力场模型来模拟对准点的实际重力，并利用分辨率为5'×5'的重力场模型计算来补偿重力扰动。根据所设置的参数，首先存在重力扰动的情况下进行初始对准仿真得到图2失准角误差曲线，然后利用本发明的重力扰动补偿方法可得到图3所示的东向失准角误差δφ_E,北向失准角误差δφ_N,天向失准角误差δφ_U曲线。从图2和图3的对比可以看出重力扰动补偿后可提高初始对准精度。 

Claims (6)

1.一种捷联惯导初始对准中重力扰动的补偿方法，其特征在于：

(1)采集光纤陀螺仪输出的角速度数据和石英加速度计输出的加速度数据；

(2)通过重力扰动位计算对准地点的重力扰动值，对石英加速度计的输出加速度数据进行补偿；

(3)采用解析法来完成粗对准，通过初始矩阵确定载体的姿态信息即纵摇角、横摇角、航向角，其中b代表载体坐标系，n′代表计算导航坐标系；

(4)选取滤波初始值

${\hat{x}}_0=E\left[x_0\right],P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right].$

其中x₀为状态变量的初值,P₀为状态变量的初始误差协方差矩阵；

(5)利用无迹卡尔曼滤波方法进行滤波，估计出平台失准角；

(6)利用估计出来的平台失准角修正***的捷联初始矩阵得到精确的初始捷联矩阵即从而完成精确的初始对准。

2.根据权利要求1所述的一种捷联惯导初始对准中重力扰动的补偿方法，其特征在于：所述的重力扰动位T(ρ,L,λ)，

$T(\mathrm{\ρ},L,\mathrm{\λ})=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{N}{\mathrm{\Σ}}}{\left(\frac{R}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[C_{\mathrm{nm}}\cos \mathrm{m\λ}+S_{\mathrm{nm}}\sin \mathrm{m\λ}]P_{\mathrm{nm}}\left(\cos \mathrm{\θ}\right),$

地球平均半径为R，θ＝90-L是地心余纬度，位系数C_nm,S_nm是已知的重力扰动的系数，可通过EGM2008重力场模型获得，L表示地球纬度，λ为地球经度，ρ为对准地点处的地心向径，GM为地球常系数，P_nm(·)为缔合勒让德多项式：

$P_{\mathrm{nm}}\left(x\right)={(1-x^2)}^{m/2}\underset{k=0}{\overset{N}{\mathrm{\Σ}}}\frac{{(-1)}^k(2n-2k)!}{2^{n}k!(n-k)!(n-m-2k)!}{x}^{n-m-2k},$

x为变量，|x|＜1；n称为阶，m称为次，N＝[(n-m)/2]；

重力扰动位的梯度即为重力扰动矢量δgⁿ：

$\mathrm{\δ}{g}^n=\mathrm{grad}\mathrm{dT}(\mathrm{\ρ},L,\mathrm{\λ})={(\frac{\∂T}{\∂\mathrm{\ρ}},\frac{\∂T}{\∂L},\frac{\∂T}{\∂\mathrm{\λ}})}^T,$

通过重力扰动矢量得到极坐标系下的重力扰动值：

$T_{\mathrm{\ρ}}=\frac{\∂T}{\∂\mathrm{\ρ}}=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}(n+1){\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[{\stackrel{\‾}{C}}_{\mathrm{nm}}\cos \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\sin \mathrm{m\λ}]{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)$

$T_L=\frac{\∂T}{\∂L}=-\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}{\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[{\stackrel{\‾}{C}}_{\mathrm{nm}}\cos \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\sin \mathrm{m\λ}]\frac{d{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)}{\mathrm{dL}}$

$T_{\mathrm{\λ}}=\frac{\∂T}{\∂\mathrm{\λ}}=\frac{\mathrm{GM}}{\mathrm{\ρ}}\underset{n=2}{\overset{\∞}{\mathrm{\Σ}}}{\left(\frac{a}{\mathrm{\ρ}}\right)}^n\underset{m=0}{\overset{n}{\mathrm{\Σ}}}[-{\stackrel{\‾}{C}}_{\mathrm{nm}}\sin \mathrm{m\λ}+{\stackrel{\‾}{S}}_{\mathrm{nm}}\cos \mathrm{m\λ}]{\stackrel{\‾}{P}}_{\mathrm{nm}}\left(\sin L\right)$

在东北天-ENU地理坐标系上，重力扰动矢量3个轴向的分量为转换到地理坐标系为：

$\mathrm{\δ}{g}_E^n=\frac{T_{\mathrm{\λ}}}{\mathrm{\ρ}\cos L},\mathrm{\δ}{g}_N^n=\frac{T_L}{\mathrm{\ρ}},\mathrm{\δ}{g}_U^n=T_{\mathrm{\ρ}},$

然后对加速度计的输出进行补偿。

3.根据权利要求1所述的一种捷联惯导初始对准中重力扰动的补偿方法，其特征在于：所述的初始矩阵：

θ₀是纵摇角，γ₀是横摇角，是航向角。

4.根据权利要求1所述的一种捷联惯导初始对准中重力扰动的补偿方法，其特征在于：所述的状态变量是平台失准角、水平速度误差。

5.根据权利要求1所述的一种捷联惯导初始对准中重力扰动的补偿方法，其特征在于：所述的步骤(5)包括：

(5.1)构造2n+1维的Sigma点

$\left\{\begin{array}{c}{\mathrm{\ξ}}_0={\hat{x}}_{k-1}\\ {\mathrm{\ξ}}_i={\hat{x}}_{k-1}+\sqrt{(m+\mathrm{\κ})P_{k-1}},i=\mathrm{1,2},...,m\\ {\mathrm{\ξ}}_{i+n}={\hat{x}}_{k-1}-\sqrt{(m+\mathrm{\κ})P_{k-1}}\end{array}\right.$

其中为k-1时刻的状态估计，P_k-1为k-1时刻的状态误差协方差矩阵，m为***状态变量的维数，κ为比例系数，可用于调节Sigma点和的距离；

(5.2)确定权值

$w_i^m=w_i=\left\{\begin{array}{cc}\mathrm{\κ}/(m+\mathrm{\κ})& (i=0)\\ 1/\left[2(m+\mathrm{\κ})\right]& (i\≠0)\end{array}\right.$

(5.3)通过滤波方程进行时间更新

γ_k/k-1＝f_k-1(ξ_k-1)

${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i{\mathrm{\γ}}_{i,k/k-1}$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i({\mathrm{\γ}}_{i,k/k-1}-{\hat{x}}_{k/k-1}){({\mathrm{\γ}}_{i,k/k-1}-{\hat{x}}_{k/k-1})}^T+Q_{k-1}$

由和P_k-1计算Sigma点ξ_k-1，通过非线性状态函数f_k-1(·)传播为γ_k/k-1，由γ_k/k-1可得状态预测和预测误差协方差阵P_k/k-1，Q_k-1为***噪声；

(5.4)通过滤波方程进行量测更新

K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H{\hat{x}}_{k/k-1})$

P_k＝(I-K_kH)P_k/k-1

其中R_k为量测噪声，K_k为滤波增益，P_k为估计误差协方差矩阵，为状态估计值其中包括平台失准角。

6.根据权利要求5所述的一种捷联惯导初始对准中重力扰动的补偿方法，其特征在于：所述滤波方程包括状态方程

$\stackrel{.}{X=}f\left(X\right)+\mathrm{GW},$ ***噪声向量 $W={\left[\begin{array}{ccccc}{w}_{\mathrm{gx}}^b& w_{\mathrm{gy}}^b& w_{\mathrm{gz}}^b& w_{\mathrm{ax}}^b& w_{\mathrm{ay}}^b\end{array}\right]}^T$

系数阵 $G=\left[\begin{array}{cc}{G}_1& 0_{3\×2}\\ 0_{2\×3}& G_2\end{array}\right],G_1=\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]G_2=\left[\begin{array}{cc}{G}_{11}^{\′}& G_{12}^{\′}\\ C_{21}^{\′}& C_{22}^{\′}\end{array}\right],$

f(X)为

${\stackrel{.}{\mathrm{\φ}}}_x=-\left(\sin {\mathrm{\φ}}_z\right){\mathrm{\ω}}_{\mathrm{ie}}\cos L+{\mathrm{\φ}}_y{\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\δ}{v}_y}{R_M}+C_{11}^{\′}({\mathrm{\ϵ}}_x^b+w_{\mathrm{gx}}^b)+C_{12}^{\′}({\mathrm{\ϵ}}_y^b+w_{\mathrm{gy}}^b)+C_{13}^{\′}({\mathrm{\ϵ}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\φ}}}_y=(1-\cos {\mathrm{\φ}}_z){\mathrm{\ω}}_{\mathrm{ie}}\cos L+{\mathrm{\φ}}_x{\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{\mathrm{\δ}{v}_x}{R_N}+C_{21}^{\′}({\mathrm{\ϵ}}_x^b+w_{\mathrm{gx}}^b)+C_{22}^{\′}({\mathrm{\ϵ}}_y^b+w_{\mathrm{gy}}^b)+C_{23}^{\′}({\mathrm{\ϵ}}_z^b+w_{\mathrm{gz}}^b)$

${\stackrel{.}{\mathrm{\φ}}}_z=(-{\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z){\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\δ}{v}_x\tan L}{R_N}+C_{31}^{\′}({\mathrm{\ϵ}}_x^b+w_{\mathrm{gx}}^b)+C_{32}^{\′}({\mathrm{\ϵ}}_y^b+w_{\mathrm{gy}}^b)+C_{33}^{\′}({\mathrm{\ϵ}}_z^b+w_{\mathrm{gz}}^b)$

$\begin{array}{c}\mathrm{\δ}{\stackrel{.}{v}}_x=-f_x(\cos {\mathrm{\φ}}_z-1)+f_y\sin {\mathrm{\φ}}_z-f_z({\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z)+2{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\δ}{v}_y\sin L+\\ C_{11}^{\′}({\▿}_x^b+w_{\mathrm{ax}}^b)+C_{12}^{\′}({\▿}_y^b+w_{\mathrm{ay}}^b)\end{array}$

$\begin{array}{c}\mathrm{\δ}{\stackrel{.}{v}}_y=-f_x\sin {\mathrm{\φ}}_z-f_y(\cos {\mathrm{\φ}}_z-1)f_z({\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z)-2{\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\δ}{v}_x\sin L+\\ C_{21}^{\′}({\▿}_x^b+w_{\mathrm{ax}}^b)+C_{22}^{\′}({\▿}_y^b+w_{\mathrm{ay}}^b)\end{array}$

其中，R_M,R_N分别为地球子午、卯酉曲率半径，φ_x,φ_y,φ_z为三个方向的平台失准角；δv_x,δv_y为速度误差；L为当地纬度；w_ie为地球自转角速度；为三个轴向的陀螺漂移；为陀螺零均值高斯白噪声；为三个轴向的加速度零偏；为加速度计零均值高斯白噪声；f_x,f_y,f_z为加速度输出比力在计算地理坐标系上的值；C′_ij是载体系到计算地理系矩阵中的元素；

量测方程为：

Z＝HX+V

其中，量测量Z为惯导水平速度误差δv_x,δv_y；H＝[I_2×2 0_2×3]，I_2×2为单位二阶矩阵，0_2×3为2行3列零矩阵；V为量测噪声。