CN104635251A - 一种ins/gps组合定位定姿新方法 - Google Patents

Abstract

本发明提供一种INS/GPS组合定位定姿新方法，包括：采用线性卡尔曼滤波器对GPS原始测量数据进行滤波估计，输出GPS导航最优估计值；以位置最优估计值向INS提供初始位置信息，以速度最优估计值向INS提供初始速度信息，对INS原始测量数据进行解算，得到INS导航信息；采用动态误差模型建立9阶扩展卡尔曼滤波器，融合INS导航信息与GPS导航最优估计值，反馈矫正同一时刻下的所述INS导航信息，输出修正融合后的最佳位置数据和姿态数据。具有精度高、数据处理速度快、硬件要求低等特点，可适用于低成本的INS结合GPS的定位定姿组合方案。

Description

一种INS/GPS组合定位定姿新方法

技术领域

本发明属于导航及摄影测量技术领域，具体涉及一种INS/GPS组合定位定姿新方法。

背景技术

组合导航***是以计算机为基础，以最佳统计理论为方法，将载体上各种单一导航设备采集的导航信息输入计算机，经过综合处理，相互取长补短，得到载体最佳的导航参数，从而形成一个统一的整体。组合导航技术克服了单一导航***中存在的各项不足，是目前导航领域发展的主要方向。在基于移动载体的摄影测量方面，主要采用全球定位***(GPS)与惯性导航***(INS)组合的导航方式直接获取移动载体的外方位元素，大大提高了数据的处理速度及数据精度。

其中，GPS是当前应用最为广泛的卫星导航定位***，能够提供全球性、全天候的导航定位授时服务，具有定位精度高和误差不随时间积累的优点，并且差分模式下定位精度可达厘米计，但是，GPS信号具有易受干扰、信号遮挡、动态环境中可靠性差、数据更新率低、没有姿态信息输出等不足；而INS(InertialNavigation System，惯性导航***)是利用固定在载体上的惯性测量装置感受载体的运动，完全自主的输出包括姿态在内的全部导航参数，还具有数据更新率高的优点，但是存在导航误差随着导航时间的延长而迅速积累的问题，不能单独长时间工作。可见INS与GPS具有非常好的互补特性，采用INS与GPS组合的导航***，一方面可以使用GPS定位误差不随时间积累的定位结果修正INS的导航结果；另一方面，短时间内高精度、高稳定的INS导航结果可以很好的解决GPS信号失锁的问题，大大的提高了***的鲁棒性，是导航及摄影测量领域实时获取载***置和姿态数据的比较理想的组合方式。

INS/GPS组合导航的实质是多传感器的数据融合技术，数据滤波技术是其中的关键技术之一。目前主要采用的数据滤波方法为卡尔曼滤波，卡尔曼滤波是一种线性最小方差估计方法，采用状态空间方法描述***，算法采用递推形式，便于在计算机上实现，可处理时变***、非平稳信号、多维信号滤波等问题。INS/GPS组合导航属于非线性***，需采用扩展卡尔曼滤波方法将非线性***转为一个近似线性化的***方程。但是，目前算法中大多直接以***的位置、速度以及姿态等值直接作为状态量，没有充分考虑***中各项误差传播情况，导致线性化误差增大、雅阁比矩阵计算繁琐、各项矩阵数值较大引起计算消耗极大内存空间，执行时间冗长、效率低等问题，极容易使***出现误差增大甚至结果发散的情况，所以设计一种高效的数据滤波方法是非常有必要的。

发明内容

针对现有技术存在的缺陷，本发明提供一种INS/GPS组合定位定姿新方法，以位置、速度、姿态的测量误差作为状态向量，以GPS与INS输出的位置与速度之差作为观测向量，结合误差协方差可以得到融合后的高精度位置和姿态数据，具有精度高、数据处理速度快、硬件要求低等特点，可适用于低成本的INS结合GPS的定位定姿组合方案。

本发明采用的技术方案如下：

本发明提供一种INS/GPS组合定位定姿新方法，包括以下步骤：

S1，采用线性卡尔曼滤波器对GPS原始测量数据进行滤波估计，输出GPS导航最优估计值；其中，所述GPS导航最优估计值包括位置最优估计值和速度最优估计值；

S2，以所述位置最优估计值向INS提供初始位置信息，以所述速度最优估计值向INS提供初始速度信息，对INS原始测量数据进行解算，得到INS导航信息；其中，所述INS导航信息包括姿态、位置和速度信息；

S3，由于INS的数据采集频率大于GPS的数据采集频率，当没有GPS原始测量数据时，所述INS导航信息即为最终输出的数据；

当有GPS原始测量数据时，采用动态误差模型建立9阶扩展卡尔曼滤波器，融合INS导航信息与GPS导航最优估计值，反馈矫正同一时刻下的所述INS导航信息，输出修正融合后的最佳位置数据和姿态数据。

优选的，S1具体包括以下步骤：

S1.1，采用实时差分或事后差分的方式采集获得所述GPS原始测量数据；

S1.2，从所述GPS原始测量数据中提取经度、纬度、高度、地速和航向信息，并将提取的球面坐标系下的经度和纬度数据转换为UTM投影坐标系下的空间平面坐标(X，Y)；

S1.3，假设运动载体做匀速直线运动且不考虑载体自身的动力因素，以载体的空间平面坐标(X，Y)与平面速度(V_x，V_y)为状态向量，以GPS的空间二维平面坐标和地速为观测向量，建立所述线性卡尔曼滤波器，所述线性卡尔曼滤波器的状态方程如式1，其观测方程见式2：

X(k+1)＝ΦX(k)+Γu(k)            (1)

Z(k)＝HX(k)+v(k)                 (2)

其中，k表示离散化时刻，X＝[X，Y，V_x，V_y]^T，Z＝[X，Y，V_x，V_y]^T， $\mathrm{\Φ}=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ 为状态转移矩阵， $\mathrm{\Γ}=\left[\begin{array}{cc}{0.5T}^2& 0\\ T& 0\\ 0& 0.5T^2\\ 0& T\end{array}\right]$ 为控制向量矩阵， $H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ 为观测矩阵，V_x＝V×|sin(h)|，V_y＝V×|cos(h)|。u，v为零均值的过程噪声和观测噪声，T为采样周期，V为GPS观测的地速，h为以真北为参考基准的地面航向；

S1.4，采用GPS接收机差分定位精度作为过程噪声协方程Q，以GPS测量精度为观测噪声协方差R，以S1.2转换后的平面坐标(X，Y)以及高度、地速和航向信息作为线性卡尔曼滤波器初始输入值，给定初始值X(0|0)及初始协方程P(0|0)后，对GPS原始测量数据进行卡尔曼滤波估计，待状态量更新后，输出位置及速度的最优估计值；其中，所述位置最优估计值包括转换为地理坐标系下的经纬度值以及GPS高程值，即：经度μ、纬度λ和高程值h；所述速度最优估计值为载体地速V。

优选的，S2具体包括以下步骤：

S2.1，获取INS数据解算必要的初始信息，包括：线性卡尔曼滤波器输出的位置最优估计值作为载体初始位置信息(μ，λ，h)、线性卡尔曼滤波器输出的速度最优估计值作为载体沿自身轴向的初始速度信息(V_bx、V_by、V_bz)和载体初始姿态角数据(Φ，θ，φ)；其中，横滚角为Φ、俯仰角为θ、航向角为φ；

S2.2，根据S2.1获取的初始信息，对***进行初始化，包括载***置和速度的初始化，使用初始姿态角数据初始化姿态矩阵并计算初始四元数(q₀、q₁、q₂、q₃)；

S2.3，使用***当前的载体速度(V_bx、V_by、V_bz)和姿态矩阵信息，解算载体经度、纬度、高程的位置微分方程，得到经纬度及高度的微分值；此步骤中采用式3将载体坐标系下的速度值转换为导航坐标系下的速度：

$\left[\begin{array}{c}{V}_N\\ V_E\\ V_D\end{array}\right]={\left(C_n^b\right)}^T\left[\begin{array}{c}{V}_{\mathrm{bx}}\\ V_{\mathrm{by}}\\ V_{\mathrm{bz}}\end{array}\right]---\left(3\right)$

其中，导航坐标系下运动载体沿坐标轴系北向速度为V_N、东向速度为V_E、向下速度为V_D；

并且，经度、纬度、高程的微分方程分别见式4、式5和式6；

$\stackrel{\·}{\mathrm{\λ}}=\frac{V_N}{R_n+h}---\left(4\right)$

$\stackrel{\·}{\mathrm{\μ}}=\frac{V_E}{(R_e+h)\cos \left(\mathrm{\λ}\right)}---\left(5\right)$

$\stackrel{\·}{H}=-V_D---\left(6\right)$

其中，为经纬度及高度的微分值，R_n为参考椭球子午面的曲率半径，R_e为垂直子午面的法线平面内的曲率半径，h为INS中惯性高度和高度表高度的加权非线性估计，即

h＝(h_a)^α(h_i)^1-α                 (7)

S2.4，提取INS中陀螺仪输出的角速率，根据地球自转角速率及载体相对地球表面运动的影响，采用式8修正INS原始输出的三轴角速率值：

$\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]={\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]}_m-C_n^b[\mathrm{\Ω}+{\mathrm{\ω}}^{\′}]---\left(8\right)$

上式中， ${\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]}_m$ 为INS的原始输出角速率， $\mathrm{\Ω}=\left[\begin{array}{cc}{\mathrm{\ω}}_e& \cos \left(\mathrm{\λ}\right)\\ & 0\\ -{\mathrm{\ω}}_e& \sin \left(\mathrm{\λ}\right)\end{array}\right]$ 为地球自转角速率影响， ${\mathrm{\ω}}^{\′}=\left[\begin{array}{c}\stackrel{\·}{\mathrm{\μ}}\cos \left(\mathrm{\λ}\right)\\ -\stackrel{\·}{\mathrm{\λ}}\\ -\stackrel{\·}{\mathrm{\μ}}\sin \left(\mathrm{\λ}\right)\end{array}\right]$ 为载体相对地球运动产生的影响，为当前的姿态矩阵。ω_e为地球自转角速率，为步骤S2.3中计算得到的经度与纬度的微分值；

然后采用四元数法建立四元数微分方程解算INS姿态矩阵，四元数微分方程为：

$\left[\begin{array}{c}\stackrel{\·}{q_0}\\ \stackrel{\·}{q_1}\\ \stackrel{\·}{q_2}\\ \stackrel{\·}{q_3}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bx}}& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]---\left(9\right)$

其中，表示四元数微分值，q表示上一时刻的四元数值，ω_bx、ω_by、ω_bz为修正后的角速率；

S2.5，提取INS中加速度计输出的三轴加速度数据，，计算载体在自身坐标系下沿自身轴系的速度微分方程：

${\stackrel{\·}{V}}_{\mathrm{bx}}=a_x+V_{\mathrm{by}}{\mathrm{\ω}}_{\mathrm{bz}}-V_{\mathrm{bz}}{\mathrm{\ω}}_{\mathrm{by}}+g\sin \left(\mathrm{\θ}\right)---\left(10\right)$

${\stackrel{\·}{V}}_{\mathrm{by}}=a_y-V_{\mathrm{bx}}{\mathrm{\ω}}_{\mathrm{bz}}+V_{\mathrm{bz}}{\mathrm{\ω}}_{\mathrm{bx}}+g\cos \left(\mathrm{\θ}\right)\sin \left(\mathrm{\Φ}\right)---\left(11\right)$

${\stackrel{\·}{V}}_{\mathrm{bz}}=a_z+V_{\mathrm{bx}}{\mathrm{\ω}}_{\mathrm{by}}-V_{\mathrm{by}}{\mathrm{\ω}}_{\mathrm{bx}}+g\cos \left(\mathrm{\θ}\right)\cos \left(\mathrm{\Φ}\right)---\left(12\right)$

其中，a_x、a_y、a_z为加速度计输出的加速度数据，ω_bx、ω_bx、ω_bx为S2.4修正后的角速率，g为当地的重力加速度值，Φ、θ为当前的姿态角。

S2.6，采用四阶龙哥-库塔算法解算四元数微分方程、速度微分方程以及位置微分方程，计算任一时刻载体的姿态角、速度和位置的变化量，并更新相应的姿态、速度及位置数据；

S2.7，重复步骤S2.3～S2.6，直到INS原始数据全部解算完毕。

优选的，S2.6中，对经四阶龙格-库塔解算后的四元数微分值，在对四元数更新后需对当前四元数进行规范化处理。

优选的，S3中，当有GPS原始测量数据时，通过以下方法得到所述最佳位置数据和姿态数据：

S3.1，***中各状态量的扰动方程表示如下：

${\hat{r}}^n=r^n+{\mathrm{\δr}}^n---\left(14\right)$

${\hat{v}}^n=v^n+{\mathrm{\δv}}^n---\left(15\right)$

$\widehat{C_b^n}=(I-E^n)C_b^n---\left(16\right)$

其中，rⁿ、vⁿ为地理坐标系下载体的位置向量与速度向量，为载体坐标系转换为地理坐标系的姿态余弦矩阵，En为姿态误差(εⁿ)的反对称形式。^表示测量值、δ表示误差值，有：

$E^n=({\mathrm{\ϵ}}^n\×)=\left[\begin{array}{ccc}0& -{\mathrm{\ϵ}}_D& {\mathrm{\ϵ}}_E\\ {\mathrm{\ϵ}}_D& 0& -{\mathrm{\ϵ}}_N\\ -{\mathrm{\ϵ}}_E& {\mathrm{\ϵ}}_N& 0\end{array}\right]---\left(17\right)$

S3.2，由于INS/GPS组合***为非线性***，根据位置、速度和姿态的微分方程，建立各自的误差动态方程，线性化且只保留一阶项后的位置、速度及姿态误差动态方程表示如下

$\mathrm{\δ}{\stackrel{\·}{r}}^n=F_{\mathrm{rr}}{\mathrm{\δr}}^n+F_{\mathrm{rv}}{\mathrm{\δv}}^n---\left(18\right)$

$\mathrm{\δ}{\stackrel{\·}{v}}^n=F_{\mathrm{vr}}{\mathrm{\δr}}^n+F_{\mathrm{vv}}{\mathrm{\δv}}^n+(f^n\×){\mathrm{\ϵ}}^n+C_b^n{\mathrm{\δf}}^b---\left(19\right)$

${\stackrel{\·}{\mathrm{\ϵ}}}^n=F_{\mathrm{er}}{\mathrm{\δr}}^n+F_{\mathrm{ev}}{\mathrm{\δv}}^n+((\mathrm{\Ω}+{\mathrm{\ω}}^{\text{\′}})\×){\mathrm{\ϵ}}^n-C_b^n{\mathrm{\δ\ω}}_{\mathrm{ib}}^b---\left(20\right)$

其中，式(18)为位置误差动态方程，(19)为速度误差动态方程，(20)为姿态误差动态方程；为位置、速度和姿态误差向量的微分值， 为载体在导航坐标系下沿各轴系的加速度，δf^b为加速度计的扰动误差，为惯性坐标系与载体坐标系的角速率扰动误差，F为各误差动态方程的雅阁比矩阵，有：

$F_{\mathrm{rr}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\λ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{h}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{h}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{h}}{\∂h}\end{array}\right]=\left[\begin{array}{ccc}0& 0& \frac{-V_N}{{(R_n+h)}^2}\\ \frac{V_e\sin \mathrm{\λ}}{(R_e+h){\cos }^2\mathrm{\λ}}& 0& \frac{-V_E}{{(R_e+h)}^2\cos \mathrm{\λ}}\\ 0& 0& 0\end{array}\right]---\left(21\right)$

$F_{\mathrm{rv}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\λ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{h}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{h}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{h}}{{\∂V}_D}\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{R_e+h}& 0& 0\\ 0& \frac{1}{(R_e+h)\cos \mathrm{\λ}}& 0\\ 0& 0& -1\end{array}\right]---\left(22\right)$

$F_{\mathrm{vr}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{V_N}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{V_N}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{V_N}}{\∂h}\\ \frac{\∂\stackrel{\·}{V_E}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{V_E}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{V_E}}{\∂h}\\ \frac{\∂\stackrel{\·}{V_D}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{V_D}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{V_D}}{\∂h}\end{array}\right]=\left[\begin{array}{ccc}-{2V}_E\mathrm{\Ω}\cos \mathrm{\λ}-\frac{V_E^2}{(R_n+h){\cos }^2\mathrm{\λ}}& 0& \frac{-V_N{V}_D}{{(R_n+h)}^2}+\frac{V_E^2\tan \mathrm{\λ}}{{(R_n+h)}^2}\\ 2\mathrm{\Ω}(V_N\cos \mathrm{\λ}-V_D\sin \mathrm{\λ})+\frac{V_E{V}_N}{(R_e+h){\cos }^2\mathrm{\λ}}& 0& \frac{V_E{V}_D}{{(R_e+h)}^2}-\frac{V_N{V}_E\tan \mathrm{\λ}}{{(R_e+h)}^2}\\ 2V_E\mathrm{\Ω}\sin \mathrm{\λ}& 0& \frac{V_E^2+V_N^2}{{(R_e+h)}^2}-\frac{2\mathrm{\γ}}{(R_e+h)}\end{array}\right]---\left(23\right)$

$F_{\mathrm{vv}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{V_N}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{V_N}}{\∂V_E}& \frac{\∂\stackrel{\·}{V_N}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{V_E}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{V_E}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{V_E}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{V_D}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{V_D}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{V_D}}{{\∂V}_D}\end{array}\right]=\left[\begin{array}{ccc}\frac{V_D}{R_n+h}& -2\mathrm{\Ω}\sin \mathrm{\λ}-2\frac{V_E\tan \mathrm{\λ}}{R_n+h}& \frac{V_N}{R_n+h}\\ 2\mathrm{\Ω}\sin \mathrm{\λ}+\frac{V_E\tan \mathrm{\λ}}{R_e+h}& \frac{V_D+V_N\tan \mathrm{\λ}}{R_e+h}& 2\mathrm{\Ω}\cos \mathrm{\λ}+\frac{V_E}{R_e+h}\\ -2\frac{V_N}{R_e+h}& -2\mathrm{\Ω}\cos \mathrm{\λ}-2\frac{V_E}{R_e+h}& 0\end{array}\right]---\left(24\right)$

$F_{\mathrm{er}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{\∂h}\end{array}\right]=\left[\begin{array}{ccc}-\mathrm{\Ω}\sin \mathrm{\λ}& 0& \frac{{-V}_E}{{(R_e+h)}^2}\\ 0& 0& \frac{V_N}{{(R_e+h)}^2}\\ -\mathrm{\Ω}\cos \mathrm{\λ}-\frac{V_E}{(R_n+h)c{\mathrm{os}}^2\mathrm{\λ}}& 0& \frac{V_E\tan \mathrm{\λ}}{{(R_e+h)}^2}\end{array}\right]---\left(25\right)$

$F_{\mathrm{ev}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{{\∂V}_D}\end{array}\right]=\left[\begin{array}{ccc}0& \frac{1}{R_e+h}& 0\\ \frac{-1}{R_n+h}& 0& 0\\ 0& \frac{-\tan \mathrm{\λ}}{R_e+h}& 0\end{array}\right]---\left(26\right)$

其中，R_n为参考椭球子午面内的曲率半径，R_e为垂直子午面的法线平面内的曲率半径，γ＝[0 0 γ]^T为当地地球重力矢量，γ₀为h＝0的正常重力值g₀；

S3.3，根据误差动态方程建立整个***随时间离散的状态预测方程为：

x_k+1＝Φ_kx_k+w_k                  (27)

其中，Φ_k＝exp(FΔt)I+FΔt为状态转移矩阵，w_k＝Gu为采样间隔内的高斯白噪声的影响。x＝[δrⁿ δvⁿ ∈ⁿ]^T为***的状态矩阵， $F=$ $\left[\begin{array}{ccc}{F}_{\mathrm{rr}}& F_{\mathrm{rv}}& 0\\ F_{\mathrm{vr}}& F_{\mathrm{vv}}& (f^b\×)\\ F_{\mathrm{er}}& F_{\mathrm{ev}}& -((\mathrm{\Ω}+{\mathrm{\ω}}^{\′})\×)\end{array}\right]$ 为***的动态矩阵， $G=\left[\begin{array}{cc}0& 0\\ C_b^n& 0\\ 0& -C_b^n\end{array}\right]$ 为***的噪声驱动矩阵， $u=\left[\begin{array}{c}{\mathrm{\δf}}^b\\ \mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b\end{array}\right]$ 为均值为零的***高斯白噪声。w_k的协方差矩阵为：

$Q_k=E\left[{\mathrm{\ω}}_k{\mathrm{\ω}}_k^T\right]\≈{\mathrm{\Φ}}_k\mathrm{GQG}{\mathrm{\Φ}}_k^T\mathrm{\Δt}---\left(28\right)$

其中， $Q=\mathrm{diag}\left(\begin{array}{cccccc}{\mathrm{\σ}}_{\mathrm{ax}}^2& {\mathrm{\σ}}_{\mathrm{ay}}^2& {\mathrm{\σ}}_{\mathrm{az}}^2& {\mathrm{\σ}}_{\mathrm{\ωx}}^2& {\mathrm{\σ}}_{\mathrm{\ωy}}^2& {\mathrm{\σ}}_{\mathrm{\ωz}}^2\end{array}\right)$ 为***的过程噪声协方差矩阵，σ_a、σ_ω分别为加速度计与陀螺仪的标准差，Δt为采样间隔；

S3.4，以GPS与INS的位置差为观测向量，建立***的观测方程为：

$z_k=r_{\mathrm{INS}}^n-r_{\mathrm{GPS}}^n=\left[\begin{array}{c}(R_n+h)({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{GPS}})\\ (R_e+h)\cos \mathrm{\λ}({\mathrm{\μ}}_{\mathrm{INS}}-{\mathrm{\μ}}_{\mathrm{GPS}})\\ h_{\mathrm{INS}}-h_{\mathrm{GPS}}\end{array}\right]---\left(29\right)$

测量噪声协方差为：

$R_k=\mathrm{diag}\left(\begin{array}{ccc}{\mathrm{\σ}}_{\mathrm{\λ}}^2& {\mathrm{\σ}}_{\mathrm{\μ}}^2& {\mathrm{\σ}}_h^2\end{array}\right)---\left(30\right)$

其观测矩阵为：

$H_k=\left[\begin{array}{cccc}(R_n+h)& 0& 0& \\ 0& (R_e+h)\cos \mathrm{\λ}& 0& \left|0_{3\×3}\right|0_{3\×3}\\ 0& 0& 1& \end{array}\right]---\left(31\right)$

S3.5，根据状态预测方程和观测方程，结合过程噪声误差协方差Q和测量噪声协方差R，在给定初始状态估计和协方差矩阵P(0|0)后，构建得到INS/GPS扩展卡尔曼滤波器，计算出任一时刻位置、速度以及姿态的误差最优估计(δrⁿ δvⁿ εⁿ)；

其中，INS/GPS扩展卡尔曼滤波器为：

协方程预测：

$P\left(k\right|k-1)={\mathrm{\Φ}}_{k}P(k-1|k-1){\mathrm{\Φ}}_K^T+Q_k---\left(32\right)$

卡尔曼增益：

$K_k=\frac{P\left(k\right|k-1)H_k^T}{(H_{k}P\left(k\right|k-1)H_k^T+R_k)}---\left(33\right)$

状态更新：

X(k|k)＝X(k|k-1)+K_k(z_k-H_kX(k|k-1))         (34)

协方程更新：

P(k|k)＝(I-K_kH_k)P(k|k-1)                  (35)

S3.6，采用经扩展卡尔曼滤波后的位置、速度及姿态的误差最优估计值采用以下公式修正同一时刻下INS解算出的位置、速度和姿态数据，获取最佳的位置、速度和姿态数据：

$r^n={\hat{r}}^n-{\mathrm{\δr}}^n---\left(36\right)$

$v^n={\hat{v}}^n-\mathrm{\δ}{v}^n---\left(37\right)$

$C_b^n=(I+E^n)\widehat{C_b^n}---\left(38\right)$

其中Eⁿ为姿态误差(εⁿ)的反对称形式。

本发明的有益效果如下：

(1)本发明以INS/GPS组合***的各项误差为状态向量，通过误差传播的动态方程建立了9阶卡尔曼滤波模型，大大减小了模型线性化带来的截断误差，以及***中存在的各类误差影响，提高了***精度。

(2)本发明涉及的状态量较少，计算过程中的均没有大量级的数值出现，减少了运算量，节省了计算中的内存消耗，计算周期短，效率高。

(3)本发明对硬件的要求较低，可应用于低成本的INS/GPS组合***，实时获取较高精度的位置和姿态数据，节省作业成本。

附图说明

图1为本发明提供的INS/GPS组合定位定姿新方法整体技术流程图；

图2为本发明提供的GPS线性卡尔曼滤波处理流程图；

图3为本发明提供的INS数据解算流程图；

图4为本发明的INS/GPS组合***扩展卡尔曼滤波流程图。

具体实施方式

以下结合附图对本发明进行详细说明：

如图1所示，为本发明的整体技术流程，其主要包括以下内容：

S1，采用线性卡尔曼滤波对GPS原始测量数据进行滤波估计，输出GPS位置和速度的最优估计值；

如图2所示，为本发明线性卡尔曼滤波器滤波处理流程图。在本发明的应用中，通过建立GPS基站，GPS采用差分模式，可采取实时差分或事后差分的方式完成对GPS原始测量数据的采集，GPS采用NMEA-0183协议输出标准数据格式，在具体实施中可采用以$GPGGA开头的数据报文提供时间、位置和状态灯信息，结合以$GPVTG开头的数据报文提供速度、地面航向等信息。

在具体实施GPS卡尔曼滤波过程前，首先需要从GPS原始测量数据中提取经度、纬度、高度以及地速和航向信息，用于在数据滤波中使用，并将提取的位置数据(即：经度和纬度)转换为UTM投影下的位置坐标(X，Y)。由于采用的GPS接收机数据更新频率为20Hz，可假设在一个数据采集周期内载体做匀速直线运动，且不考虑载体自身的动力因素，以GPS原始测量数据中载体空间平面坐标(X，Y)与平面速度(V_x，V_y)为状态向量，以GPS的空间二维平面坐标、地速为观测向量，建立线性卡尔曼滤波器对GPS原始测量数据进行最优估计，其中，所建立的状态方程及观测方程如下：

X(k+1)＝ΦX(k)+Γu(k)            (1)

Z(k)＝HX(k)+v(k)                 (2)

其中，k表示离散化时刻，X＝[X，Y，V_x，V_y]^T，Z＝[X，Y，V_x，V_y]^T， $\mathrm{\Φ}=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ 为状态转移矩阵， $\mathrm{\Γ}=\left[\begin{array}{cc}{0.5T}^2& 0\\ T& 0\\ 0& 0.5T^2\\ 0& T\end{array}\right]$ 为控制向量矩阵， $H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ 为观测矩阵，V_x＝V×|sin(h)|，V_y＝V×|cos(h)|。u，v为零均值的过程噪声和观测噪声，T为采样周期，V为GPS观测的地速，h为以真北为参考基准的地面航向。

由于在具体实施时可能无法直接获取随机加速度对载体运动的影响，考虑到载体做匀速运动的情况，可采用GPS接收机差分定位精度作为过程噪声协方程Q，以GPS测量精度为观测噪声协方差R，从GPS原始测量数据中获取初始的GPS位置、速度、航向信息作为卡尔曼滤波初始输入值，给定初始值X(0|0)及初始协方程P(0|0)后，即可完成对GPS原始测量数据的卡尔曼滤波估计，待状态量更新后，输出位置及速度的最优估计值。最后将坐标值转换为球面坐标系下的经纬度值，即：WGS-84地理坐标系下的经纬度值；结合GPS输出的高度数据，输出经度(μ)、纬度(λ)、高程(h)和地速(V)。待GPS/INS数据融合与滤波时使用。

S2，以GPS滤波结果向INS提供初始的位置和速度信息，对INS原始数据进行解算，得到INS输出的姿态、位置和速度信息；

如图3所示，为本发明所采用的INS原始数据解算流程图，其具体的实施方式包括如下步骤：

步骤(1)，获取INS数据解算必要的初始信息，包括从GPS信息中获取载体初始位置信息(μ，λ，h)和载体沿自身轴向的初始速度信息(V_bx、V_by、V_bz)，获取载体初始姿态角数据(Φ，θ，φ)。其中，横滚角(Φ)、俯仰角(θ)、航向角(φ)。

步骤(2)，根据步骤(1)获取的初始信息，对***进行初始化，包括载***置和速度的初始化，使用初始姿态角数据初始化姿态矩阵并计算初始四元数(q₀、q₁、q₂、q₃)。

步骤(3)，使用***当前的载体速度(V_bx、V_by、V_bz)和姿态矩阵信息，解算载体经度、纬度、高程的位置微分方程。此步骤中需要采用下式(3)将载体坐标系下的速度值转换为导航坐标系下的速度：

$\left[\begin{array}{c}{V}_N\\ V_E\\ V_D\end{array}\right]={\left(C_n^b\right)}^T\left[\begin{array}{c}{V}_{\mathrm{bx}}\\ V_{\mathrm{by}}\\ V_{\mathrm{bz}}\end{array}\right]---\left(3\right)$

其中，导航坐标系下运动载体沿坐标轴系北向速度为V_N、东向速度为V_E、向下速度为V_D；

然后解算运动载体经度、纬度、高度的微分方程，其中，经度、纬度、高程的微分方程采用如下形式：

$\stackrel{\·}{\mathrm{\λ}}=\frac{V_N}{R_n+h}---\left(4\right)$

$\stackrel{\·}{\mathrm{\μ}}=\frac{V_E}{(R_e+h)\cos \left(\mathrm{\λ}\right)}---\left(5\right)$

$\stackrel{\·}{H}=-V_D---\left(6\right)$

其中，为经纬度及高度的微分值，R_n为参考椭球子午面的曲率半径，R_e为垂直子午面的法线平面内的曲率半径，h为INS中惯性高度和高度表高度的加权非线性估计，即

h＝(h_a)^α(h_i)^1-α                (7)

步骤(4)，提取INS中陀螺仪输出的角速率，考虑地球自转角速率及载体相对地球表面运动的影响，修正INS原始输出的三轴角速率值，采用四元数法表示载体的姿态，并建立四元数微分方程，计算四元数微分值。此步骤中，在计算四元数微分方程前，由于陀螺仪输出的角速率包含了地球自转以及载体对于地球表面相对运动的影响，因此需要对陀螺仪输出的角速率进行修正，其角速率修正方程为：

$\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]={\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]}_m-C_n^b[\mathrm{\Ω}+{\mathrm{\ω}}^{\′}]---\left(8\right)$

上式中， ${\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]}_m$ 为INS的原始输出角速率， $\mathrm{\Ω}=\left[\begin{array}{cc}{\mathrm{\ω}}_e& \cos \left(\mathrm{\λ}\right)\\ & 0\\ -{\mathrm{\ω}}_e& \sin \left(\mathrm{\λ}\right)\end{array}\right]$ 为地球自转角速率影响， ${\mathrm{\ω}}^{\′}=\left[\begin{array}{c}\stackrel{\·}{\mathrm{\μ}}\cos \left(\mathrm{\λ}\right)\\ -\stackrel{\·}{\mathrm{\λ}}\\ -\stackrel{\·}{\mathrm{\μ}}\sin \left(\mathrm{\λ}\right)\end{array}\right]$ 为载体相对地球运动产生的影响，为当前的姿态矩阵。ω_e为地球自转角速率，为步骤(3)中计算得到的经度与纬度的微分值。

然后采用四元数法建立四元数微分方程解算INS姿态矩阵，四元数微分方程为：

$\left[\begin{array}{c}\stackrel{\·}{q_0}\\ \stackrel{\·}{q_1}\\ \stackrel{\·}{q_2}\\ \stackrel{\·}{q_3}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bx}}& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]---\left(9\right)$

其中，表示四元数微分值，q表示上一时刻的四元数值，ω_bx、ω_by、ω_bz为修正后的角速率。

步骤(5)，提取INS中加速度计输出的三轴加速度数据，计算载体在自身坐标系下沿自身轴系的速度微分方程：

${\stackrel{\·}{V}}_{\mathrm{bx}}=a_x+V_{\mathrm{by}}{\mathrm{\ω}}_{\mathrm{bz}}-V_{\mathrm{bz}}{\mathrm{\ω}}_{\mathrm{by}}+g\sin \left(\mathrm{\θ}\right)---\left(10\right)$

${\stackrel{\·}{V}}_{\mathrm{by}}=a_y-V_{\mathrm{bx}}{\mathrm{\ω}}_{\mathrm{bz}}+V_{\mathrm{bz}}{\mathrm{\ω}}_{\mathrm{bx}}+g\cos \left(\mathrm{\θ}\right)\sin \left(\mathrm{\Φ}\right)---\left(11\right)$

${\stackrel{\·}{V}}_{\mathrm{bz}}=a_z+V_{\mathrm{bx}}{\mathrm{\ω}}_{\mathrm{by}}-V_{\mathrm{by}}{\mathrm{\ω}}_{\mathrm{bx}}+g\cos \left(\mathrm{\θ}\right)\cos \left(\mathrm{\Φ}\right)---\left(12\right)$

其中，a_x、a_y、a_z为加速度计输出的加速度数据，ω_bx、ω_bx、ω_bx为步骤(4)修正后的角速率，g为当地的重力加速度值，Φ、θ为当前的姿态角。

对于上述步骤(2)至步骤(5)中涉及的所有微分方程，包括：四元数微分方程、速度微分方程和位置微分方程，本发明统一采用四阶龙格-库塔算法进行求解，计算任一时刻载体的姿态角、速度和位置的变化量，并更新相应的姿态、速度及位置数据。所述的采用四阶龙格-库塔算法解算微分方程，算法中涉及到角速率以及加速度值，由于采样间隔极短，在计算中可统一采用角速率值和加速度值以当前的输入值。所述的采用四元数法表示载体的姿态，在姿态四元数更新后，还需对四元数进行规范化处理。

$X(t+T)=X\left(t\right)+\frac{1}{6}(k_1+{2k}_2+{2k}_3+k_4)$

其中有

$\left\{\begin{array}{c}{k}_1=\frac{T}{2}f[X\left(t\right),V\left(t\right)]\\ k_2=\frac{T}{2}f[X\left(t\right)+\frac{k_1}{2},v(t+\frac{T}{2})]\\ k_3=\frac{T}{2}f[X\left(t\right)+\frac{k_2}{2},v(t+\frac{T}{2})]\\ k_4=\frac{T}{2}f[X\left(t\right)+k_3,v(t+T)]\end{array}\right.---\left(13\right)$

上式12-13中，X＝[q₀ q₁ q₂ q₃ V_bx V_by V_bz λ μ h]^T为当前的状态量，V＝[ω_bx ω_by ω_bz a_x a_y a_z]为***当前的角速率及加速度，T为采用周期，f为微分方程的解算过程。

(6)修正完成后，重复步骤2-5，完成INS原始数据的解算。

S3，采用动态误差模型建立9阶扩展卡尔曼滤波器，融合INS与GPS数据，反馈矫正INS的解算值，输出最佳的位置和姿态数据。

如图4所示为本发明中INS/GPS组合***卡尔曼滤波流程图，采用了***中的位置误差、速度误差和姿态误差为状态向量，通过动态误差模型建立了扩展卡尔曼滤波模型，其具体实现方式如下所述：

(1)***初始化，主要对卡尔曼滤波中涉及的变量及矩阵进行初始化，包括初始化状态向量协方差矩阵P(0|0)，状态转移矩阵Φ，观测矩阵H，过程噪声协方差Q，测量噪声协方差R_k。

(2)由于INS与GPS的数据采集频率不同，INS的数据采集频率大于GPS的数据采集频率，对INS与GPS的融合滤波只需在有GPS数据的时刻进行，其他时刻均采用INS数据解算结果，当判断某一时刻有GPS数据时，执行卡尔曼滤波算法。

(3)INS/GPS组合***属于非线性***，根据***中各项误差的传播特点，即：在充分考虑***中存在的位置误差、速度误差以及姿态误差的前提下，采用动态误差模型构建了9阶扩展卡尔曼滤波模型，其***中各状态量的扰动方程表示如下：

${\hat{r}}^n=r^n+{\mathrm{\δr}}^n---\left(14\right)$

${\hat{v}}^n=v^n+{\mathrm{\δv}}^n---\left(15\right)$

$\widehat{C_b^n}=(I-E^n)C_b^n---\left(16\right)$

其中，rⁿ、vⁿ为地理坐标系下载体的位置向量与速度向量，为载体坐标系转换为地理坐标系的姿态余弦矩阵，Eⁿ为姿态误差(εⁿ)的反对称形式。^表示测量值、δ表示误差值，有

$E^n=({\mathrm{\ϵ}}^n\×)=\left[\begin{array}{ccc}0& -{\mathrm{\ϵ}}_D& {\mathrm{\ϵ}}_E\\ {\mathrm{\ϵ}}_D& 0& -{\mathrm{\ϵ}}_N\\ -{\mathrm{\ϵ}}_E& {\mathrm{\ϵ}}_N& 0\end{array}\right]---\left(17\right)$

由于INS/GPS组合***为非线性***，根据位置、速度和姿态的微分方程，建立各自的误差动态方程，线性化且只保留一阶项后的位置、速度及姿态误差动态方程可表示如下

$\mathrm{\δ}{\stackrel{\·}{r}}^n=F_{\mathrm{rr}}{\mathrm{\δr}}^n+F_{\mathrm{rv}}{\mathrm{\δv}}^n---\left(18\right)$

$\mathrm{\δ}{\stackrel{\·}{v}}^n=F_{\mathrm{vr}}{\mathrm{\δr}}^n+F_{\mathrm{vv}}{\mathrm{\δv}}^n+(f^n\×){\mathrm{\ϵ}}^n+C_b^n{\mathrm{\δf}}^b---\left(19\right)$

${\stackrel{\·}{\mathrm{\ϵ}}}^n=F_{\mathrm{er}}{\mathrm{\δr}}^n+F_{\mathrm{ev}}{\mathrm{\δv}}^n+((\mathrm{\Ω}+{\mathrm{\ω}}^{\text{\′}})\×){\mathrm{\ϵ}}^n-C_b^n{\mathrm{\δ\ω}}_{\mathrm{ib}}^b---\left(20\right)$

其中，式(18)为位置误差动态方程，式(19)为速度误差动态方程，式(20)为姿态误差动态方程。为位置、速度和姿态误差向量的微分值，为载体在导航坐标系下沿各轴系的加速度，δf^b为加速度计的扰动误差，为惯性坐标系与载体坐标系的角速率扰动误差，F为各误差动态方程的雅阁比矩阵，有：

$F_{\mathrm{rr}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\λ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{h}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{h}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{h}}{\∂h}\end{array}\right]=\left[\begin{array}{ccc}0& 0& \frac{-V_N}{{(R_n+h)}^2}\\ \frac{V_e\sin \mathrm{\λ}}{(R_e+h){\cos }^2\mathrm{\λ}}& 0& \frac{-V_E}{{(R_e+h)}^2\cos \mathrm{\λ}}\\ 0& 0& 0\end{array}\right]---\left(21\right)$

$F_{\mathrm{rv}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\λ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{h}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{h}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{h}}{{\∂V}_D}\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{R_e+h}& 0& 0\\ 0& \frac{1}{(R_e+h)\cos \mathrm{\λ}}& 0\\ 0& 0& -1\end{array}\right]---\left(22\right)$

$F_{\mathrm{vr}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{V_N}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{V_N}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{V_N}}{\∂h}\\ \frac{\∂\stackrel{\·}{V_E}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{V_E}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{V_E}}{\∂h}\\ \frac{\∂\stackrel{\·}{V_D}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{V_D}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{V_D}}{\∂h}\end{array}\right]=\left[\begin{array}{ccc}-{2V}_E\mathrm{\Ω}\cos \mathrm{\λ}-\frac{V_E^2}{(R_n+h){\cos }^2\mathrm{\λ}}& 0& \frac{-V_N{V}_D}{{(R_n+h)}^2}+\frac{V_E^2\tan \mathrm{\λ}}{{(R_n+h)}^2}\\ 2\mathrm{\Ω}(V_N\cos \mathrm{\λ}-V_D\sin \mathrm{\λ})+\frac{V_E{V}_N}{(R_e+h){\cos }^2\mathrm{\λ}}& 0& \frac{V_E{V}_D}{{(R_e+h)}^2}-\frac{V_N{V}_E\tan \mathrm{\λ}}{{(R_e+h)}^2}\\ 2V_E\mathrm{\Ω}\sin \mathrm{\λ}& 0& \frac{V_E^2+V_N^2}{{(R_e+h)}^2}-\frac{2\mathrm{\γ}}{(R_e+h)}\end{array}\right]---\left(23\right)$

$F_{\mathrm{vv}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{V_N}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{V_N}}{\∂V_E}& \frac{\∂\stackrel{\·}{V_N}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{V_E}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{V_E}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{V_E}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{V_D}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{V_D}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{V_D}}{{\∂V}_D}\end{array}\right]=\left[\begin{array}{ccc}\frac{V_D}{R_n+h}& -2\mathrm{\Ω}\sin \mathrm{\λ}-2\frac{V_E\tan \mathrm{\λ}}{R_n+h}& \frac{V_N}{R_n+h}\\ 2\mathrm{\Ω}\sin \mathrm{\λ}+\frac{V_E\tan \mathrm{\λ}}{R_e+h}& \frac{V_D+V_N\tan \mathrm{\λ}}{R_e+h}& 2\mathrm{\Ω}\cos \mathrm{\λ}+\frac{V_E}{R_e+h}\\ -2\frac{V_N}{R_e+h}& -2\mathrm{\Ω}\cos \mathrm{\λ}-2\frac{V_E}{R_e+h}& 0\end{array}\right]---\left(24\right)$

$F_{\mathrm{er}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{\∂h}\end{array}\right]=\left[\begin{array}{ccc}-\mathrm{\Ω}\sin \mathrm{\λ}& 0& \frac{{-V}_E}{{(R_e+h)}^2}\\ 0& 0& \frac{V_N}{{(R_e+h)}^2}\\ -\mathrm{\Ω}\cos \mathrm{\λ}-\frac{V_E}{(R_n+h)c{\mathrm{os}}^2\mathrm{\λ}}& 0& \frac{V_E\tan \mathrm{\λ}}{{(R_e+h)}^2}\end{array}\right]---\left(25\right)$

$F_{\mathrm{ev}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{{\∂V}_D}\end{array}\right]=\left[\begin{array}{ccc}0& \frac{1}{R_e+h}& 0\\ \frac{-1}{R_n+h}& 0& 0\\ 0& \frac{-\tan \mathrm{\λ}}{R_e+h}& 0\end{array}\right]---\left(26\right)$

其中，R_n为参考椭球子午面内的曲率半径，R_e为垂直子午面的法线平面内的曲率半径，T＝[0 0 γ]^T为当地地球重力矢量，γ₀为h＝0的正常重力值g₀。

(4)根据误差动态方程建立整个***随时间离散的状态预测方程为：

x_k+1Φ_kx_k+w_k                  (27)

其中，Φ_k＝exp(FΔt)I+FΔt为状态转移矩阵，w_k＝Gu为采样间隔内的高斯白噪声的影响。x＝[δrⁿ δvⁿ ∈ⁿ]^T为***的状态矩阵， $F=$ $\left[\begin{array}{ccc}{F}_{\mathrm{rr}}& F_{\mathrm{rv}}& 0\\ F_{\mathrm{vr}}& F_{\mathrm{vv}}& (f^b\×)\\ F_{\mathrm{er}}& F_{\mathrm{ev}}& -((\mathrm{\Ω}+{\mathrm{\ω}}^{\′})\×)\end{array}\right]$ 为***的动态矩阵， $G=\left[\begin{array}{cc}0& 0\\ C_b^n& 0\\ 0& -C_b^n\end{array}\right]$ 为***的噪声驱动矩阵， $u=\left[\begin{array}{c}{\mathrm{\δf}}^b\\ \mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b\end{array}\right]$ 为均值为零的***高斯白噪声。w_k的协方差矩阵为：

$Q_k=E\left[{\mathrm{\ω}}_k{\mathrm{\ω}}_k^T\right]\≈{\mathrm{\Φ}}_k\mathrm{GQG}{\mathrm{\Φ}}_k^T\mathrm{\Δt}---\left(28\right)$

其中， $Q=\mathrm{diag}\left(\begin{array}{cccccc}{\mathrm{\σ}}_{\mathrm{ax}}^2& {\mathrm{\σ}}_{\mathrm{ay}}^2& {\mathrm{\σ}}_{\mathrm{az}}^2& {\mathrm{\σ}}_{\mathrm{\ωx}}^2& {\mathrm{\σ}}_{\mathrm{\ωy}}^2& {\mathrm{\σ}}_{\mathrm{\ωz}}^2\end{array}\right)$ 为***的过程噪声协方差矩阵，σ_a、σ_ω分别为加速度计与陀螺仪的标准差，Δt为采样间隔。

(5)以GPS与INS的位置差为观测向量，并考虑到做差后数值过小，可能会引起卡尔曼增益的变化较大，因此需要在纬度差和经度差上各乘以(R_n+h)和(R_e+h)cosλ以避免卡尔曼增益的较大波动，因此建立***的观测方程为：

$z_k=r_{\mathrm{INS}}^n-r_{\mathrm{GPS}}^n=\left[\begin{array}{c}(R_n+h)({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{GPS}})\\ (R_e+h)\cos \mathrm{\λ}({\mathrm{\μ}}_{\mathrm{INS}}-{\mathrm{\μ}}_{\mathrm{GPS}})\\ h_{\mathrm{INS}}-h_{\mathrm{GPS}}\end{array}\right]---\left(29\right)$

测量噪声协方差为：

$R_k=\mathrm{diag}\left(\begin{array}{ccc}{\mathrm{\σ}}_{\mathrm{\λ}}^2& {\mathrm{\σ}}_{\mathrm{\μ}}^2& {\mathrm{\σ}}_h^2\end{array}\right)---\left(30\right)$

其观测矩阵为：

$H_k=\left[\begin{array}{cccc}(R_n+h)& 0& 0& \\ 0& (R_e+h)\cos \mathrm{\λ}& 0& \left|0_{3\×3}\right|0_{3\×3}\\ 0& 0& 1& \end{array}\right]---\left(31\right)$

以观测值z_k，过程噪声协方差Q以及测量噪声协方差R为输入值，进行卡尔曼滤波，输出对***位置、速度与姿态误差的最优估计值，即δrⁿ，δvⁿ，εⁿ。

具体的，对于***状态预测方程和观测方程，结合过程噪声误差协方差Q和测量噪声协方差R，在给定初始状态估计和协方差矩阵P(0|0)后，即可构建INS/GPS扩展卡尔曼滤波器，计算出任一时刻位置、速度以及姿态的误差最优估计(δrⁿδvⁿεⁿ)。其中，INS/GPS扩展卡尔曼滤波器为：

协方程预测：

$P\left(k\right|k-1)={\mathrm{\Φ}}_{k}P(k-1|k-1){\mathrm{\Φ}}_K^T+Q_k---\left(32\right)$

卡尔曼增益：

$K_k=\frac{P\left(k\right|k-1)H_k^T}{(H_{k}P\left(k\right|k-1)H_k^T+R_k)}---\left(33\right)$

状态更新：

X(k|k)＝X(k|k-1)+K_k(z_k-H_kX(k|k-1))        (34)

协方程更新：

P(k|k)＝(I-K_kH_k)P(k|k-1)                  (35)

最后采用经扩展卡尔曼滤波后的位置、速度及姿态的误差最优估计值采用以下公式修正同一时刻下INS解算出的位置、速度和姿态数据，获取最佳的位置、速度和姿态数据：

$r^n={\hat{r}}^n-{\mathrm{\δr}}^n---\left(36\right)$

$v^n={\hat{v}}^n-\mathrm{\δ}{v}^n---\left(37\right)$

$C_b^n=(I+E^n)\widehat{C_b^n}---\left(38\right)$

其中Eⁿ为姿态误差(εⁿ)的反对称形式。

(6)修正INS解算的位置、速度和姿态数据，并将当前的扩展卡尔曼滤波状态向量置为0，开始下一周期的扩展卡尔曼计算流程。最后输出***最优的位置和姿态数据。

本发明针对目前INS/GPS组合导航***中采用的扩展卡尔曼滤波算法大多直接将***当前的位置、速度、姿态作为状态向量，从而存在线性化截断误差较大、雅阁比矩阵计算繁琐、各项矩阵数值较大而导致的算法内存占用量大、执行时间冗长和算法效率低，以及未充分考虑误差传播对***状态的影响等不足，采用了以***各项误差为状态向量，使用误差动态方程建立了9阶扩展卡尔曼滤波算法。本发明可以在一定程度上减少***在线性化中带来的截断误差，提高***精度，减少数据运算量，减少了内存消耗，提高了扩展卡尔曼滤波算法的执行效率。

扩展卡尔曼滤波器，由于INS的数据采集频率大于GPS的数据采集频率，只需在有GPS数据的时刻进行扩展卡尔曼滤波，修正INS的解算值，其他时刻均采用INS的解算值即可。并且在扩展卡尔曼滤波对INS解算值反馈矫正后，为防止各项误差值的累积，需将扩展卡尔曼滤波的状态向量置为0。

本发明的有益效果可以总结如下：

(1)本发明以INS/GPS组合***的各项误差为状态向量，通过误差传播的动态方程建立了9阶卡尔曼滤波模型，大大减小了模型线性化带来的截断误差，以及***中存在的各类误差影响，提高了***精度。

(2)本发明涉及的状态量较少，计算过程中的均没有大量级的数值出现，减少了运算量，节省了计算中的内存消耗，计算周期短，效率高。

(3)本发明对硬件的要求较低，可应用于低成本的INS/GPS组合***，实时获取较高精度的位置和姿态数据，节省作业成本。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视本发明的保护范围。

Claims (5)

1.一种INS/GPS组合定位定姿新方法，其特征在于，包括以下步骤：

S1，采用线性卡尔曼滤波器对GPS原始测量数据进行滤波估计，输出GPS导航最优估计值；其中，所述GPS导航最优估计值包括位置最优估计值和速度最优估计值；

S2，以所述位置最优估计值向INS提供初始位置信息，以所述速度最优估计值向INS提供初始速度信息，对INS原始测量数据进行解算，得到INS导航信息；其中，所述INS导航信息包括姿态、位置和速度信息；

S3，由于INS的数据采集频率大于GPS的数据采集频率，当没有GPS原始测量数据时，所述INS导航信息即为最终输出的数据；

当有GPS原始测量数据时，采用动态误差模型建立9阶扩展卡尔曼滤波器，融合INS导航信息与GPS导航最优估计值，反馈矫正同一时刻下的所述INS导航信息，输出修正融合后的最佳位置数据和姿态数据。

2.根据权利要求1所述的INS/GPS组合定位定姿新方法，其特征在于，S1具体包括以下步骤：

S1.1，采用实时差分或事后差分的方式采集获得所述GPS原始测量数据；

S1.2，从所述GPS原始测量数据中提取经度、纬度、高度、地速和航向信息，并将提取的球面坐标系下的经度和纬度数据转换为UTM投影坐标系下的空间平面坐标(X，Y)；

S1.3，假设运动载体做匀速直线运动且不考虑载体自身的动力因素，以载体的空间平面坐标(X，Y)与平面速度(V_x，V_y)为状态向量，以GPS的空间二维平面坐标和地速为观测向量，建立所述线性卡尔曼滤波器，所述线性卡尔曼滤波器的状态方程如式1，其观测方程见式2：

X(k+1)＝ΦX(k)+Γu(k)            (1)

Z(k)＝HX(k)+v(k)                 (2)

其中，k表示离散化时刻，X＝[X，Y，V_x，V_y]^T，Z＝[X，Y，V_x，V_y]^T， $\mathrm{\Φ}=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ 为状态转移矩阵， $\mathrm{\Γ}=\left[\begin{array}{cc}{0.5T}^2& 0\\ T& 0\\ 0& 0.5T^2\\ 0& T\end{array}\right]$ 为控制向量矩阵， $H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ 为观测矩阵，V_x＝V×|sin(h)|，V_y＝V×|cos(h)|。u，v为零均值的过程噪声和观测噪声，T为采样周期，V为GPS观测的地速，h为以真北为参考基准的地面航向；

S1.4，采用GPS接收机差分定位精度作为过程噪声协方程Q，以GPS测量精度为观测噪声协方差R，以S1.2转换后的平面坐标(X，Y)以及高度、地速和航向信息作为线性卡尔曼滤波器初始输入值，给定初始值X(0|0)及初始协方程P(0|0)后，对GPS原始测量数据进行卡尔曼滤波估计，待状态量更新后，输出位置及速度的最优估计值；其中，所述位置最优估计值包括转换到地理坐标系下的经纬度值以及GPS高程值，即：经度μ、纬度λ和高程值h；所述速度最优估计值为载体地速V。

3.根据权利要求2所述的INS/GPS组合定位定姿新方法，其特征在于，S2具体包括以下步骤：

S2.1，获取INS数据解算必要的初始信息，包括：线性卡尔曼滤波器输出的位置最优估计值作为载体初始位置信息(μ，λ，h)、线性卡尔曼滤波器输出的速度最优估计值作为载体沿自身轴向的初始速度信息(V_bx、V_by、V_bz)和载体初始姿态角数据(Φ，θ，φ)；其中，横滚角为Φ、俯仰角为θ、航向角为φ；

S2.2，根据S2.1获取的初始信息，对***进行初始化，包括载***置和速度的初始化，使用初始姿态角数据初始化姿态矩阵并计算初始四元数(q₀、q₁、q₂、q₃)；

S2.3，使用***当前的载体速度(V_bx、V_by、V_bz)和姿态矩阵信息，解算载体经度、纬度、高程的位置微分方程，得到经纬度及高度的微分值；此步骤中采用式3将载体坐标系下的速度值转换为导航坐标系下的速度：

$\left[\begin{array}{c}{V}_N\\ V_E\\ V_D\end{array}\right]={\left(C_n^b\right)}^T\left[\begin{array}{c}{V}_{\mathrm{bx}}\\ V_{\mathrm{by}}\\ V_{\mathrm{bz}}\end{array}\right]---\left(3\right)$

其中，导航坐标系下运动载体沿坐标轴系北向速度为V_N、东向速度为V_E向下速度为V_D；

并且，经度、纬度、高程的微分方程分别见式4、式5和式6；

$\stackrel{\·}{\mathrm{\λ}}=\frac{V_N}{R_n+h}---\left(4\right)$

$\stackrel{\·}{\mathrm{\μ}}=\frac{V_E}{(R_e+h)\cos \left(\mathrm{\λ}\right)}---\left(5\right)$

$\stackrel{\·}{H}=-V_D---\left(6\right)$

其中，为经纬度及高度的微分值，R_n为参考椭球子午面的曲率半径，R_e为垂直子午面的法线平面内的曲率半径，h为INS中惯性高度和高度表高度的加权非线性估计，即

h＝(h_a)^α(h_i)^1-α                (7)

S2.4，提取INS中陀螺仪输出的角速率，根据地球自转角速率及载体相对地球表面运动的影响，采用式8修正INS原始输出的三轴角速率值：

$\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]={\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]}_m-C_n^b[\mathrm{\Ω}+{\mathrm{\ω}}^{\′}]---\left(8\right)$

上式中， ${\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{bz}}\end{array}\right]}_m$ 为INS的原始输出角速率， $\mathrm{\Ω}=\left[\begin{array}{cc}{\mathrm{\ω}}_e& \cos \left(\mathrm{\λ}\right)\\ & 0\\ -{\mathrm{\ω}}_e& \sin \left(\mathrm{\λ}\right)\end{array}\right]$ 为地球自转角速率影响， ${\mathrm{\ω}}^{\′}=\left[\begin{array}{c}\stackrel{\·}{\mathrm{\μ}}\cos \left(\mathrm{\λ}\right)\\ -\stackrel{\·}{\mathrm{\λ}}\\ -\stackrel{\·}{\mathrm{\μ}}\sin \left(\mathrm{\λ}\right)\end{array}\right]$ 为载体相对地球运动产生的影响，为当前的姿态矩阵。ω_e为地球自转角速率，为步骤S2.3中计算得到的经度与纬度的微分值；

然后采用四元数法建立四元数微分方程解算INS姿态矩阵，四元数微分方程为：

$\left[\begin{array}{c}\stackrel{\·}{q_0}\\ \stackrel{\·}{q_1}\\ \stackrel{\·}{q_2}\\ \stackrel{\·}{q_3}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{bx}}& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]---\left(9\right)$

其中，表示四元数微分值，q表示上一时刻的四元数值，ω_bx、ω_by、ω_bz为修正后的角速率；

S2.5，提取INS中加速度计输出的三轴加速度数据，，计算载体在自身坐标系下沿自身轴系的速度微分方程：

${\stackrel{\·}{V}}_{\mathrm{bx}}=a_x+V_{\mathrm{by}}{\mathrm{\ω}}_{\mathrm{bz}}-V_{\mathrm{bz}}{\mathrm{\ω}}_{\mathrm{by}}+g\sin \left(\mathrm{\θ}\right)---\left(10\right)$

${\stackrel{\·}{V}}_{\mathrm{by}}=a_y-V_{\mathrm{bx}}{\mathrm{\ω}}_{\mathrm{bz}}+V_{\mathrm{bz}}{\mathrm{\ω}}_{\mathrm{bx}}+g\cos \left(\mathrm{\θ}\right)\sin \left(\mathrm{\Φ}\right)---\left(11\right)$

${\stackrel{\·}{V}}_{\mathrm{bz}}=a_z+V_{\mathrm{bx}}{\mathrm{\ω}}_{\mathrm{by}}-V_{\mathrm{by}}{\mathrm{\ω}}_{\mathrm{bx}}+g\cos \left(\mathrm{\θ}\right)\cos \left(\mathrm{\Φ}\right)---\left(12\right)$

其中，a_x、a_y、a_z为加速度计输出的加速度数据，ω_bx、ω_bx、ω_bx为S2.4修正后的角速率，g为当地的重力加速度值，Φ、θ为当前的姿态角。

S2.6，采用四阶龙哥-库塔算法解算四元数微分方程、速度微分方程以及位置微分方程，计算任一时刻载体的姿态角、速度和位置的变化量，并更新相应的姿态、速度及位置数据；

S2.7，重复步骤S2.3～S2.6，直到INS原始数据全部解算完毕。

4.根据权利要求3所述的INS/GPS组合定位定姿新方法，其特征在于，S2.6中，对经四阶龙格-库塔解算后的四元数微分值，在对四元数更新后需对当前四元数进行规范化处理。

5.根据权利要求1所述的INS/GPS组合定位定姿新方法，其特征在于，S3中，当有GPS原始测量数据时，通过以下方法得到所述最佳位置数据和姿态数据：

S3.1，***中各状态量的扰动方程表示如下：

${\hat{r}}^n=r^n+{\mathrm{\δr}}^n---\left(14\right)$

${\hat{v}}^n=v^n+{\mathrm{\δv}}^n---\left(15\right)$

$\widehat{C_b^n}=(I-E^n)C_b^n---\left(16\right)$

其中，rⁿ、vⁿ为地理坐标系下载体的位置向量与速度向量，为载体坐标系转换为地理坐标系的姿态余弦矩阵，Eⁿ为姿态误差(εⁿ)的反对称形式。^表示测量值、δ表示误差值，有：

$E^n=({\mathrm{\ϵ}}^n\×)=\left[\begin{array}{ccc}0& -{\mathrm{\ϵ}}_D& {\mathrm{\ϵ}}_E\\ {\mathrm{\ϵ}}_D& 0& -{\mathrm{\ϵ}}_N\\ -{\mathrm{\ϵ}}_E& {\mathrm{\ϵ}}_N& 0\end{array}\right]---\left(17\right)$

S3.2，由于INS/GPS组合***为非线性***，根据位置、速度和姿态的微分方程，建立各自的误差动态方程，线性化且只保留一阶项后的位置、速度及姿态误差动态方程表示如下

$\mathrm{\δ}{\stackrel{\·}{r}}^n=F_{\mathrm{rr}}{\mathrm{\δ}}^n+F_{\mathrm{rv}}{\mathrm{\δv}}^n---\left(18\right)$

$\mathrm{\δ}{\stackrel{\·}{v}}^n=F_{\mathrm{vr}}{\mathrm{\δr}}^n+F_{\mathrm{vv}}{\mathrm{\δv}}^n+(f^n\×){\mathrm{\ϵ}}^n+C_b^n{\mathrm{\δf}}^b---\left(19\right)$

${\stackrel{\·}{\mathrm{\ϵ}}}^n=F_{\mathrm{er}}{\mathrm{\δr}}^n+F_{\mathrm{ev}}{\mathrm{\δv}}^n+((\mathrm{\Ω}+{\mathrm{\ω}}^{\text{\′}})\×){\mathrm{\ϵ}}^n-C_b^n{\mathrm{\δ\ω}}_{\mathrm{ib}}^b---\left(20\right)$

其中，式(18)为位置误差动态方程，(19)为速度误差动态方程，(20)为姿态误差动态方程；为位置、速度和姿态误差向量的微分值， 为载体在导航坐标系下沿各轴系的加速度，δf^b为加速度计的扰动误差，为惯性坐标系与载体坐标系的角速率扰动误差，F为各误差动态方程的雅阁比矩阵，有：

$F_{\mathrm{rr}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\λ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{h}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{h}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{h}}{\∂h}\end{array}\right]=\left[\begin{array}{ccc}0& 0& \frac{-V_N}{{(R_n+h)}^2}\\ \frac{V_e\sin \mathrm{\λ}}{(R_e+h){\cos }^2\mathrm{\λ}}& 0& \frac{-V_E}{{(R_e+h)}^2\cos \mathrm{\λ}}\\ 0& 0& 0\end{array}\right]---\left(21\right)$

$F_{\mathrm{rv}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\λ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\λ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\μ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{h}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{h}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{h}}{{\∂V}_D}\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{R_e+h}& 0& 0\\ 0& \frac{1}{(R_e+h)\cos \mathrm{\λ}}& 0\\ 0& 0& -1\end{array}\right]---\left(22\right)$

$F_{\mathrm{vr}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{V_N}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{V_N}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{V_N}}{\∂h}\\ \frac{\∂\stackrel{\·}{V_E}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{V_E}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{V_E}}{\∂h}\\ \frac{\∂\stackrel{\·}{V_D}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{V_D}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{V_D}}{\∂h}\end{array}\right]=\left[\begin{array}{ccc}-{2V}_E\mathrm{\Ω}\cos \mathrm{\λ}-\frac{V_E^2}{(R_n+h){\cos }^2\mathrm{\λ}}& 0& \frac{-V_N{V}_D}{{(R_n+h)}^2}+\frac{V_E^2\tan \mathrm{\λ}}{{(R_n+h)}^2}\\ 2\mathrm{\Ω}(V_N\cos \mathrm{\λ}-V_D\sin \mathrm{\λ})+\frac{V_E{V}_N}{(R_e+h){\cos }^2\mathrm{\λ}}& 0& \frac{V_E{V}_D}{{(R_e+h)}^2}-\frac{V_N{V}_E\tan \mathrm{\λ}}{{(R_e+h)}^2}\\ 2V_E\mathrm{\Ω}\sin \mathrm{\λ}& 0& \frac{V_E^2+V_N^2}{{(R_e+h)}^2}-\frac{2\mathrm{\γ}}{(R_e+h)}\end{array}\right]---\left(23\right)$

$F_{\mathrm{vv}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{V_N}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{V_N}}{\∂V_E}& \frac{\∂\stackrel{\·}{V_N}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{V_E}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{V_E}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{V_E}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{V_D}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{V_D}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{V_D}}{{\∂V}_D}\end{array}\right]=\left[\begin{array}{ccc}\frac{V_D}{R_n+h}& -2\mathrm{\Ω}\sin \mathrm{\λ}-2\frac{V_E\tan \mathrm{\λ}}{R_n+h}& \frac{V_N}{R_n+h}\\ 2\mathrm{\Ω}\sin \mathrm{\λ}+\frac{V_E\tan \mathrm{\λ}}{R_e+h}& \frac{V_D+V_N\tan \mathrm{\λ}}{R_e+h}& 2\mathrm{\Ω}\cos \mathrm{\λ}+\frac{V_E}{R_e+h}\\ -2\frac{V_N}{R_e+h}& -2\mathrm{\Ω}\cos \mathrm{\λ}-2\frac{V_E}{R_e+h}& 0\end{array}\right]---\left(24\right)$

$F_{\mathrm{er}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{\∂h}\\ \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{\∂\mathrm{\λ}}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{\∂\mathrm{\μ}}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{\∂h}\end{array}\right]=\left[\begin{array}{ccc}-\mathrm{\Ω}\sin \mathrm{\λ}& 0& \frac{{-V}_E}{{(R_e+h)}^2}\\ 0& 0& \frac{V_N}{{(R_e+h)}^2}\\ -\mathrm{\Ω}\cos \mathrm{\λ}-\frac{V_E}{(R_n+h)c{\mathrm{os}}^2\mathrm{\λ}}& 0& \frac{V_E\tan \mathrm{\λ}}{{(R_e+h)}^2}\end{array}\right]---\left(25\right)$

$F_{\mathrm{ev}}=\left[\begin{array}{ccc}\frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\Φ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\θ}}}{{\∂V}_D}\\ \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{{\∂V}_N}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{{\∂V}_E}& \frac{\∂\stackrel{\·}{\mathrm{\ψ}}}{{\∂V}_D}\end{array}\right]=\left[\begin{array}{ccc}0& \frac{1}{R_e+h}& 0\\ \frac{-1}{R_n+h}& 0& 0\\ 0& \frac{-\tan \mathrm{\λ}}{R_e+h}& 0\end{array}\right]---\left(26\right)$

其中，R_n为参考椭球子午面内的曲率半径，R_e为垂直子午面的法线平面内的曲率半径，γ＝[0 0 γ]^T为当地地球重力矢量，γ₀为h＝0的正常重力值g₀；

S3.3，根据误差动态方程建立整个***随时间离散的状态预测方程为：

x_k+1＝Φ_kx_k+w_k                  (27)

其中，Φ_k＝exp(FΔt)≈I+FΔt为状态转移矩阵，w_k＝Gu为采样间隔内的高斯白噪声的影响。x＝[δrⁿ δvⁿ ∈ⁿ]^T为***的状态矩阵， $F=$ $\left[\begin{array}{ccc}{F}_{\mathrm{rr}}& F_{\mathrm{rv}}& 0\\ F_{\mathrm{vr}}& F_{\mathrm{vv}}& (f^b\×)\\ F_{\mathrm{er}}& F_{\mathrm{ev}}& -((\mathrm{\Ω}+{\mathrm{\ω}}^{\′})\×)\end{array}\right]$ 为***的动态矩阵， $G=\left[\begin{array}{cc}0& 0\\ C_b^n& 0\\ 0& -C_b^n\end{array}\right]$ 为***的噪声驱动矩阵， $u=\left[\begin{array}{c}{\mathrm{\δf}}^b\\ \mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b\end{array}\right]$ 为均值为零的***高斯白噪声。w_k的协方差矩阵为：

$Q_k=E\left[{\mathrm{\ω}}_k{\mathrm{\ω}}_k^T\right]\≈{\mathrm{\Φ}}_k\mathrm{GQG}{\mathrm{\Φ}}_k^T\mathrm{\Δt}---\left(28\right)$

其中， $Q=\mathrm{diag}\left(\begin{array}{cccccc}{\mathrm{\σ}}_{\mathrm{ax}}^2& {\mathrm{\σ}}_{\mathrm{ay}}^2& {\mathrm{\σ}}_{\mathrm{az}}^2& {\mathrm{\σ}}_{\mathrm{\ωx}}^2& {\mathrm{\σ}}_{\mathrm{\ωy}}^2& {\mathrm{\σ}}_{\mathrm{\ωz}}^2\end{array}\right)$ 为***的过程噪声协方差矩阵，σ_a、σ_ω分别为加速度计与陀螺仪的标准差，Δt为采样间隔；

S3.4，以GPS与INS的位置差为观测向量，建立***的观测方程为：

$z_k=r_{\mathrm{INS}}^n-r_{\mathrm{GPS}}^n=\left[\begin{array}{c}(R_n+h)({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{GPS}})\\ (R_e+h)\cos \mathrm{\λ}({\mathrm{\μ}}_{\mathrm{INS}}-{\mathrm{\μ}}_{\mathrm{GPS}})\\ h_{\mathrm{INS}}-h_{\mathrm{GPS}}\end{array}\right]---\left(29\right)$

测量噪声协方差为：

$R_k=\mathrm{diag}\left(\begin{array}{ccc}{\mathrm{\σ}}_{\mathrm{\λ}}^2& {\mathrm{\σ}}_{\mathrm{\μ}}^2& {\mathrm{\σ}}_h^2\end{array}\right)---\left(30\right)$

其观测矩阵为：

$H_k=\left[\begin{array}{cccc}(R_n+h)& 0& 0& \\ 0& (R_e+h)\cos \mathrm{\λ}& 0& \left|0_{3\×3}\right|0_{3\×3}\\ 0& 0& 1& \end{array}\right]---\left(31\right)$

S3.5，根据状态预测方程和观测方程，结合过程噪声误差协方差Q和测量噪声协方差R，在给定初始状态估计和协方差矩阵P(0|0)后，构建得到INS/GPS扩展卡尔曼滤波器，计算出任一时刻位置、速度以及姿态的误差最优估计(δrⁿ δvⁿ εⁿ)；

其中，INS/GPS扩展卡尔曼滤波器为：

协方程预测：

$P\left(k\right|k-1)={\mathrm{\Φ}}_{k}P(k-1|k-1){\mathrm{\Φ}}_K^T+Q_k---\left(32\right)$

卡尔曼增益：

$K_k=\frac{P\left(k\right|k-1)H_k^T}{(H_{k}P\left(k\right|k-1)H_k^T+R_k)}---\left(33\right)$

状态更新：

X(k|k)＝X(k|k-1)+K_k(z_k-H_kX(k|k-1))        (34)

协方程更新：

P(k|k)＝(I-K_kH_k)P(k|k-1)                  (35)

S3.6，采用经扩展卡尔曼滤波后的位置、速度及姿态的误差最优估计值采用以下公式修正同一时刻下INS解算出的位置、速度和姿态数据，获取最佳的位置、速度和姿态数据：

$r^n={\hat{r}}^n-{\mathrm{\δr}}^n---\left(36\right)$

$v^n={\hat{v}}^n-\mathrm{\δ}{v}^n---\left(37\right)$

$C_b^n=(I+E^n)\widehat{C_b^n}---\left(38\right)$

其中Eⁿ为姿态误差(εⁿ)的反对称形式。