一种基于杆臂估计的INS/GPS组合导航系统反馈校正方法

摘要

一种基于杆臂估计的INS/GPS组合导航系统反馈校正方法，该方法将杆臂误差扩展至系统误差状态向量中进行估计，通过比较杆臂估计值与真实值之差是否在误差阈值范围内，来判断惯性器件随机误差是否得到准确估计。当杆臂估计值误差小于设定阈值时惯性器件误差得到有效估计，利用估计出的惯性器件随机常值误差修正陀螺仪及加速度计测量输出，并在组合Kalman滤波时进行全反馈校正，反之，则不对惯性器件误差进行反馈校正。本发明可实现对惯性器件随机常值误差进行有效估计并进行全反馈校正，能够有效提高INS/GPS组合导航系统的精度。

说明书

技术领域

本发明涉及一种INS/GPS组合导航系统的误差估计及反馈校正方法。既可对导航参数误差进行精确估计及反馈校正，又可对惯性器件随机常值误差进行准确估计及反馈校正。

背景技术

INS/GPS组合导航系统是一种利用GPS长期定位精度高及INS短期定位定向精度高实现优势互补的组合导航系统。通常采用卡尔曼滤波去估计惯性导航系统的各误差状态，再用误差状态估值去校正系统，达到综合的目的。校正一般有两种方式：输出校正(开环卡尔曼滤波器)和反馈校正(闭环卡尔曼滤波器)。

输出校正与反馈校正的区别在于：在利用导航参数误差的估值校正系统输出的导航参数后，输出校正时导航参数误差估计值不归零，而反馈校正时导航参数误差估计值归零。从本质上讲，输出校正与反馈校正精度相同。输出校正时，估计的导航误差参数没有归零，随着计算时间的累计，待估计的导航参数误差变大，导致INS系统误差模型变为非线性。由于卡尔曼滤波主要适用于线性系统，因此，输出校正时卡尔曼滤波估计的精度随计算时间累计而降低，甚至出现崩溃。因此在INS/GPS组合导航时必须对导航参数误差进行反馈校正。当前的导航参数误差反馈校正方案通常包括两种：一是混合校正方案，在滤波初始阶段，由于各项误差未得到准确估计，采用输出校正的方案，而经过一段时间后，在误差状态得到准确估计后，对导航参数误差进行反馈校正；二是不完全反馈校正方案。该方案仅对惯性导航解算出的位置、速度、姿态误差进行反馈校正，不对惯性器件随机常值误差进行反馈校正；

除导航参数误差外，惯性器件误差也是INS/GPS组合导航系统的一个重要误差源，由于无法确定惯性器件随机常值误差是否得到准确估计，在通常的反馈校正方案中，一般不对惯性器件随机常值误差进行反馈校正。这是因为，在一个滤波周期内，由惯性器件随机常值误差引起的INS误差较小。且由于INS误差方程中位置、速度、姿态误差与惯性器件误差的耦合特性，当对导航参数误差进行反馈校正时间接地抑制了惯性器件随机常值误差产生的误差的累积。然而，如果能准确估计出惯性器件的随机常值误差，不仅可有效降低滤波区间内的INS误差，还可提高GPS信号采样点处kalman滤波的估计精度。尤其在GPS信号失锁时，INS/GPS组合导航系统无法对INS误差进行校正，惯性器件随机常值误差对系统输出影响显著增大，此时必须考虑惯性器件随机常值误差的反馈校正方案。

当前INS/GPS组合导航系统的两种校正方案均存在以下不足：1、由于无绝对参考基准，缺乏判断导航系统误差参数得到准确估计的条件。2、没有对惯性器件随机常值误差进行反馈校正，降低了INS/GPS组合导航系统的精度。

发明内容

本发明的技术解决的问题是：比较杆臂估值与真实值之误差是否达到设定阈值范围内来判断惯性器件随机常值误差是否得到了准确估计，并将此作为INS/GPS组合导航系统执行不完全反馈校正和全反馈校正方案的依据。

本发明的技术解决方案为：一种基于杆臂估计的INS/GPS组合导航系统反馈校正方法。其特征在于包括下列步骤：

（1）进行飞行试验前，利用激光全站仪测量出INS/GPS组合导航系统中IMU至GPS天线间的真实杆臂R_t；

（2）采集飞行试验过程中INS/GPS组合导航系统的惯性测量数据及GPS数据，其中惯性测量数据包括三轴陀螺仪数据和三轴加速度计数据，x,y,z轴的陀螺仪数据分别表示为x,y,z轴的加速度计数据分别表示为；GPS数据包括GPS时间及当地地理系下的位置和速度，其中位置包括纬度L_gps，经度λ_gps及高度H_gps，速度包括东向速度V_Egps，北向速度V_Ngps，天向速度V_Ugps，记第i个GPS采样时刻的GPS时间为T_gps(i)，i=1，2，...，n，n为一次飞行试验过程中飞机从起飞至降落时的飞行总时间，单位为秒；

（3）根据步骤（1）测量的真实杆臂R_t给定杆臂初值R_ini＝R_t-[0.5,0.5,0.5]^T，并根据C/A码GPS信号中的位置误差精度给定杆臂估值误差阈值为R_th，采用包括位置误差δP、速度误差δV、失准角误差ψ、陀螺随机常值漂移ε_g，加速度计随机常值零偏ε_a及杆臂误差δl在内的18维误差状态作为Kalman滤波器的误差状态向量，在GPS信号未到来时，利用步骤（2）采集到的惯性测量数据进行惯性导航解算；当GPS信号到来时，在GPS采样时刻进行Kalman滤波，对18维误差状态向量进行估计。每次滤波后，利用滤波估计出的位置误差δP、速度误差δV、失准角误差ψ对INS解算结果进行反馈校正，利用杆臂误差δl对杆臂初值R_ini进行校正得到杆臂估计值R_e，同时用反馈校正后的杆臂估计值R_e更新R_ini。

（4）将步骤（3）得到的杆臂估计值R_e与步骤（1）测量的真实杆臂R_t相比，如果其误差小于步骤（3）给定的杆臂估值误差阈值R_th时，将此时估计得到的陀螺漂移ε_g和加速度计零偏ε_a作为陀螺漂移估计值ε_ge和加速度计零偏估计值ε_ae，在下一个GPS采样时刻，执行步骤（5）；否则，在下一个GPS采样时刻，返回执行步骤（3）。

（5）去掉步骤（3）中18维状态向量中的3个杆臂误差，得到15维向量，将该15维状态向量作为Kalman滤波器误差状态向量，利用步骤（4）获取的惯性器件随机常值误差ε_ge和ε_ae校正陀螺和加速度计采样值，利用修正后的陀螺及加速度计数据进行INS解算，在GPS采样时刻，利用步骤（1）的真实杆臂R_t进行杆臂误差补偿，采用15维状态向量的kalman滤波器进行全反馈INS/GPS组合导航。

本发明的原理是：在飞行前，对IMU至GPS天线之间的杆臂值进行精确测量，该值在整个飞行过程中保持不变，将其作为杆臂估值误差的参考基准。当杆臂误差可观测时载体需要进行变化的角机动。变化的角机动使得惯性器件随机常值误差变为可观测状态。因此，利用杆臂估计值与真实值之差是否收敛到预设的误差阈值范围内就可判断出惯性器件随机常值误差是否得到准确估计。本发明分为两个阶段，第一阶段是INS/GPS组合导航不完全反馈校正阶段：此阶段采用包括杆臂误差在内的18维系统误差状态向量。在每个GPS采样点进行卡尔曼滤波，对估计得到的位置误差、速度误差、姿态误差和杆臂误差进行反馈校正，不对惯性器件随机常值误差进行反馈校正。当杆臂估计值与真值相比较，如果其误差到达设定阈值范围内，则将此时估计得到的性器件随机常值误差作为其准确估计值，在下一个GPS采样时刻进入第二阶段全反馈校正阶段。否则在下一个GPS采样时刻依然执行第一阶段的不完全反馈校正。第二阶段为INS/GPS组合导航全反馈校正阶段：此时，采用不包括杆臂误差在内的15维系统误差模型，在滤波模型中，利用测得的杆臂真值进行杆臂误差补偿。在每个GPS采样时刻，对位置误差、速度误差及姿态误差进行反馈校正，利用第一阶段得到的惯性器件随机常值误差准确估计值校正陀螺及加速度计输出，从而实现INS/GPS组合导航系统的全反馈校正。

本发明与现有技术相比的优点在于：本发明中，由于杆臂真实值不随时间及载体动态变化，可作为绝对基准。通过判断杆臂估计值与真实值之差是否在阈值内确定惯性器件随机常值误差是否得到有效估计，既给出了系统执行不完全反馈校正和全反馈校正的条件，有利于系统实现，又能准确估计出惯性器件随机常值误差进行反馈校正，提高了INS/GPS组合导航系统的精度。

附图说明

图1为本发明的基于杆臂估计的INS/GPS组合导航系统反馈校正方法流程图；

图2为本发明的步骤3中离散化Kalman滤波示意图。

具体实施方式

一种基于杆臂估计的INS/GPS组合导航系统反馈校正方法主要分为两个阶段：第一阶段是基于包含杆臂误差在内的18维系统误差状态模型的不完全反馈校正系统；第二阶段是不包含杆臂误差在内的15维系统误差状态模型全反馈校正阶段。

本发明的具体实施步骤如下：

（1）进行飞行试验前，利用激光全站仪测量出INS/GPS组合导航系统中IMU至GPS天线间的真实杆臂R_t；

（2）采集飞行试验过程中INS/GPS组合导航系统的惯性测量数据及GPS数据，其中惯性测量数据包括三轴陀螺仪数据和三轴加速度计数据x,y,z轴的陀螺仪数据分别表示为x,y,z轴的加速度计数据分别表示为；GPS数据包括GPS时间及当地地理系下的位置和速度，其中位置包括纬度L_gps，经度λ_gps及高度H_gps，速度包括东向速度V_Egps，北向速度V_Ngps，天向速度V_Ugps，记第i个GPS采样时刻的GPS时间为T_gps(i)，i=1，2，...，n，n为一次飞行试验过程中飞机从起飞至降落时的飞行总时间，单位为秒；

（3）根据步骤（1）测量的真实杆臂R_t给定杆臂初值R_ini＝R_t-[0.5,0.5,0.5]^T，并根据C/A码GPS信号中的位置误差精度给定杆臂估值误差阈值为R_th，采用包括位置误差δP、速度误差δV、失准角误差ψ、陀螺随机常值漂移ε_g，加速度计随机常值零偏ε_a及杆臂误差δl在内的18维误差状态作为Kalman滤波器的误差状态向量，在GPS信号未到来时，利用步骤（2）采集到的惯性测量数据进行惯性导航解算；当GPS信号到来时，在GPS采样时刻进行Kalman滤波，对18维误差状态向量进行估计。每次滤波后，利用滤波估计出的位置误差δP、速度误差δV、失准角误差ψ对INS解算结果进行反馈校正，利用杆臂误差δl对杆臂初值R_ini进行校正得到杆臂估计值R_e，同时用反馈校正后的杆臂估计值R_e更新R_ini。

①利用步骤（2）采集的惯性测量数据进行INS解算，捷联解算输出包括位置、速度、和姿态。位置包括纬度L_i、经度λ_i和高度H_i，速度包括东向速度V_Ei、北向速度V_Ni及天向速度V_Ui及姿态：航向角ψ_i，俯仰角θ_i，滚动角γ_i；

②当采集到GPS信号时，利用同一时刻的INS解算输出和GPS信息进行Kalman滤波解算。

a.建立Kalman滤波系统系统方程

$>\stackrel{·}{x}=\mathrm{Fx}+\mathrm{Gw}$>

其中，x为系统误差状态矢量，w为系统噪声矢量，F为系统状态转移矩阵，G为系统噪声转换矩阵：

系统误差状态向量x＝[x_f x_a x_l]^T

x_f＝[δL δλ δH δV_E δV_N δV_U φ_E φ_N φ_U]^T

x_a＝[ε_gx ε_gy ε_gz ε_ax ε_ay ε_az]^T

x_l＝[δl_x δl_y δl_z]^T

其中，δL、δλ、δH分别代表惯性导航位置误差中的纬度误差、经度误差和高度误差；δV_E、δV_N、δV_U分别代表惯性导航速度误差中东向速度误差、北向速度误差和天向速度误差；φ_E、φ_N、φ_U分别代表惯性导航失准角误差中的东向失准角、北向失准角、天向失准角；ε_gx、ε_gy、ε_gz分别代表x轴陀螺、y轴陀螺和z轴陀螺的随机常值漂移；ε_ax、ε_ay、ε_az分别代表x轴加速度计、y轴加速度计和z轴加速度计的随机常值零偏；δl_x、δl_y和δl_z分别代表x轴、y轴、z轴的杆臂误差。

其中，$>G=\left(\begin{array}{ccc}& 0_{3\times 6}& \\ 0_{3\times 3}& & C_b^n\\ C_b^n& & 0_{3\times 3}\\ & 0_{9\times 6}& \end{array}\right),$>是载体系到导航系的姿态变换矩阵。

$>C_b^n=\left(\begin{array}{ccc}\cos {\gamma }_i\cos {\psi }_i-\sin {\gamma }_i\sin {\theta }_i\sin {\psi }_i& -\cos {\theta }_i\sin {\psi }_i& \sin {\gamma }_i\cos {\psi }_i+\cos {\gamma }_i\sin {\theta }_i\sin {\psi }_i\\ \cos {\gamma }_i\sin {\psi }_i+\sin {\gamma }_i\sin {\theta }_i\cos {\psi }_i& \cos {\theta }_i\cos {\psi }_i& \sin {\gamma }_i\sin {\psi }_i-\cos {\gamma }_i\sin {\theta }_i\cos {\psi }_i\\ -\sin {\gamma }_i\cos {\theta }_i& \sin {\theta }_i& \cos {\gamma }_i\cos {\theta }_i\end{array}\right)$>

$>w={\left(\begin{array}{cccccc}{w_{ϵ}}_{\mathrm{gx}}& {w_{ϵ}}_{\mathrm{gy}}& {w_{ϵ}}_{\mathrm{gz}}& {w_{ϵ}}_{\mathrm{ax}}& {w_{ϵ}}_{\mathrm{ay}}& {w_{ϵ}}_{\mathrm{az}}\end{array}\right)}^T$>

其中，分别为x轴陀螺、y轴陀螺和z轴陀螺的噪声。和分别为x轴加速度计，y轴加速度计和z轴加速度计的噪声。

$>F=\left(\begin{array}{cccccc}{F}_{11}& F_{12}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ F_{21}& F_{22}& F_{23}& 0_{3\times 3}& C_b^n& 0_{3\times 3}\\ F_{31}& F_{32}& F_{33}& C_b^n& 0_{3\times 3}& 0_{3\times 3}\\ & 0_{9\times 9}& & & 0_{9\times 9}& \end{array}\right)$>

系统状态转移矩阵中各子矩阵表示如下：

$>F_{11}=\left(\begin{array}{ccc}0& 0& \frac{-V_{\mathrm{Ni}}}{{(R_M+H_i)}^2}\\ \frac{V_{\mathrm{Ei}}·\sec L_i\text{·tan}{L}_i}{R_N+H_i}& 0& \frac{-V_{\mathrm{Ei}}\sec L_i}{{(R_N+H_i)}^2}\\ 0& 0& 0\end{array}\right)$>$>F_{12}=\left(\begin{array}{ccc}0& \frac{1}{R_M+H_i}& 0\\ \frac{\sec L_i}{R_N+H_i}& 0& 0\\ 0& 0& 1\end{array}\right)$>

$>F_{21}=\left(\begin{array}{ccc}2{\omega }_{\mathrm{ie}}{V}_{\mathrm{Ni}}\cos L_i+\frac{V_{\mathrm{Ei}}{V}_{\mathrm{Ni}}{\sec }^2{L}_i}{R_N+H_i}+2{\omega }_{\mathrm{ie}}{V}_{\mathrm{Ui}}\sin L_i& 0& \frac{V_{\mathrm{Ei}}{V}_{\mathrm{Ui}}-V_{\mathrm{Ei}}{V}_{\mathrm{Ni}}\tan L_i}{{(R_N+H_i)}^2}\\ (2{\omega }_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}{\sec }^2{L}_i}{R_N+H_i})V_{\mathrm{Ei}}& 0& \frac{V_{\mathrm{Ni}}{V}_{\mathrm{Ui}}-V_{\mathrm{Ei}}^2\tan L_i}{{(R_N+H_i)}^2}\\ -2{\omega }_{\mathrm{ie}}{V}_{\mathrm{Ei}}\sin L_i& 0& \frac{V_{\mathrm{Ei}}^2+V_{\mathrm{Ni}}^2}{{(R_N+H_i)}^2}\end{array}\right)$>

$>F_{22}=\left(\begin{array}{ccc}\frac{V_{\mathrm{Ni}}\tan L_i-V_{\mathrm{Ui}}}{R_N+H_i}& 2{\omega }_{\mathrm{ie}}\sin L_i+\frac{V_{\mathrm{Ei}}\tan L_i}{R_N+H_i}& -(2{\omega }_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}}{R_N+H_i})\\ -(2{\omega }_{\mathrm{ie}}\sin L_i+\frac{V_{\mathrm{Ei}}\tan L_i}{R_N+H_i})& -\frac{V_{\mathrm{Ui}}}{R_M+H_i}& -\frac{V_{\mathrm{Ni}}}{R_M+H_i}\\ 2({\omega }_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}}{R_N+H_i})& 2\frac{V_{\mathrm{Ni}}}{R_M+H_i}& 0\end{array}\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)$>$>F_{31}=\left(\begin{array}{ccc}0& 0& \frac{V_{\mathrm{Ni}}}{{(R_M+H_i)}^2}\\ -{\omega }_{\mathrm{ie}}\sin L_i& 0& \frac{-V_{\mathrm{Ei}}}{{(R_N+H_i)}^2}\\ {\omega }_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}{\sec }^2{L}_i}{R_N+H_i}& 0& -\frac{V_{\mathrm{Ei}}\tan L_i}{{(R_N+H_i)}^2}\end{array}\right)$>

$>F_{32}=\left(\begin{array}{ccc}0& \frac{-1}{R_M+H_i}& 0\\ \frac{1}{R_N+H_i}& 0& 0\\ \frac{\tan L_i}{R_N+H_i}& 0& \frac{-V_{\mathrm{Ei}}\tan L_i}{{(R_N+H_i)}^2}\end{array}\right)$>

$>F_{33}=\left(\begin{array}{ccc}0& {\omega }_{\mathrm{ie}}\sin L_i+\frac{V_{\mathrm{Ei}}\tan L_i}{R_N+H_i}& -({\omega }_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}}{R_N+H_i})\\ -({\omega }_{\mathrm{ie}}\sin L_i+\frac{V_{\mathrm{Ei}}\tan L_i}{R_N+H_i})& 0& \frac{-V_{\mathrm{Ni}}}{R_M+H_i}\\ {\omega }_{\mathrm{ie}}\cos L_i+\frac{V_{\mathrm{Ei}}}{R_N+H_i}& \frac{V_{\mathrm{Ni}}}{R_M+H_i}& \frac{-V_{\mathrm{Ei}}\tan L_i}{{(R_N+H_i)}^2}\end{array}\right)$>

$>\left(\begin{array}{c}{f}_E\\ F_N\\ f_U\end{array}\right)=C_b^n{f}_{\mathrm{ib}}^b$>

上式中，R_M、R_N分别为地球的子午圈主曲率半径，卯酉圈主曲率半径，ω_ie为地球自转角速度。

b.建立Kalman滤波量测方程

IMU至GPS天线间的杆臂引起的位置误差和速度误差表示如下：

$>z_R=\left(\begin{array}{c}{z}_{\mathrm{PR}}\\ z_{\mathrm{VR}}\end{array}\right)$>

其中，$>z_{\mathrm{PR}1}=C_b^n{R}_{\mathrm{ini}},$>$>z_{\mathrm{PR}}=\left(\begin{array}{c}{z}_{\mathrm{PR}1}\left(2\right)\\ z_{\mathrm{PR}1}\left(1\right)\\ z_{\mathrm{PR}1}\left(3\right)\end{array}\right);$>$>z_{\mathrm{VR}}=C_b^n({\omega }_{\mathrm{nb}}^b\times R_{\mathrm{ini}})$>

上式中，为载体系至导航系的姿态转换矩阵。

Kalman滤波量测方程为：z＝Hx+z_R+v

其中，z为量测向量，H为量测矩阵，v为量测噪声。

量测向量为GPS采样时刻的INS与GPS位置、速度之差。

z＝[δL′ δλ′ δH′ δV_E′ δV_N′ δV_U′]^T

$>\left(\begin{array}{c}{\mathrm{\delta L}}^{\prime }=L_i-L_{\mathrm{gps}}\\ {\mathrm{\delta \lambda }}^{\prime }={\lambda }_i-{\lambda }_{\mathrm{gps}}\\ {\mathrm{\delta H}}^{\prime }=H_i-H_{\mathrm{gps}}\\ {\mathrm{\delta V}}_E^{\prime }=V_{\mathrm{Ei}}-V_{\mathrm{Egps}}\\ {\mathrm{\delta V}}_N^{\prime }=V_{\mathrm{Ni}}-V_{\mathrm{Ngps}}\\ {\mathrm{\delta V}}_U^{\prime }=V_{\mathrm{Ui}}-V_{\mathrm{Ugps}}\end{array}\right)$>

量测矩阵H＝[H_P H_V]^T

H_P＝[diag([R_M+H_i,(R_N+H_i)cosL_i,1]) 0_3×12 lc]

$>H_v=\left(\begin{array}{cccc}{0}_{3\times 3}& \mathrm{diag}\left(\right[\mathrm{1,1,1}\left]\right)& 0_{3\times 9}& C_b^n{\omega }_{\mathrm{nb}}^b\end{array}\right)$>

其中，$>\mathrm{lc}=\left(\begin{array}{ccc}{C}_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)& C_b^n\left(\mathrm{2,3}\right)\\ C_b^n\left(\mathrm{1,1}\right)& C_b^n\left(\mathrm{1,2}\right)& C_b^n\left(\mathrm{1,3}\right)\\ C_b^n\left(\mathrm{3,1}\right)& C_b^n\left(\mathrm{3,2}\right)& C_b^n\left(\mathrm{3,3}\right)\end{array}\right).$>

$>v={\left(\begin{array}{cccccc}{v}_{{\mathrm{\delta L}}^{\prime }}& v_{{\mathrm{\delta \lambda }}^{\prime }}& v_{{\mathrm{\delta H}}^{\prime }}& v_{{\mathrm{\delta V}}_E^{\prime }}& v_{{\mathrm{\delta V}}_N^{\prime }}& v_{{\mathrm{\delta V}}_U^{\prime }}\end{array}\right)}^T$>

w和v为零均值的高斯白噪声，即满足:

$>\left(\begin{array}{c}E\left[w\left(t\right)\right]=0,E\left[w\left(t\right)w^T(t+\mathrm{\Delta t})\right]=\mathrm{Q\delta }\left(\mathrm{\Delta t}\right)\\ E\left[v\left(t\right)\right]=0,E\left[v\left(t\right)v^T(t+\mathrm{\Delta t})\right]=\mathrm{R\delta }\left(\mathrm{\Delta t}\right)\\ E\left[w\left(t\right)v^T(t+\mathrm{\Delta t})\right]=0\end{array}\right)$>

式中：Q∈R^l×l过程噪声方差阵，为非负定阵；R∈R^m×m为量测噪声方差阵，为正定阵；Δt为采样时间间隔。

c.Kalman滤波系统方程和量测方程离散化

令t＝t_k-1，t+Δt＝t_k。t_k时刻的线性离散型系统方程可表示为:

$>x_k={\Phi }_{k/k-1}{x}_{k-1}+G_{k-1}^w{w}_{k-1}$>

z_k＝H_kx_k+v_k

式中：Φ_k/k-1为状态转移矩阵F的离散化形式。当Δt（即滤波周期）较短时，F(t)可近似看作常阵，即

F(t)≈F(t_k-1)t_k-1≤t＜t_k

此时，状态转移矩阵Φ_k/k-1有如下计算式：

$>{\Phi }_{k/k-1}=I+F_{k-1}\mathrm{\Delta t}+F_{k-1}^2\frac{\Delta t^2}{2!}+F_{k-1}^3\frac{\Delta t^3}{3!}+···$>

离散化的卡尔曼滤波基本算法编排:

状态一步预测方程

$>{\hat{X}}_{k/k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}$>

状态估值计算方程

$>{\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$>

滤波增量方程

$>K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$>

一步预测均方误差方程

估计均方误差方程

$>P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$>

卡尔曼滤波流程图如图2所示，左右侧回路分别为增益计算回路及滤波计算回路。在增益计算回路中，根据系统状态转移矩阵Φ_k/k-1、前一时刻的估计均方误差P_k-1、系统噪声方差阵Q_k-1得到一步预测均方误差P_k/k-1，由P_k/k-1及观测矩阵H_k和量测噪声方差阵R_k得到滤波增益阵K_k。根据K_k及P_k/k-1得到本次估计均方误差。在滤波计算回路中，由前一时刻状态估值及Φ_k/k-1得到状态单步预测由量测Z_k及滤波增益K_k得到状态估计

经过卡尔曼滤波，得到该时刻的位置误差估计值δL、δλ、δH；速度误差估计值δV_E、δV_N、δV_U；失准角误差估计值为φ_E、φ_N、φ_U；陀螺随机常值漂移估计值ε_gx、ε_gy、ε_gz；加速度计随机常值零偏估计值ε_ax、ε_ay、ε_az；杆臂误差估计值δl_x、δl_y、δl_z。

d.反馈校正

将Kalman滤波估计的位置误差、速度误差、姿态误差反馈至INS解算输出，对INS解算输出的位置、速度、姿态进行校正。利用杆臂误差δl对杆臂初值R_ini进行校正得到杆臂估计值R_e，同时用反馈校正后的杆臂估计值R_e更新R_ini；

位置误差校正：

[L λ H]^T＝[L_i λ_i H_i]^T-[δL δλ δH]^T

速度误差校正：

[V_E V_N V_U]^T＝[V_Ei V_Ui V_Ni]^T-[δV_E δV_N δV_U]^T

姿态误差校正：

$>C_b^n=\left(\begin{array}{ccc}1& -{\Phi }_U& {\Phi }_N\\ 0& 1& -{\Phi }_E\\ -{\Phi }_N& {\Phi }_E& 1\end{array}\right)C_b^n$>

由修正后的可解得航向角Ψ、俯仰角θ和滚动角γ。

$>\psi =\arctan [{-C}_b^n\left(\mathrm{1,2}\right)/C_b^n\left(\mathrm{2,2}\right)]$>

$>\theta =\mathrm{acr}\sin \left[C_b^n\left(\mathrm{3,2}\right)\right]$>

$>\gamma =\arctan [-C_b^n\left(\mathrm{3,1}\right)/C_b^n\left(\mathrm{3,3}\right)]$>

杆臂误差反馈校正：

R_e＝R_ini-x_l

校正后利用杆臂估计值R_e更新杆臂初值，以进行下一次迭代计算。

R_ini=R_e

反馈后，参与反馈的误差状态归零：

x_f＝0，x_l＝0。

（4）将步骤（3）得到的杆臂估计值R_e与步骤（1）测量的真实杆臂R_t相比，如果其误差小于步骤（3）给定的杆臂估值误差阈值R_th时，将此时估计得到的陀螺漂移ε_g和加速度计零偏ε_a作为陀螺漂移估计值ε_ge和加速度计零偏估计值ε_ae，在下一个GPS采样时刻，执行步骤（5）；否则，在下一个GPS采样时刻，返回执行步骤（3）。

当||R_e-R_t||＜R_th时，ε_ge＝ε_g，ε_ae＝ε_a；

其中，$>R_{\mathrm{th}}=\left(\begin{array}{cc}0.15& \mathrm{GPS}(C/A)\\ 0.05& \mathrm{DGPS}\end{array}\right)$>

（5）去掉步骤（3）中18维状态向量中的3个杆臂误差，得到15维向量，将该15维状态向量作为Kalman滤波器误差状态向量，利用步骤（4）获取的惯性器件随机常值误差ε_ge和ε_ae校正陀螺和加速度计采样值，利用修正后的陀螺及加速度计数据进行INS解算，在GPS采样时刻，利用步骤（1）的真实杆臂R_t进行杆臂误差补偿，采用15维状态向量的kalman滤波器进行全反馈INS/GPS组合导航。步骤如下：

①在每个惯性测量数据采样时刻，利用步骤（4）得到的ε_ge和ε_ae校正陀螺及加速度计输出，利用校正后的值进行INS解算。

②在每次GPS采样时刻进行Kalman滤波时，利用不含杆臂误差的15维状态向量进行Kalman滤波全反馈校正估计。

15维误差状态向量表示为：

x＝[x_f x_a]^T

x_f＝[δL δλ δH δV_E δV_N δV_U φ_E φ_N φ_U]^T

x_a＝[ε_gx ε_gy ε_gz ε_ax ε_ay ε_az]^T

全反馈Kalman滤波离散化方程表示如下：

状态一步预测方程：

$>{\hat{X}}_{k/k-1}=0$>

状态估值计算方程：

滤波增量方程：

$>K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$>

一步预测均方误差方程：

估计均方误差方程：

$>P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$>

全反馈Kalman滤波离散化方程各符号与不完全反馈表示意义相同。