一种实现陆用惯性导航系统运动对准的方法

摘要

本发明涉及一种实现陆用惯性导航系统运动对准的方法，属于惯性导航系统初始对准技术领域。本发明在惯性导航系统动态误差模型和观测方程的基础上，以GPS输出速度作为观测量，采用卡尔曼滤波对载车的加加速度进行估计，并判别GPS速度输出是否为野值。若GPS速度输出为野值，采用卡尔曼滤波平滑方法对当前状态进行递推；否则，采用主、从卡尔曼滤波器来实现惯性导航系统状态和噪声统计特性的同步估计，主滤波器对惯性导航系统的状态进行估计，其新息和方差作为从滤波器的观测量；从滤波器对主滤波器观测噪声的均值和方差进行估计，估计结果为下一次主滤波器的滤波提供噪声特性输入；从而实现陆用惯性导航系统的运动对准。

说明书

技术领域

本发明涉及一种实现陆用惯性导航系统运动对准的方法，适用于各类惯性导航系统的初始对准，属于惯性导航系统初始对准技术领域。

背景技术

惯性导航系统是复杂的高精度机电综合系统，由于具有完全自主性的优点而广泛应用于陆海空天领域。惯性导航系统在转入导航状态前需要进行初始对准以提供必要的初始值，依据载体的运动情况划分，对准分为：静基座对准和运动对准。同静基座对准相比，运动对准在缩短对准时间、提高对准精度和机动性等方面具有明显的优势。为实现惯性导航系统在运动过程中的对准，必须引入外部观测信息(包括速度、位置等)。对于陆用车辆而言，通常采用全球定位系统(GPS)作为辅助导航设备来实现惯性导航系统运动对准。

目前已有的GPS辅助惯性导航系统实现运动对准的方法，即传统卡尔曼滤波方法，其原理是根据GPS输出的位置或速度与惯性导航系统的位置或速度之差作为观测量，在惯性导航系统动态误差模型的基础上，采用传统卡尔曼滤波方法对惯性导航系统的误差状态进行估计，对姿态矩阵进行开环或闭环方式反馈，实现运动对准。这种方法的缺点是：在实际应用中，由于车辆的随机振动和行驶环境(如山区、隧道等)的影响，GPS的速度输出带有各种干扰噪声，且干扰信号统计特性不完全已知。对这些受干扰的观测量如果不采取有效的处理方法而直接进行卡尔曼滤波，则不仅会降低当前的估计精度，对于后续的估计效果也会产生影响，严重时会引起滤波发散，导致对准失败。

发明内容

本发明的目的是为了克服已有技术存在的不足，提出一种实现陆用惯性导航系统运动对准的方法。本发明的基本原理是：在惯性导航系统动态误差模型和观测方程的基础上，以GPS输出速度作为观测量，采用卡尔曼滤波对载车的加加速度进行估计，并判别GPS速度输出是否为野值。若GPS速度输出为野值，采用卡尔曼滤波平滑方法对当前状态进行递推；否则，采用主、从卡尔曼滤波器来实现惯性导航系统状态和噪声统计特性的同步估计，主滤波器对惯性导航系统的状态进行估计，其新息和方差作为从滤波器的观测量；从滤波器对主滤波器观测噪声的均值和方差进行估计，估计结果为下一次主滤波器的滤波提供噪声特性输入；从而实现陆用惯性导航系统的运动对准。

本发明的目的是通过下述技术方案实现的。

一种实现陆用惯性导航系统运动对准的方法，包括如下步骤：

步骤一、建立包括位置误差、速度误差、失准角和惯性器件误差漂移的惯性导航系统动态误差模型；动态误差模型为φ角误差方程或ψ角误差方程；

在东北天坐标系下，动态误差模型表示如公式1所示。

$\stackrel{·}{x}\left(t\right)=F\left(t\right)x\left(t\right)+w\left(t\right)---\left(1\right)$

式中，t为时间值，是正实数；x(t)表示惯性导航系统动态误差模型的状态向量，由位置误差δP、速度误差δVⁿ、失准角陀螺仪零偏ε_g和加速度计零偏组成；w(t)表示惯性导航系统动态误差模型的系统噪声；F(t)为转移矩阵；表示惯性导航系统动态误差模型的状态向量的变化量。

对该动态误差模型进行离散化处理，可得：

x_k＝F_k-1x_k-1+w_k                        (2)

式中，k表示时间值，为正整数；F_k-1表示离散化的惯性导航系统的一步转移矩阵，x_k为离散化后的k时刻惯性导航系统的状态向量，w_k表示k时刻的惯性导航系统动态误差模型的系统噪声，是均值为零，方差为Q的白噪声序列，Q值根据实际应用环境人为设定，Q为正实数。

步骤二、建立步骤一所述的惯性导航系统动态误差模型的观测方程，如公式3所示。

$Y_k=V_k^{\mathrm{INS}}-V_k^{\mathrm{GPS}}=H_k{x}_k+{\eta }_k---\left(3\right)$

式中，Y_k表示k时刻的观测量；表示惯性导航系统的速度输出；表示经杆臂补偿后GPS的速度输出；H_k为k时刻的观测矩阵，H_k＝[O_3×3 I_3×3  O_3×9]；O_3×3表示3阶O矩阵；I_3×3表示3阶单位阵，O_3×9表示3行9列的O矩阵；η_k表示k时刻的惯性导航系统动态误差模型的观测方程噪声。

步骤三、对GPS输出进行实时野值检测。

通过GPS测量得到的当前时刻载车东向速度和北向速度计算水平合速度Z_k，并以此作为观测量，建立加加速度跟踪模型，如公式4所示。

$\frac{d}{\mathrm{dt}}\left(\begin{array}{c}v\left(t\right)\\ a\left(t\right)\\ \stackrel{·}{a}\left(t\right)\end{array}\right)=\left(\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right)\left(\begin{array}{c}v\left(t\right)\\ a\left(t\right)\\ \stackrel{·}{a}\left(t\right)\end{array}\right)+\zeta \left(t\right)---\left(4\right)$

式中，t为时间值，为正实数；v(t)表示水平合速度，a(t)和分别表示水平加速度和水平加加速度；ζ(t)表示加加速度跟踪模型的系统噪声，是均值为零，方差为的白噪声序列，值根据实际应用环境人为设定，为正实数。

对公式4进行离散化，离散化后的系统方程表示为：

$\left(\begin{array}{c}{v}_k\\ a_k\\ {\stackrel{·}{a}}_k\end{array}\right)=G_{k-1}\left(\begin{array}{c}{v}_{k-1}\\ a_{k-1}\\ {\stackrel{·}{a}}_{k-1}\end{array}\right)+{\zeta }_k---\left(5\right)$

式中，v_k表示k时刻水平合速度，a_k和分别表示k时刻水平加速度和水平加加速度，G_k-1表示离散化的加加速度跟踪模型的一步转移矩阵，ζ_k表示离散后加加速度跟踪模型的系统噪声。

加加速度跟踪模型的观测方程表示为：

$Z_k=\left(\begin{array}{ccc}1& 0& 0\end{array}\right)\left(\begin{array}{c}{v}_k\\ a_k\\ {\stackrel{·}{a}}_k\end{array}\right)+{\xi }_k---\left(6\right)$

式中，Z_k表示k时刻水平合速度的观测量，其计算公式为：ξ_k表示加加速度跟踪模型的观测方程噪声，是均值为零，方差为的白噪声序列，值根据实际应用环境人为设定，为正实数。

根据公式5和公式6，采用卡尔曼滤波器即可估计出k时刻水平合速度v_k、相应的加速度a_k和加加速度和

根据载车的类型和对准动态情况，预先设定阈值J_max，如果成立，则判断当前的观测量为有效值，执行步骤四；否则，判断其为野值，在该时刻做卡尔曼平滑，同时将k+1的值赋给k，然后重复步骤三。

步骤四、对惯性导航系统动态误差模型的观测方程噪声η_k的均值和方差进行估计；

根据步骤二中的惯性导航系统动态误差模型的观测方程得到的k时刻的观测量Y_k以及观测矩阵H_k，采用主、从两个卡尔曼滤波器分别对k时刻惯性导航系统的状态x_k、k时刻的观测噪声η_k的均值和方差进行估计；具体为：和的状态方程为：

$x_{k+1}^R=\left(\begin{array}{cc}{I}_1& 0\\ 0& I_2\end{array}\right)x_k^R+w_k^R---\left(7\right)$

式中，表示主对角线元素；该状态方程的系统噪声是均值为零，方差为Q_R的白噪声序列，Q_R值根据实际应用环境人为设定，Q_R为正实数；为m₁行1列的向量；I₁为m₁行m₁列的单位阵；为m₂行1列的向量；I₂为m₂行m₂列的单位阵。

公式7对应的观测方程为：

$z_k^R=\left(\begin{array}{c}{Y}_k-H_k{\hat{x}}_k\\ \mathrm{diag}(C_k-H_k{P}_{k|k-1}{H}_k^T)\end{array}\right)---\left(8\right)$

式中，C_k表示残差序列的方差，满足和P_k|k-1的初始值分别为和P₀，均为人为设定值；此后，和P_k|k-1由主卡尔曼滤波器提供；

公式8可进一步整理为：

$z_k^R=\left(\begin{array}{cc}I& 0\\ 0& I\end{array}\right)x_k^R+{\eta }_k^R---\left(9\right)$

式中，量测噪声是均值为零，方差为R_R的白噪声序列，R_R值根据实际应用环境人为设定，R_R为正实数。

根据公式7和公式9，采用从卡尔曼滤波器即可估计出k时刻的观测噪声η_k的均值和方差从卡尔曼滤波器为经典卡尔曼滤波器。

步骤五、对惯性导航系统的状态x_k进行估计；

根据步骤一建立的惯性导航系统动态误差模型及步骤二中的观测方程，结合步骤四给出的观测噪声η_k的均值和方差的估计值，采用主卡尔曼滤波器对惯性导航系统动态误差模型的状态向量x_k进行估计。具体为：

主卡尔曼滤波器为经典卡尔曼滤波器的改进，计算过程如公式10～14所示：

${\hat{x}}_{k|k-1}=F_{k-1}{\hat{x}}_{k-1}---\left(10\right)$

$P_{k|k-1}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+Q_k---\left(11\right)$

$K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+{\hat{x}}_k^R\left(1\right)]}^{-1}---\left(12\right)$

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k[Y_k-F_{k-1}{\hat{x}}_{k|k-1}-{\hat{x}}_k^R\left(2\right)]---\left(13\right)$

P_k＝P_k|k-1-K_kH_kP_k|k-1       (14)

其中，表示惯性导航系统动态误差模型的状态向量x_k的一步预测；P_k|k-1表示一步预测方差；表示惯性导航系统动态误差模型的状态向量x_k的估计值；P_k表示估计方差；K_k表示滤波增益；表示步骤四中的估计值；表示的估计值，其主对角线元素等于步骤四中的估计值，非主对角线的元素为0。

经过上述步骤即可得到k时刻惯性导航系统动态误差模型状态向量x_k的估计值包含位置误差(δP)_k，速度误差(δVⁿ)_k和失准角利用这些误差估计结果对惯性导航系统的位置输出速度输出和姿态矩阵输出进行校正，通过公式15～17即可得到修正后的载车的位置速度姿态矩阵

${\tilde{P}}_k=P_k^{\mathrm{INS}}-{\left(\mathrm{\delta P}\right)}_k---\left(15\right)$

${\tilde{V}}_k=V_k^{\mathrm{INS}}-{\left({\mathrm{\delta V}}^n\right)}_k---\left(16\right)$

其中，和分别表示k时刻惯性导航系统输出的位置、速度和姿态矩阵，是已知量；I_3×3表示3阶单位阵；表示由构成的斜负对称阵。

步骤六、对步骤一中的惯性导航系统动态误差模型一步转移矩阵F_k-1进行更新，同时将k+1的值赋给k，然后返同到步骤二。

有益效果

与已有的GPS辅助惯性导航系统实现运动对准的方法比较，本发明方法放宽了传统卡尔曼滤波方法对噪声特性的苛刻要求，解决了车辆运动过程中由于GPS包含各类未知噪声而造成对准精度不高及对准时间延长的缺陷；野值检测方法充分利用了运动对准过程中车辆的行驶特点，采用卡尔曼滤波方法实现了实时检测；噪声特性同样采用卡尔曼滤波实现实时估计，与现有技术相比，不需要对当前时刻之前的若干个时刻残差进行存储，使其更加适用于惯性导航系统在环境恶劣下的初始对准。

附图说明

图1为本发明的具体实施例中的载车运行轨迹示意图；

图2为本发明的具体实施例中的俯仰角误差比较示意图；

图3为本发明的具体实施例中的横滚角误差比较示意图；

图4为本发明的具体实施例中的航向角误差比较示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

本实施例中，将惯性导航系统固联在载车上，将GPS接收器安装在车顶。载车静止30秒进行粗对准后开始运动，运动轨迹如图1所示，其横坐标为纬度，纵坐标为经度。3个陀螺仪的随机漂移均为0.01/h，常值漂移均为0.02/h；3个加速度计随机漂移均为50μg，常值漂移均为100μg；初始纬度为39.800343；初始经度为116.166874；初始高程为40.87m；野值检测阈值.J_max＝0.5；惯性导航系统动态误差模型的状态向量为零向量；惯性导航系统动态误差模型的观测方程噪声η_k的均值和方差的状态方程中的初始值为零向量。其过程如下：

步骤一、建立包括位置误差、速度误差、失准角和惯性器件误差漂移的惯性导航系统动态误差模型；动态误差模型为φ角误差方程或ψ角误差方程；在东北天坐标系下，动态误差模型表示如公式1所示。

$\stackrel{·}{x}\left(t\right)=F\left(t\right)x\left(t\right)+w\left(t\right)---\left(1\right)$

动态误差模型为φ角误差方程，F(t)表示为：

$F\left(t\right)=\left(\begin{array}{ccccc}{F}_{11}& F_{12}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ F_{21}& F_{22}& F_{23}& 0_{3\times 3}& F_{23}\\ F_{31}& F_{32}& F_{33}& F_{34}& 0_{3\times 3}\\ 0_{6\times 3}& 0_{6\times 3}& 0_{6\times 3}& 0_{6\times 3}& 0_{6\times 3}\end{array}\right)---\left(18\right)$

各个矩阵块的形式表示为：

$F_{11}=\left(\begin{array}{ccc}\frac{{\rho }_E{R}_{\mathrm{mm}}}{R_m+h}& 0& \frac{{\rho }_E}{R_m+h}\\ {\rho }_N\sec \left(L\right)(\tan \left(L\right)-\frac{R_{\mathrm{tt}}}{R_t+h})& 0& -\frac{{\rho }_N\sec \left(L\right)}{R_t+h}\\ 0& 0& 0\end{array}\right)---\left(19\right)$

式中，[ρ_E ρ_N ρ_U]表示地球坐标系相对地理坐标系的转动角速率；L和h分别表示载体所处纬度和高程；R_m和R_t分别表示卯酉圈和子午圈的半径；R_mm表示卯酉圈的半径关于纬度求导，可表示如下：

$R_{\mathrm{mm}}={\partial R}_m/\partial L=6R_{0}e\sin \left(L\right)\cos \left(L\right)---\left(20\right)$

其中，R₀为地球半径，e为椭圆率。

$F_{12}=\left(\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{\sec \left(L\right)}{R_t+h}& 0& 0\\ 0& 0& 1\end{array}\right)---\left(21\right)$

$F_{21}=\left(\begin{array}{ccc}(2{\omega }_N+{\rho }_N{\sec }^2\left(L\right)-\frac{{\rho }_U{R}_{\mathrm{tt}}}{R_t+h})v_N+({2\omega }_U+\frac{{\rho }_N{R}_{\mathrm{tt}}}{R_t+h})v_U& 0& \frac{v_U{\rho }_N}{R_t+h}-\frac{{\rho }_N\tan \left(L\right)}{R_t+h}{v}_U\\ (-2{\omega }_N-{\rho }_N{\sec }^2\left(L\right)+\frac{{\rho }_U{R}_{\mathrm{tt}}}{R_t+h})v_E-\frac{{\rho }_E{R}_{\mathrm{mm}}}{R_m+h}{v}_U& 0& \frac{{\rho }_N\tan \left(L\right)}{R_t+h}{v}_E-\frac{{\rho }_E{v}_U}{R_m+h}\\ \frac{{\rho }_E{R}_{\mathrm{mm}}}{R_m+h}{v}_N-({2\omega }_U+\frac{{\rho }_N{R}_{\mathrm{tt}}}{R_t+h})v_E& 0& \frac{v_N{\rho }_E}{R_m+h}-\frac{{\rho }_N{v}_E}{R_t+h}\end{array}\right)---\left(22\right)$

式中，[ω_E ω_n ω_U]表示地球自转角速率在东北天方向的分量；[v_E v_N v_U]表示载体在东北天方向的运动速度；R_tt表示子午圈的半径关于纬度求导，可表示如下：

$R_{\mathrm{tt}}={\partial R}_t/\partial L=2R_{0}e\sin \left(L\right)\cos \left(L\right)---\left(23\right)$

$F_{22}=\left(\begin{array}{ccc}\frac{\tan \left(L\right)v_N-v_U}{R_t+h}& 2{\omega }_U+{\rho }_U& -2{\omega }_N-{\rho }_N\\ -2{\omega }_U-2{\rho }_U& -\frac{v_U}{R_m+h}& {\rho }_E\\ 2{\omega }_N+2{\rho }_N& -2{\rho }_E& 0\end{array}\right)---\left(24\right)$

$F_{23}=\left(\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right)---\left(25\right)$

式中，[f_E f_N f_U]表示加速度计测量的东向、北向和天向的比力。

$F_{25}=C_b^n---\left(26\right)$

式中，表示姿态矩阵。

$F_{31}=\left(\begin{array}{ccc}-\frac{{\rho }_E{R}_{\mathrm{mm}}}{R_m+h}& 0& -\frac{{\rho }_E}{R_m+h}\\ -{\omega }_U-\frac{{\rho }_N{R}_{\mathrm{tt}}}{R_t+h}& 0& -\frac{{\rho }_N}{R_t+h}\\ {\omega }_N+{\rho }_N{\sec }^2\left(L\right)-\frac{{\rho }_N\tan \left(L\right)}{R_t+h}& 0& -\frac{{\rho }_N\tan \left(L\right)}{R_t+h}\end{array}\right)---\left(27\right)$

$F_{32}=\left(\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_t+h}& 0& 0\\ \frac{\tan \left(L\right)}{R_t+h}& 0& 0\end{array}\right)---\left(26\right)$

$F_{33}=\left(\begin{array}{ccc}0& {\omega }_U+{\rho }_U& -{\omega }_N-{\rho }_N\\ -{\omega }_U-{\rho }_U& 0& {\rho }_E\\ {\omega }_N+{\rho }_N& -{\rho }_E& 0\end{array}\right)---\left(28\right)$

$F_{34}=-C_b^n---\left(29\right)$

对该动态误差模型进行离散化处理，可得公式2：

x_k＝F_k-1x_k-1+w_k          (2)

步骤二、建立步骤一所述的惯性导航系统动态误差模型的观测方程，如公式3所示。

$Y_k=V_k^{\mathrm{INS}}-V_k^{\mathrm{GPS}}=H_k{x}_k+{\eta }_k---\left(3\right)$

式中，Y_k表示k时刻的观测量；表示惯性导航系统的速度输出；表示经杆臂补偿后GPS的速度输出；H_k为k时刻的观测矩阵，H_k＝[O_3×3 I_3×3  O_3×9]；O_3×3表示3阶O矩阵；I_3×3表示3阶单位阵，O_3×9表示3行9列的O矩阵；η_k表示k时刻的惯性导航系统动态误差模型的观测方程噪声。

步骤三、对GPS输出进行实时野值检测。

通过GPS测量得到的当前时刻载车东向速度和北向速度计算水平合速度Z_k，并以此作为观测量，建立加加速度跟踪模型，如公式4所示。

$\frac{d}{\mathrm{dt}}\left(\begin{array}{c}v\left(t\right)\\ a\left(t\right)\\ \stackrel{·}{a}\left(t\right)\end{array}\right)=\left(\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right)\left(\begin{array}{c}v\left(t\right)\\ a\left(t\right)\\ \stackrel{·}{a}\left(t\right)\end{array}\right)+\zeta \left(t\right)---\left(4\right)$

式中，t为时间值，为正实数；v(t)表示水平合速度，a(t)和分别表示水平加速度和水平加加速度；ζ(t)表示加加速度跟踪模型的系统噪声，是均值为零，方差为的白噪声序列，值根据实际应用环境人为设定，为正实数。

对公式4进行离散化，离散化后的系统方程表示为：

$\left(\begin{array}{c}{v}_k\\ a_k\\ {\stackrel{·}{a}}_k\end{array}\right)=G_{k-1}\left(\begin{array}{c}{v}_{k-1}\\ a_{k-1}\\ {\stackrel{·}{a}}_{k-1}\end{array}\right)+{\zeta }_k---\left(5\right)$

式中，v_k表示k时刻水平合速度，a_k和分别表示k时刻水平加速度和水平加加速度，G_k-1表示离散化的加加速度跟踪模型的一步转移矩阵，ζ_k表示离散后加加速度跟踪模型的系统噪声。

加加速度跟踪模型的观测方程表示为：

$Z_k=\left(\begin{array}{ccc}1& 0& 0\end{array}\right)\left(\begin{array}{c}{v}_k\\ a_k\\ {\stackrel{·}{a}}_k\end{array}\right)+{\xi }_k---\left(6\right)$

式中，Z_k表示k时刻水平合速度的观测量，其计算公式为：ξ_k表示加加速度跟踪模型的观测方程噪声，是均值为零，方差为的白噪声序列，值根据实际应用环境人为设定，为正实数。

根据公式5和公式6，采用卡尔曼滤波器即可估计出k时刻水平合速度v_k、相应的加速度a_k和加加速度和

根据载车的类型和对准动态情况，预先设定阈值J_max，如果成立，则判断当前的观测量为有效值，执行步骤四；否则，判断其为野值，在该时刻做卡尔曼平滑，同时将k+1的值赋给k，然后重复步骤三。

步骤四、对惯性导航系统动态误差模型的观测方程噪声η_k的均值和方差进行估计；

根据步骤二中的惯性导航系统动态误差模型的观测方程得到的k时刻的观测量Y_k以及观测矩阵H_k，采用主、从两个卡尔曼滤波器分别对k时刻惯性导航系统的状态x_k、k时刻的观测噪声η_k的均值和方差进行估计；具体为：

和的状态方程为：

$x_{k+1}^R=\left(\begin{array}{cc}{I}_1& 0\\ 0& I_2\end{array}\right)x_k^R+w_k^R---\left(7\right)$

式中，表示主对角线元素；该状态方程的系统噪声是均值为零，方差为Q_R的白噪声序列，Q_R值根据实际应用环境人为设定，Q_R为正实数；为m₁行1列的向量；I₁为m₁行m₁列的单位阵；为m₂行1列的向量；I₂为m₂行m₂列的单位阵。

公式7对应的观测方程为：

$z_k^R=\left(\begin{array}{c}{Y}_k-H_k{\hat{x}}_k\\ \mathrm{diag}(C_k-H_k{P}_{k|k-1}{H}_k^T)\end{array}\right)---\left(8\right)$

式中，C_k表示残差序列的方差，满足和P_k|k-1的初始值分别为和P₀，均为人为设定值；此后，和P_k|k-1由主卡尔曼滤波器提供；

公式8可进一步整理为：

$z_k^R=\left(\begin{array}{cc}I& 0\\ 0& I\end{array}\right)x_k^R+{\eta }_k^R---\left(9\right)$

式中，量测噪声是均值为零，方差为R_R的白噪声序列，R_R值根据实际应用环境人为设定，R_R为正实数。

根据公式7和公式9，采用从卡尔曼滤波器即可估计出k时刻的观测噪声η_k的均值和方差从卡尔曼滤波器为经典卡尔曼滤波器。

步骤五、对惯性导航系统的状态x_k进行估计；

根据步骤一建立的惯性导航系统动态误差模型及步骤二中的观测方程，结合步骤四给出的观测噪声η_k的均值和方差的估计值，采用主卡尔曼滤波器对惯性导航系统动态误差模型的状态向量x_k进行估计。具体为：

主卡尔曼滤波器为经典卡尔曼滤波器的改进，计算过程如公式10～14所示：

${\hat{x}}_{k|k-1}=F_{k-1}{\hat{x}}_{k-1}---\left(10\right)$

$P_{k|k-1}=F_{k-1}{P}_{k-1}{F}_{k-1}^T+Q_k---\left(11\right)$

$K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+{\hat{x}}_k^R\left(1\right)]}^{-1}---\left(12\right)$

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k[Y_k-F_{k-1}{\hat{x}}_{k|k-1}-{\hat{x}}_k^R\left(2\right)]---\left(13\right)$

P_k＝P_k|k-1-K_kH_kP_k|k-1         (14)

其中，表示惯性导航系统动态误差模型的状态向量x_k的一步预测；P_k|k-1表示一步预测方差；表示惯性导航系统动态误差模型的状态向量x_k的估计值；P_k表示估计方差；K_k表示滤波增益；表示步骤四中的估计值；表示的估计值，其主对角线元素等于步骤四中的估计值，非主对角线的元素为0。

经过上述步骤即可得到k时刻惯性导航系统动态误差模型状态向量x_k的估计值包含位置误差(δP)_k，速度误差(δVⁿ)_k和失准角利用这些误差估计结果对惯性导航系统的位置输出速度输出和姿态矩阵输出进行校正，通过公式15～17即可得到修正后的载车的位置速度姿态矩阵

${\tilde{P}}_k=P_k^{\mathrm{INS}}-{\left(\mathrm{\delta P}\right)}_k---\left(15\right)$

${\tilde{V}}_k=V_k^{\mathrm{INS}}-{\left({\mathrm{\delta V}}^n\right)}_k---\left(16\right)$

其中，和分别表示k时刻惯性导航系统输出的位置、速度和姿态矩阵，是已知量；I_3×3表示3阶单位阵；表示由构成的斜负对称阵。

步骤六、对步骤一中的惯性导航系统动态误差模型一步转移矩阵F_k-1进行更新，同时将k+1的值赋给k，然后返回到步骤二。

为了说明本发明的效果，采用传统卡尔曼滤波方法在相同的实验设置下进行试验，得到俯仰角误差比较图如图2所示，其横坐标为时间，纵坐标为俯仰角误差值；横滚角误差比较图如图3所示，其横坐标为时间，纵坐标为横滚角误差值；航向角误差比较图如图4所示，其横坐标为时间，纵坐标为航向角误差值。从这三幅图中可以看出，采用本发明后对准结果受环境扰动的影响较小，自收敛后(100秒-600秒)的均方差结果如表1所示。

表1两种方法估计结果的均方差比较

  所采用方法  俯仰角误差(度)  横滚角误差(度)  航向角误差(度)  传统卡尔曼滤波方法  0.0099  0.0146  0.1377  本发明方法  0.0028  0.0047  0.0530

表中均方差的计算结果表明本发明在抑制野值及噪声特性的估计中能够起到有效的作用。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，或者对其中部分技术特征进行等同替换，这些改进和替换也应视为本发明的保护范围。