本发明公开了一种基于静止修正的垂线偏差测量方法，包括建立组合系统垂线偏差观测模型的姿态位置速度方程和测量方程；建立静止修正垂线偏差观测模型；判断载体是否匀速，若载体匀速，则将位置差值和航向角修正的测量值作为测量方程对垂线偏差方程进行改正；否则，将速度、位置差值及航向角修正的测量值作为观测量对垂线偏差方程进行改正；基于改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量进行静止修正。本发明不仅使用速度、位置信息作为观测量，同时添加零角度修正观测量来限制航向角误差，通过更新角速度传感器的常值零偏来减少航向角漂移，解决了组合系统垂线偏差测量中天向姿态误差补偿不到位和航向角误差随时间积累增大的问题。
1.一种基于静止修正的垂线偏差测量方法，其特征在于，包括以下步骤：(1)建立惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程和测量方程；
(2)建立惯导与全球定位组合系统静止修正垂线偏差观测模型；具体的：静止修正垂线偏差观测模型的建立方法如下：
(21)设置惯导与全球定位组合系统的初始参数，包括初始姿态信息，初始位置信息和初始速度信息；
(22)惯性系统初始校准，包括水平方向校准和方位校准，使平台坐标系与地理坐标系趋于一致；
(23)对惯性系统进行捷联解算，根据加速度传感器和角速度传感器信号，解算出惯性系统的航向，姿态，位置及速度；
(24)判断是否到达静止修正时间，若是，则转到步骤(25)，若否，则转到步骤(23)；其中，静止修正时间设定如下：其中，t0为零点修正时间，δa为设置最大零漂误差，ε0为惯导与全球定位组合系统的零偏精度；
(251)对载体进行静止检测，根据惯性器件角速度传感器和加速度传感器三轴输出以及变化规律判断载体是否静止，并通过改变惯导与全球定位组合系统垂线偏差观测模型的测量方程来对载体静止时的误差进行修正；
(252)对载体进行静止修正，当载体静止时，两个历元间航向角的变化为零，并通过更新角速度传感器的常值零偏来减少航向角漂移，具体方法如下：真实航向角的变化率 表达式为：
其中，Ψ、φ、θ分别表示载体的真实航向角、真实横滚角和真实俯仰角； 为角速度传感器的真实输出角速率； 为载体静止时地球自转在导航坐标系中的角速率；
其中， 分别表示载体的测量航向角、测量横滚角和测量俯仰角； 为角速度传感器的测量输出角速率； 为载体静止时地球自转在导航坐标系中的角速率；
其中，bg表示角速度传感器的零偏矢量；ng表示角速度传感器的随机噪声； 为角速度传感器的测量输出角速率； 为角速度传感器的真实输出角速率； 为角速度传感器相对导航坐标系的角速率；
设俯仰角、横滚角为恒定值，令 δΨ表示航向角的误差，在运算过程中作近似处理，cosδψ＝1sinδψ＝δψ；则将测量航向角的变化率 与真实航向角的变化率 相减得：其中， 为航向角变化率误差；
去掉第一项地球的自转误差，静止修正的垂线偏差观测模型得：其中，△tk为航向角更新的时间，k表示迭代次数；
(26)判断是否到达预先设定的迭代时间ti，若是，则转到步骤(27)，若否，则转到步骤(25)；
(27)判断是否到达预先设定的解算结束时间ts，若是，则转到步骤(28)，若否，则转到步骤(23)；
(3)判断载体是否匀速，若载体匀速，则执行步骤(4)；否则，执行步骤(5)；
(4)将位置差值和航向角修正的测量值作为观测量对垂线偏差方程进行改正；
(5)将速度、位置差值以及航向角修正的测量值作为观测量对垂线偏差方程进行改正；
(6)基于步骤(4)或步骤(5)中改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量进行真值估计。
2.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(1)中惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程建立方法如下：惯导与全球定位组合系统的姿态位置速度方程的线性化形式表示为：其中， 为惯导与全球定位组合系统垂线偏差中系统的线性化状态向量，包括当地导航坐标系下的姿态线性化误差矢量 速度线性化误差矢量 位置线性化误差角速度传感器线性化误差矢量 和加速度传感器线性化误差矢量 X(t)为惯导与全球定位组合系统垂线偏差中的状态向量，包括当地导航坐标系下的姿态误差矢量φn、速度误差矢量δvn、位置误差δrn、角速度传感器误差矢量 和加速度传感器误差矢量 F(t)为动态系数矩阵，包括惯导与全球定位组合系统的位置、速度、姿态，其中，F11～F33分别表示F(t)内部的动态系数，导航坐标系与载体坐标系之间的转置矩阵为 W(t)为惯导与全球定位组合系统的噪声向量，包括角速度传感器噪声向量 和加速度传感器噪声向量 G(t)为惯导与全球定位组合系统的噪声系数矩阵。
3.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(1)中惯导与全球定位组合系统垂线偏差观测模型的测量方程建立方法如下：(a)在当地导航系统中，惯导与全球定位组合系统垂线偏差观测模型的测量方程是惯导系统和全球定位系统的位置、速度的差值，其测量方程Z(t)表示为：其中， 是惯性系统的位置， 是全球定位系统的位置， 是惯性系统的速度， 是全球定位系统的速度， 是位置的随机噪声向量， 是速度的随机噪声向量，φn是姿态误差矢量，δvn是速度误差矢量，δrn是位置误差， 是角速度传感器误差矢量， 是加速度传感器误差矢量；
(b)全球定位系统给出的位置信息为经度λ、纬度 和高度h，将其转换为当地导航坐标系中量测方程中的位置之差为：其中，RM为子午圆半径；RN为卯酉圈半径；λI是惯性系统的经度，λG是全球定位系统的经度， 是惯性系统的纬度， 是全球定位系统的纬度。
4.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(4)中将位置差值和航向角修正的测量值作为测量方程对静止修正垂线偏差观测模型进行改正，在静止观测的基础上添加了航向角修正测量值，建立改正的测量方程Z1(t)，并用Z1(t)代替Z(t)：具体测量方程如下所示：其中， 为惯性系统相对导航坐标系的位置矢量； 为全球定位系统相对导航坐标系的位置矢量； 为第k次迭代的真实航向角的变化率； 为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差， 为角速度传感器的零偏， 为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角；
表示全球定位系统的位置随机噪声；wψ为航向角观测的随机噪声。
5.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(5)中将速度、位置差值以及航向角修正的测量值作为观测量对静止修正垂线偏差观测模型进行改正，建立改正的测量方程Z2(t)，并用Z2(t)代替Z(t)：其中， 为惯性系统相对导航坐标系的位置矢量； 为全球定位系统相对导航坐标系的位置矢量； 为惯性系统相对导航坐标系的速度矢量； 为全球定位系统相对导航坐标系的速度矢量； 为第k次迭代的真实航向角的变化率； 为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差， 为角速度传感器的零偏， 为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角； 和 分别表示全球定位系统的位置和速度的随机噪声；wψ为航向角观测的随机噪声。
6.根据权利要求1所述的一种基于静止修正的垂线偏差测量方法，其特征在于，步骤(6)中基于改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量的真值估计方案如下：(61)惯性系统输出状态变量真值和系统误差值，输出值Xs(t)用下列姿态位置速度方程描述：Xs(t)＝X(t)+G(t)W(t)；
其中，G(t)为惯性系统的噪声系数矩阵，W(t)表示惯性系统的噪声向量，X1(t)为惯性导航系统的误差状态向量，X2(t)为传感器误差的增广状态向量，如下所示：X1(t)＝[φE,φN,φU,δVE,δVN,δVU,δL,δλ,δh]T；
其中，φE、φN、φU分别为东向、北向和天向的平台误差角，δVE、δVN、δVU分别为东向、北向和天向的速度误差，δL、δλ、δh分别为纬度、经度和高度误差；εbE、εbN、εbU分别为东向、北向和天向的角速度传感器随机常数误差，εrE、εrN、εrU分别为东向、北向和天向的角速度传感器的一阶马尔可夫过程误差， 分别为东向、北向和天向的加速度传感器的一阶马尔可夫过程误差；
其中， 为角速度传感器噪声向量和 为加速度传感器噪声向量，导航坐标系与载体坐标系之间的转置矩阵为
(62)将惯性系统输出状态变量值Xs(t)，结合静止修正测量噪声 输入递归滤波器进行递归静止修正解算，去除惯性系统噪声，并输出惯性系统的状态变量真值估计。
一种基于静止修正的垂线偏差测量方法
技术领域
[0001] 本发明涉及惯导与全球定位组合系统垂线偏差测量技术，特别是涉及一种基于静止修正的垂线偏差测量方法。
背景技术
[0002] 将全球定位系统与惯性系统结合起来应用信息融合理论形成最优组合测量系统已经成为新型测量技术的发展方向。它们之间具备良好的互补特性，可以获得稳定、精度好、数据更新率高的三维位置、速度、姿态信息。在垂线偏差组合测量系统中通常以全球定位系统输出位置、速度测量值并通过递归滤波等算法来限制惯性系统误差的累积，但由于全球定位系统输出的位置、速度测量值含有误差。静止修正方法的原理是利用惯性测量系统在载体停车时的速度输出作为惯性导航系统速度误差的观测量，在此基础上纠正其他误差，还可用载体运动时垂向和横向速度为零作为约束条件进行动态修正。在实际应用中，由于速度误差可能无限制的增长，这将直接影响到姿态和位置误差，使得测量精度受到威胁。相对于单个系统而言，静止修正很好地改善了惯性测量精度。
[0003] 当载体处于静止状态时，理论上速度输出始终为零，位置始终保持不变，此时如果以静止度作为测量值，则相当于用精度极高的速度测量值来限制惯性导航误差的累积，可显著提高惯导与全球定位组合系统垂线偏差测量导航在静止状态下的结果。在载体静止时，由于惯导与全球定位组合系统垂线偏差测量中对于天向姿态误差的补偿不到位，使得对其观测质量差，导致航向角误差的积累随时间逐渐增大，急需改正的垂线偏差观测模型来解决这些问题。
发明内容
[0004] 发明目的：提供一种基于静止修正的垂线偏差测量方法，该方法不仅使用速度、位置信息作为观测量，同时添加零角度修正观测量来限制航向角误差通过更新角速度传感器的常值零偏来减少航向角漂移，旨在解决垂线偏差测量中天向姿态误差的补偿不到位和航向角误差随时间积累增大的问题。
[0005] 技术方案：为实现上述发明目的，本发明采用以下技术方案：
[0006] 一种基于静止修正的垂线偏差测量方法，包括以下步骤：
[0007] (1)建立惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程和测量方程；
[0008] (2)建立惯导与全球定位组合系统静止修正垂线偏差观测模型；
[0009] (3)判断载体是否匀速，若载体匀速，则执行步骤(4)；否则，执行步骤(5)；
[0010] (4)将位置差值和航向角修正的测量值作为观测量对垂线偏差方程进行改正；
[0011] (5)将速度、位置差值以及航向角修正的测量值作为观测量对垂线偏差方程进行改正；
[0012] (6)基于步骤(4)或步骤(5)中改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量进行真值估计。
[0013] 进一步的，步骤(1)中惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程建立方法如下：
[0014] 惯导与全球定位组合系统的姿态位置速度方程的线性化形式表示为：
[0015]
[0016]
[0017] 其中， 为惯导与全球定位组合系统垂线偏差中系统的线性化状态向量，包括当地导航坐标系下的姿态线性化误差矢量 速度线性化误差矢量 位置线性化误差角速度传感器线性化误差矢量 和加速度传感器线性化误差矢量 X(t)为惯导与
全球定位组合系统垂线偏差中的状态向量，包括当地导航坐标系下的姿态误差矢量φn、速度误差矢量δvn、位置误差δrn、角速度传感器误差矢量 和加速度传感器误差矢量 F(t)为动态系数矩阵，包括惯导与全球定位组合系统的位置、速度、姿态，其中，F11～F33分别表示F(t)内部的动态系数，导航坐标系与载体坐标系之间的转置矩阵为 W(t)为惯导与全球定位组合系统的噪声向量，包括角速度传感器噪声向量 和加速度传感器噪声向量G(t)为惯导与全球定位组合系统的噪声系数矩阵。
[0018] 进一步的，步骤(1)中惯导与全球定位组合系统垂线偏差观测模型的测量方程建立方法如下：
[0019] (a)在当地导航系统中，惯导与全球定位组合系统垂线偏差观测模型的测量方程是惯导系统和全球定位系统的位置、速度的差值，其测量方程Z(t)表示为：
[0020]
[0021] 其中， 是惯性系统的位置， 是全球定位系统的位置， 是惯性系统的速度，是全球定位系统的速度， 是位置的随机噪声向量， 是速度的随机噪声向量，φn是姿态误差矢量，δvn是速度误差矢量，δrn是位置误差， 是角速度传感器误差矢量， 是加速度传感器误差矢量；
[0022] (b)全球定位系统给出的位置信息为经度λ、纬度 和高度h，将其转换为当地导航坐标系中量测方程中的位置之差为：
[0023]
[0024] 其中，RM为子午圆半径；RN为卯酉圈半径；λI是惯性系统的经度，λG是全球定位系统的经度， 是惯性系统的纬度， 是全球定位系统的纬度。
[0025] 进一步的，步骤(2)中静止修正垂线偏差观测模型的建立方法如下：
[0026] (21)设置惯导与全球定位组合系统的初始参数，包括初始姿态信息，初始位置信息和初始速度信息；
[0027] (22)惯性系统初始校准，包括水平方向校准和方位校准，使平台坐标系与地理坐标系趋于一致；
[0028] (23)对惯性系统进行捷联解算，根据加速度传感器和角速度传感器信号，解算出惯性系统的航向，姿态，位置及速度；
[0029] (24)判断是否到达静止修正时间，若是，则转到步骤(25)，若否，则转到步骤(23)；其中，静止修正时间设定如下：
[0030]
[0031] 其中，t0为零点修正时间，δa为设置最大零漂误差，ε0为惯导与全球定位组合系统的零偏精度；
[0032] (25)进行递归静止修正解算；
[0033] (26)判断是否到达预先设定的迭代时间ti，若是，则转到步骤(27)，若否，则转到步骤(25)；
[0034] (27)判断是否到达预先设定的解算结束时间ts，若是，则转到步骤(28)，若否，则转到步骤(23)；
[0035] (28)解算结束。
[0036] 更进一步的，步骤(25)中递归静止修正解算方法如下：
[0037] (251)对载体进行静止检测，根据惯性器件角速度传感器和加速度传感器三轴输出以及变化规律判断载体是否静止，并通过改变惯导与全球定位组合系统垂线偏差观测模型的测量方程来对载体静止时的误差进行修正；
[0038] (252)对载体进行静止修正，当载体静止时，认为两个历元间航向角的变化应该为零，并通过更新角速度传感器的常值零偏来减少航向角漂移，具体方法如下：
[0039] 真实航向角的变化率 表达式为：
[0040]
[0041] 其中，Ψ、φ、θ分别表示载体的真实航向角、真实横滚角和真实俯仰角； 为角速度传感器的真实输出角速率； 为载体静止时地球自转在导航坐标系中的角速率；
[0042] 测量航向角的变化率 为：
[0043]
[0044] 其中， 分别表示载体的测量航向角、测量横滚角和测量俯仰角； 为角速度传感器的测量输出角速率； 为载体静止时地球自转在导航坐标系中的角速率；
[0045] 载体处于静止状态，所以 角速度传感器的输出为：
[0046]
[0047] 其中，bg表示角速度传感器的零偏矢量；ng表示角速度传感器的随机噪声； 为角速度传感器的测量输出角速率； 为角速度传感器的真实输出角速率； 为角速度传感器相对导航坐标系的角速率；
[0048] 设俯仰角、横滚角为恒定值，令 δΨ表示航向角的误差，在运算过程中作近似处理，如cosδψ＝1、sinδψ＝δψ；则将测量航向角的变化率 与真实航向角的变化率相减得：
[0049]
[0050] 其中， 为航向角变化率误差；
[0051] 去掉第一项地球的自转误差，静止修正的垂线偏差观测模型得：
[0052]
[0053] 其中，Δtk为航向角更新的时间，k表示迭代次数。
[0054] 进一步的，步骤(4)中将位置差值和航向角修正的测量值作为测量方程对静止修正垂线偏差观测模型进行改正，在静止观测的基础上添加了航向角修正测量值，建立改正的测量方程Z1(t)，并用Z1(t)代替Z(t)：具体测量方程如下所示：
[0055]
[0056] 其中， 为惯性系统相对导航坐标系的位置矢量； 为全球定位系统相对导航坐标系的位置矢量； 为第k次迭代的真实航向角的变化率； 为第k-1次迭代的真实航向n n n角的变化率；φ为姿态误差，δv为速度误差，δr为位置误差， 为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角； 表示全球定位系统的位置随机噪声；wψ为航向角观测的随机噪声。
[0057] 进一步的，步骤(5)中将速度、位置差值以及航向角修正的测量值作为观测量对静止修正垂线偏差观测模型进行改正，建立改正的测量方程Z2(t)，并用Z2(t)代替Z(t)：
[0058]
[0059] 其中， 为惯性系统相对导航坐标系的位置矢量； 为全球定位系统相对导航坐标系的位置矢量； 为惯性系统相对导航坐标系的速度矢量； 为全球定位系统相对导航坐标系的速度矢量； 为第k次迭代的真实航向角的变化率； 为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差， 为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角； 和 分别表示全球定位系统的位置和速度的随机噪声；wψ为航向角观测的随机噪声。
[0060] 进一步的，步骤(6)中基于改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量的真值估计方案如下：
[0061] (61)惯性系统输出状态变量真值和系统误差值，输出值Xs(t)用下列姿态位置速度方程描述：
[0062] Xs(t)＝X(t)+G(t)W(t)；
[0063] 上式中的状态向量X(t)分为两个向量来考虑为：
[0064] X(t)＝[X1(t),X2(t)]；
[0065] 其中，G(t)为惯性系统的噪声系数矩阵，W(t)表示惯性系统的噪声向量，X1(t)为惯性导航系统的误差状态向量，X1(t)为传感器误差的增广状态向量，如下所示：
[0066] X1(t)＝[φE,φN,φU,δVE,δVN,δVU,δL,δλ,δh]T；
[0067]
[0068] 其中，φE、φN、φU分别为东向、北向和天向的平台误差角，δVE、δVN、δVU分别为东向、北向和天向的速度误差，δL、δλ、δh分别为纬度、经度和高度误差；εbE、εbN、εbU分别为东向、北向和天向的角速度传感器随机常数误差，εrE、εrN、εrU分别为东向、北向和天向的角速度传感器的一阶马尔可夫过程误差， 分别为东向、北向和天向的加速度传感器的一阶马尔可夫过程误差。
[0069] 将惯性系统输出值Xs(t)的公式展开得到：
[0070]
[0071] 其中， 为角速度传感器噪声向量和 为加速度传感器噪声向量，导航坐标系与载体坐标系之间的转置矩阵为
[0072] (62)将惯性系统输出状态变量值Xs(t)，结合静止修正测量噪声 输入递归滤波器进行递归静止修正解算，去除惯性系统噪声，并输出惯性系统的状态变量真值估计。
[0073] 有益效果：与现有技术相比，本发明通过对静止修正的垂线偏差观测模型的改正，不仅使用速度、位置信息作为观测量，同时添加零角度修正观测量来限制航向角误差通过更新角速度传感器的常值零偏来减少航向角漂移，解决了由于惯导与全球定位惯导与全球定位组合系统垂线偏差测量中对于天向姿态误差的补偿不到位，航向角误差的积累随时间逐渐增大的问题。
附图说明
[0074] 图1是本发明方法流程图；
[0075] 图2是递归静止修正模型流程图；
[0076] 图3是静止修正的递归解算原理图；
[0077] 图4(a)、(b)、(c)分别为经纬度误差曲线图、速度误差曲线图和姿态误差曲线图；
[0078] 图5是垂线偏差扰动的测量精度曲线。
具体实施方式
[0079] 下面结合附图和具体实施例对本发明的技术方案进行详细说明。
[0080] 本发明的一种基于静止修正的垂线偏差测量方法，具体是惯导与全球定位惯导与全球定位组合系统垂线偏差观测模型姿态位置速度方程和测量方程的建立方法，针对载体匀速与否提出的静止修正的垂线偏差观测模型的改正方法和改正静止修正的垂线偏差观测模型的建立方法。该方法不仅使用速度、位置信息作为观测量，同时添加零角度修正观测量来限制航向角误差通过更新角速度传感器的常值零偏来减少航向角漂移，解决了惯导与全球定位惯导与全球定位组合系统垂线偏差测量中天向姿态误差的补偿不到位和航向角误差随时间积累增大的问题。
[0081] 如图1所示，一种基于静止修正的垂线偏差测量方法，包括如下步骤：
[0082] (1)建立惯导与全球定位惯导与全球定位组合系统垂线偏差观测模型的姿态位置速度方程和测量方程；
[0083] (11)惯导与全球定位组合系统的姿态位置速度方程的线性化形式可表示为：
[0084]
[0085]
[0086] 其中， 为惯导与全球定位组合系统垂线偏差中系统的线性化状态向量，包括当地导航坐标系下的姿态线性化误差矢量 速度线性化误差矢量 位置线性化误差角速度传感器线性化误差矢量 和加速度传感器线性化误差矢量 X(t)为惯导与
全球定位组合系统垂线偏差中的状态向量，包括当地导航坐标系下的姿态误差矢量φn、速度误差矢量δvn、位置误差δrn、角速度传感器误差矢量 和加速度传感器误差矢量 F(t)为动态系数矩阵，包括惯导与全球定位组合系统的位置、速度、姿态，其中，F11～F33分别表示F(t)内部的动态系数，导航坐标系与载体坐标系之间的转置矩阵为 W(t)为惯导与全球定位组合系统的噪声向量，包括角速度传感器噪声向量 和加速度传感器噪声向量G(t)为惯导与全球定位组合系统的噪声系数矩阵。
[0087] (12)在当地导航系统中，惯导与全球定位组合系统的测量方程是惯导系统和全球定位系统的位置、速度的差值，其测量方程Z(t)可以表示为：
[0088]
[0089] 其中， 是惯性系统的位置， 是全球定位系统的位置， 是惯性系统的速度，是全球定位系统的速度， 是位置的随机噪声向量， 是速度的随机噪声向量。
[0090] (13)全球定位系统给出的位置信息为经度λ、纬度 和高度h，将其转换为当地导航坐标系中量测方程中的位置之差为：
[0091]
[0092] 其中，RM为子午圆半径；RN为卯酉圈半径；λI是惯性系统的经度，λG是全球定位系统的经度， 是惯性系统的纬度， 是全球定位系统的纬度。
[0093] (2)建立静止修正垂线偏差观测模型，如图2所示；
[0094] 利用系统停车时的速度输出作为观测量，建立递归滤波器来估计惯导与全球定位惯导与全球定位组合系统的速度误差、位置误差以及姿态角误差，然后将估计到的各项误差补偿到相应的变量中，得到相对比较接近于状态变量真值的估计。采用递归滤波进行静止修正的流程如图2所示，主要包括以下步骤：
[0095] (21)设置惯导与全球定位惯导与全球定位组合系统的初始参数，包括初始姿态信息，初始位置信息和初始速度信息；
[0096] (22)惯性系统初始校准，包括水平方向校准和方位校准，使平台坐标系与地理坐标系趋于一致。
[0097] (23)对惯性系统进行捷联解算，根据加速度传感器和角速度传感器信号，解算出惯性系统的航向，姿态，位置及速度。
[0098] (24)判断是否到达静止修正时间，若是，则转到步骤(25)，若否，则转到步骤(23)。其中，静止修正时间设定如下：
[0099]
[0100] 其中，t0为零点修正时间，δa为设置最大零漂误差，ε0为惯导与全球定位组合系统的零偏精度。
[0101] (25)进行递归静止修正解算，其基本原理如图3所示。
[0102] (251)对载体进行静止检测，根据惯性器件角速度传感器和加速度传感器三轴输出以及变化规律判断载体是否静止，并通过改变惯导与全球定位组合系统的测量方程来对载体静止时的误差进行修正。
[0103] (252)对载体进行静止修正。当载体静止时，认为两个历元间航向角的变化应该为零，并通过更新角速度传感器的常值零偏来减少航向角漂移，具体方法如下：
[0104] 真实航向角的变化率 表达式为：
[0105]
[0106] 其中，Ψ、φ、θ分别表示载体的真实航向角、真实横滚角和真实俯仰角； 为角速度传感器的真实输出角速率； 为载体静止时地球自转在导航坐标系中的角速率。
[0107] 测量航向角的变化率 为：
[0108]
[0109] 其中， 分别表示载体的测量航向角、测量横滚角和测量俯仰角； 为角速度传感器的测量输出角速率； 为载体静止时地球自转在导航坐标系中的角速率。
[0110] 载体处于静止状态，所以 角速度传感器的输出可以写为：
[0111]
[0112] 其中，bg表示角速度传感器的零偏矢量；ng表示角速度传感器的随机噪声； 为角速度传感器的测量输出角速率； 为角速度传感器的真实输出角速率； 为角速度传感器相对导航坐标系的角速率。
[0113] 假设俯仰角、横滚角为恒定值，令 δΨ表示航向角的误差。在运算过程中作近似处理，如cosδψ＝1、sinδψ＝δψ。则将式(7)和式(6)相减可得：
[0114]
[0115] 其中，Ψ、φ、θ分别表示载体的真实航向角、真实横滚角和真实俯仰角； 为角速度传感器相对导航坐标系的角速率；bg表示角速度传感器的零偏矢量；ng表示角速度传感器的随机噪声； 为测量航向角的变化率； 为真实航向角的变化率； 为航向角变化率误差。
[0116] 去掉第一项地球的自转误差，静止修正的垂线偏差观测简化模型可得：
[0117]
[0118] 其中，Δtk为航向角更新的时间，k表示迭代次数。
[0119] (26)判断是否到达迭代时间，若是，则转到步骤(27)，若否，则转到步骤(25)。其中，迭代时间ti预先设定。
[0120] (27)判断是否到达解算结束时间，若是，则转到步骤(28)，若否，则转到步骤(23)。其中，解算结束时间ts预先设定。
[0121] (28)解算结束。
[0122] (3)判断载体是否匀速，若载体匀速，则执行步骤(4)；否则，执行步骤(5)；
[0123] (4)将位置差值和航向角修正的测量值作为观测量对静止修正垂线偏差观测模型进行改正，建立改正的测量方程Z1(t)，并用Z1(t)代替Z(t)。在静止观测的基础上添加了航向角修正测量值，具体测量方程如下所示：
[0124]
[0125] 其中， 为惯性系统相对导航坐标系的位置矢量； 为全球定位系统相对导航坐标系的位置矢量； 为第k次迭代的真实航向角的变化率； 为第k-1次迭代的真实航向角的变化率；φn为姿态误差，δvn为速度误差，δrn为位置误差， 为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角； 表示全球定位系统的位置随机噪声；wψ为航向角观测的随机噪声。
[0126] (5)将速度、位置差值以及航向角修正的测量值作为观测量对静止修正垂线偏差观测模型进行改正，建立改正的测量方程Z2(t)，并用Z2(t)代替Z(t)；
[0127]
[0128] 其中， 为惯性系统相对导航坐标系的位置矢量； 为全球定位系统相对导航坐标系的位置矢量； 为惯性系统相对导航坐标系的速度矢量； 为全球定位系统相对导航坐标系的速度矢量； 为第k次迭代的真实航向角的变化率； 为第k-1次迭代的真实航n n n向角的变化率；φ为姿态误差，δv为速度误差，δr为位置误差， 为角速度传感器的零偏，为加速度传感器的零偏；I3×3是惯性系统的3×3向量矩阵；θ为俯仰角，ψ为航向角，φ为翻滚角； 和 分别表示全球定位系统的位置和速度的随机噪声；wψ为航向角观测的随机噪声。
[0129] (6)基于改正的静止修正的垂线偏差观测模型对惯性系统输出状态变量进行真值估计。
[0130] (61)惯性系统输出状态变量真值和系统误差值，输出值Xs(t)可用下列状态方程描述：
[0131] Xs(t)＝X(t)+G(t)W(t) (13)；
[0132] 上式中的状态向量X(t)可分为两个向量来考虑：
[0133] X(t)＝[X1(t),X2(t)] (14)；
[0134] 其中，G(t)为惯性系统的噪声系数矩阵，W(t)表示惯性系统噪声，X1(t)为惯性导航系统的误差状态向量，X1(t)为传感器误差的增广状态向量，如下所示：
[0135] X1(t)＝[φE,φN,φU,δVE,δVN,δVU,δL,δλ,δh]T(15)；
[0136]
[0137] 其中，φE、φN、φU分别为东向、北向和天向的平台误差角，δVE、δVN、δVU分别为东向、北向和天向的速度误差，δL、δλ、δh分别为纬度、经度和高度误差；εbE、εbN、εbU分别为东向、北向和天向的角速度传感器随机常数误差，εrE、εrN、εrU分别为东向、北向和天向的角速度传感器的一阶马尔可夫过程误差， 分别为东向、北向和天向的加速度传感器的一阶马尔可夫过程误差。
[0138] 式(13)展开得到：
[0139]
[0140] 其中， 为角速度传感器噪声向量和 为加速度传感器噪声向量，导航坐标系与载体坐标系之间的转置矩阵为
[0141] (62)将惯性系统输出状态变量值Xs(t)，结合静止修正测量噪声 输入递归滤波器进行递归静止修正解算，去除惯性系统噪声，并输出惯性系统的状态变量真值估计。
[0142] 为了验证载体在静止条件下使用静止修正技术时系统输出的导航信息修正效果，对垂线偏差测量导航系统进行了仿真实验，表1为惯性系统的主要指标。
[0143] 表1惯性系统的主要指标
[0144]
[0145] 仿真结果如图4所示。具体地，图4(a)所示为经纬度误差曲线图，图4(b)所示为速度误差曲线图，图4(c)所示为姿态误差曲线图；从图4可以看出，改正后方案都可以有效的改善载体的姿态误差，提高惯性系统的姿态输出精度，即可以采用速度位置姿态匹配的方法，输出惯性系统姿态，并将其作为惯性测量单元姿态的初始值。最后利用Kalman滤波估算出垂线偏差。
[0146] 图5是垂线偏差扰动的测量结果。由图可知，总体上改正方案精度在2″以内，证实了该方法的可行性。
[0147] 为减小惯性系统输出的姿态误差对垂线偏差计算产生误差影响，本发明利用静止修正技术改善了姿态误差输出精度。在静止修正模式下对垂线偏差建立相应的姿态位置速度方程，采用速度、位置匹配建立测量方程，并在位置、速度差观测量的基础上添加姿态角观测量，使角速度传感器的零偏变成可直接测量的参数，提高了航向角的估计精度，解决了垂线偏差测量中天向姿态误差的补偿不到位和航向角误差随时间积累增大的问题。
