CN102608596A - 一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法 - Google Patents

Abstract

一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法，它有六大布骤：步骤一：建立惯性器件、多普勒雷达的误差模型；步骤二：基于惯性/多普勒组合导航在机载固定翼飞机上的应用，确定状态变量；步骤三：建立惯性/多普勒雷达组合导航状态方程；步骤四：建立惯性/多普勒雷达组合导航量测方程；步骤五：将卡尔曼滤波连续系统离散化；步骤六：进行Kalman滤波数据融合解算，通过状态估计，得到关于导航参数的次优估计，采用闭环反馈方式对惯导和多普勒雷达同时修正。本发明充分利用多普勒信息与惯导信息，提出了用于机载惯性/多普勒雷达组合导航的数据融合方案，有效解决了由于误差模型不完善引起的导航误差问题，提高了系统的精度。

Description

一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法

(一)技术领域

本发明涉及一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法，它详细提出了适合本系统的实时导航方法，这种方法应用于机载组合导航中，可以有效提高系统精度，属于航空、航天导航技术领域。

(二)背景技术

目前，导航系统已经成为舰船、飞机、导弹、宇宙飞行器、汽车等航行体上必不可少的重要设备。导航所需要的最基本的导航参数就是载体的即时位置、速度和姿态。随着现代战争触及的领域宽度、深度和高度的急剧扩大，导航技术在现代战争中起着越来越重要的地位。

为满足高精度导航的要求，必须采用组合导航技术。众所周知，惯导是一种完全自主的导航方式，它具有不依赖外界信息，隐蔽性强，机动灵活等优点，但是存在误差随时间迅速积累的问题，导航精度随时间而发散，在精度要求较高的情况下，惯导系统不能单独长时间工作，必须不断以其他信息加以修正；GPS导航精度高、误差无积累，但其容易受干扰和屏蔽、依赖性太强，不适合战争实用；多普勒技术有效的结合了二者的优点，多普勒技术具有完全自主、隐蔽性的优势，且长时间测量精度高。对多普勒和惯性导航系统进行组合，利用多普勒测速装置输出的高精度的速度测量值对惯导系统进行实时修正，既能实现载体完全自主导航，又能保证长时间高精度导航，增强武器装备的实用性，这也是目前西方发达国家的主流导航方案。

采用惯性导航作为基本导航方式，并在机体上引入多普勒雷达信息辅助惯导，可以充分利用惯导与多普勒雷达的互补特性，发挥各自的优点，弥补各自的不足，具体体现在：

1、将惯导和多普勒雷达组合使用，通过外速度信息的引入，利用滤波技术进行定位误差等参数的最优估计，可用来补偿捷联惯导系统随时间快速发散的高度通道以及随积分计算不断积累的速度、位置误差等，并提高导航系统的定位精度和可靠性；

2、在多普勒雷达信息不理想或发生故障情况下，惯导可以暂时独立的提供导航信息，并用惯导数据完成自我修正。因而，组合导航系统的误差要比纯惯导系统导航误差都小；

3、在满足同样的精度要求的情况下，加入多普勒雷达导航信息可以降低对惯导系统的精度要求，大大降低系统成本，并且能够实现在高动态环境下实时的、高精度的导航定位和姿态计算。

现有的技术方案

在现有技术中，组合导航的方法在目前的工程实践中应用较多。由于存在误差随时间积累的问题，导航精度随时间而发散，在精度要求较高的情况下，惯导系统不能单独长时间工作，必须不断用其他信息加以修正。多普勒雷达长时间测量精度高，误差不随时间积累，能够为惯导系统提供相对准确的速度信息。因此，利用多普勒信息对导航参数进行修正，可以提高现有系统的导航精度。惯性/多普勒雷达组合导航系统的信息融合是利用惯导解算出的速度误差、位置误差、姿态误差角、陀螺零偏、加速度计零偏等作为状态量建立状态方程；利用惯导与多普勒雷达在解算出的在导航坐标系下的速度各分量的差值作为量测量建立量测方程。由于速度和位置等信息不是直接从测量元件得到，不能在量测方程中直接反映出姿态误差角、位置等关系，故必须将姿态误差角等误差量都列为状态量，它们之间的关系在状态方程中描述。该技术误差模型不完善，运算速度较快，因而易于实现。

现有技术方案存在的问题

现有的惯性/多普勒组合导航在机载应用中有着如下不足之处：

1.没考虑多普勒雷达自身存在的误差；

2.和动态环境、系统精度相关，需针对不同的应用背景专门设计；

本发明采用机载惯性/多普勒雷达组合导航系统对信息融合方法的研究，即以惯导系统作为主要传感元件，多普勒雷达信息辅助惯导，研究惯导系统及多普勒雷达误差传播机理，建立完善的误差模型，采用卡尔曼滤波方法进行信息融合，通过状态估计，得到关于导航参数的最优或次优估计。本发明可以适用于机载惯性/多普勒雷达组合导航系统。

(三)发明内容

本发明一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法，它是对惯导与多普勒雷达进行组合导航，Kalman滤波器可以根据参考信息，对惯导系统和多普勒雷达系统的误差进行相互修正。这种组合导航方法分以下步骤来完成：

设n为导航坐标系，b为载体坐标系。

本发明一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法，该方法具体布骤如下：

步骤一：建立惯性器件、多普勒雷达的误差模型；

a.加速度计误差方程

加速度计零偏的误差模型可用一阶马尔科夫过程模型方程表示：

$\left\{\begin{array}{c}{\stackrel{\·}{\▿}}_x^b=-{\mathrm{\β}}_{\▿x}{\▿}_x^b+W_{\▿x}\\ {\stackrel{\·}{\▿}}_y^b=-{\mathrm{\β}}_{\▿y}{\▿}_y^b+W_{\▿y}\\ {\stackrel{\·}{\▿}}_z^b=-{\mathrm{\β}}_{\▿z}{\▿}_z^b+W_{\▿z}\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{Ax}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{Ay}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{Az}}=0\end{array}\right.---\left(3\right)$

上述方程中，分别为加速度计x、y、z轴向在载体坐标系下的零偏；

分别为加速度计x、y、z轴向理想值；

分别为加速度计x、y、z轴向测量值；δK_Ax、δK_Ay、δK_Az分别为加速度计x、y、z轴向刻度因子误差；A_ij为加速度计j轴向偏向i轴向的失准角；

分别为加速度计x、y、z轴向噪声；分别为加速度计x、y、z轴向过程的相关时间；

b.陀螺误差模型

陀螺零偏的误差模型可用一阶马尔科夫过程模型方程表示：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_x^b=-{\mathrm{\β}}_{\mathrm{\ϵx}}{\mathrm{\ϵ}}_x^b+W_{\mathrm{\ϵx}}\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y^b=-{\mathrm{\β}}_{\mathrm{\ϵy}}{\mathrm{\ϵ}}_y^b+W_{\mathrm{\ϵy}}\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z^b=-{\mathrm{\β}}_{\mathrm{\ϵz}}{\mathrm{\ϵ}}_z^b+W_{\mathrm{\ϵz}}\end{array}\right.---\left(5\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{gx}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{gy}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{gz}}=0\end{array}\right.---\left(6\right)$

上述方程中，

分别为陀螺x、y、z轴向在载体坐标系下的零偏；

分别为x、y、z轴向陀螺理想值；

分别为x、y、z轴向陀螺测量值；δK_gx、δK_gy、δK_gz分别为陀螺x、y、z轴向刻度因子误差；M_ij为陀螺j轴向偏向i轴向的失准角；W_εx、W_εy、W_εz分别为陀螺x、y、z轴向噪声；1/β_εx、1/β_εy、1/β_εz分别为陀螺x、y、z轴向过程的相关时间；

c.多普勒雷达误差方程

多普勒雷达零偏的误差模型可用一阶马尔科夫过程模型方程表示：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{dx}}=-{\mathrm{\β}}_{\mathrm{dx}}{\mathrm{\δv}}_{\mathrm{dx}}+W_{\mathrm{dx}}\\ \mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{dy}}={-\mathrm{\β}}_{\mathrm{dy}}{\mathrm{\δv}}_{\mathrm{dy}}+W_{\mathrm{dy}}\\ \mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{dz}}=-{\mathrm{\β}}_{\mathrm{dz}}{\mathrm{\δv}}_{\mathrm{dz}}+W_{\mathrm{dz}}\end{array}\right.---\left(8\right)$

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{K_{\mathrm{di}}}=0\\ {\stackrel{\·}{D}}_{\mathrm{ij}}=0\end{array}\right.i=x,y,z;j=x,y,z---\left(9\right)$

上述方程中，δv_dx、δv_dy、δv_dz分别为多普勒雷达x、y、z轴向在载体坐标系下的零偏；

分别为x、y、z轴向多普勒雷达理想值；

分别为x、y、z轴向多普勒雷达测量值；δK_dx、δK_dy、δK_dz分别为多普勒雷达x、y、z轴向刻度因子误差；D_ij为多普勒雷达j轴向偏向i轴向的失准角；W_dx、W_dy、W_dz分别为多普勒雷达x、y、z轴向噪声；1/β_dx、1/β_dy、1/β_dz分别为多普勒雷达x、y、z轴向过程的相关时间；

步骤二：基于惯性/多普勒组合导航在机载固定翼飞机上的应用，确定状态变量；

建立坐标系如图1所示。

本发明适用于机载固定翼飞机，该飞行器的特点是动力沿纵轴方向，横轴和垂轴方向无动力。因此，纵轴方向速度是载体的主要速度，风力等外部环境会造成横轴和垂轴方向很小的速度，即 $V_y^b>>V_x^b,V_y^b>>V_z^b.$

对多普勒雷达而言，由刻度因子误差δK_d和失准角D_ij引起的误差分别为

和因此由飞行器的特点可知δK_dx、δK_dz、D_xy、D_yz、D_zx、D_xz引起的误差可忽略，只需考虑δK_dy、D_xy、D_zy引起的误差。

对惯导系统而言，失准角引起的误差远小于刻度因子引起的误差，因此不考虑失准角引起的误差。由飞行器的特点可知，航向角和俯仰角相对变化较小，飞机横滚角相对变化较大，即

较大，

较小，只需考虑陀螺由刻度因子δK_gy以及加速度计由刻度因子δK_Ay、δK_Ay引起的误差。

因此惯导系统与多普勒雷达器件需选取δK_dy、D_xy、D_zy、δv_dx、δv_dy、δv_dz、δKgy、

${\mathrm{\ϵ}}_y^b,{\mathrm{\ϵ}}_z^b,{\▿}_x^b,{\▿}_y^b,{\▿}_z^b,$ δK_Az、δK_Ax作为状态量。

步骤三：建立惯性/多普勒雷达组合导航状态方程；

a.Kalman滤波的基本过程如下：

首先，给定动态系统的一阶线性状态方程和量测方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(10\right)$

Z(t)＝H(t)X(t)+V(t)            (11)

进而将状态方程(10)和量测方程(11)离散化可得：

X_k＝Ф_k，k-1X_k-1+Г_k-1W_k-1     (12)

Z_k＝H_kX_k+V_k    (13)

状态预测估计方程为：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(14\right)$

方差预测方程为：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(15\right)$

状态预测估计方程为：

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

方差迭代方程：

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

$=(I-K_k{H}_k)P_{k/k-1}---\left(17\right)$

滤波增益方程为：

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

初始条件为：

${\hat{X}}_0=E\left(X_0\right)={\mathrm{\μ}}_0,\mathrm{var}{\tilde{X}}_0=\mathrm{var}{X}_0=P_0---\left(19\right)$

验前统计量为：

E[W_k]＝0，Cov[W_k，W_j]＝E[W_kW_j ^T]＝Q_kδ_kj    (20)

E[V_k]＝0，Cov[V_k，V_j]＝E[V_kV_j ^T]＝R_kδ_kj    (21)

Cov[W_k，V_j]＝E[W_kV_j ^T]＝0                   (22)

${\mathrm{\δ}}_{\mathrm{kj}}=\left\{\begin{array}{c}0,k\≠j\\ 1,k=j\end{array}\right.---\left(23\right)$

b.姿态误差方程

$\left\{\begin{array}{c}\stackrel{\·}{{\mathrm{\ψ}}_x^n}={({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_z{\mathrm{\ψ}}_y^n-{({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_y{\mathrm{\ψ}}_z^n-T_{12}{\mathrm{\δK}}_{\mathrm{Gy}}{w}_{\mathrm{ibx}}^b-{\mathrm{\ϵ}}_x^n\\ \stackrel{\·}{{\mathrm{\ψ}}_y^n}={({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_z{\mathrm{\ψ}}_x^n+{({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_x{\mathrm{\ψ}}_z^n-T_{22}{\mathrm{\δK}}_{\mathrm{Gy}}{w}_{\mathrm{iby}}^b-{\mathrm{\ϵ}}_y^n\\ \stackrel{\·}{{\mathrm{\ψ}}_z^n}={({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_y{\mathrm{\ψ}}_x^n-{({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_x{\mathrm{\ψ}}_y^n-T_{32}{\mathrm{\δK}}_{\mathrm{Gy}}{w}_{\mathrm{ibz}}^b-{\mathrm{\ϵ}}_z^n\end{array}\right.---\left(24\right)$

式中：

$\left\{\begin{array}{c}{\mathrm{\ϵ}}_x^n=T_{11}{\mathrm{\ϵ}}_x^b+T_{12}{\mathrm{\ϵ}}_y^b+T_{13}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_y^n=T_{21}{\mathrm{\ϵ}}_x^b+T_{22}{\mathrm{\ϵ}}_y^b+T_{23}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_z^n=T_{31}{\mathrm{\ϵ}}_x^b+T_{32}{\mathrm{\ϵ}}_y^b+T_{33}{\mathrm{\ϵ}}_z^b\end{array}\right.---\left(25\right)$

其中为i(x、y、z)轴向平台坐标系相对计算地理坐标系之间的姿态误差角；

为地球自转角速率；为位移角速度；T为姿态矩阵；

c.位置误差方程

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{\mathrm{\θ}}}_x^n=-\frac{V_z^n}{R+h}\mathrm{\δ}{\mathrm{\θ}}_x^n+\frac{V_y^n}{{(R+h)}^2}\mathrm{\δh}-\frac{1}{R+h}\mathrm{\δ}{V}_y^1\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\θ}}}_y^n=-\frac{V_z^n}{R+h}{\mathrm{\δ\θ}}_y^n-\frac{V_x^n}{{(R+h)}^2}\mathrm{\δh}+\frac{1}{R+h}{\mathrm{\δV}}_x^1\\ \mathrm{\δ}\stackrel{\·}{h}=-V_y^n{\mathrm{\δ\θ}}_x^n+V_x^n{\mathrm{\δ\θ}}_y^n+{\mathrm{\δV}}_z^1\end{array}\right.---\left(26\right)$

其中

为i(x、y、z)轴向位置误差；h为高度；为i(x、y、z)轴向速度；为i(x、y、z)轴向速度误差；R为地球半径；

d.速度误差方程

$\left\{\begin{array}{c}{\mathrm{\δ}\stackrel{\·}{V}}_x^1=-\mathrm{g\δ}{\mathrm{\θ}}_y-f_z^n{\mathrm{\ψ}}_y^n+f_y^n{\mathrm{\ψ}}_z^n+{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_z{\mathrm{\δV}}_y^1\\ -{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_y{\mathrm{\δV}}_z^1+T_{11}{\mathrm{\δK}}_{\mathrm{Ax}}{f}_x^b+T_{13}\mathrm{\δ}{K}_{\mathrm{Az}}{f}_z^b+{\▿}_x^n\\ \mathrm{\δ}{\stackrel{\·}{V}}_y^1=\mathrm{g\δ}{\mathrm{\θ}}_x+f_z^n{\mathrm{\ψ}}_x^n-f_x^n{\mathrm{\ψ}}_z^n-{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_z{\mathrm{\δV}}_x^1\\ +{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_x{\mathrm{\δV}}_z^1+T_{21}\mathrm{\δ}{K}_{\mathrm{Ax}}{f}_y^b+T_{23}\mathrm{\δ}{K}_{\mathrm{Az}}{f}_z^b+{\▿}_y^n\\ \mathrm{\δ}{\stackrel{\·}{V}}_z^1=\frac{{2\mathrm{gR}}^2}{{(R+h)}^3}\mathrm{\δh}-f_y^n{\mathrm{\ψ}}_x^n+f_x^n{\mathrm{\ψ}}_y^n+{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_y{\mathrm{\δV}}_x^1\\ -{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_x{\mathrm{\δV}}_y^1+T_{31}{\mathrm{\δK}}_{\mathrm{Ax}}{f}_x^b+T_{33}\mathrm{\δ}{K}_{\mathrm{Az}}{f}_z^b+{\▿}_z^n\end{array}\right.---\left(27\right)$

式中

$\left\{\begin{array}{c}{\▿}_x^n=T_{11}{\▿}_x^b+T_{12}{\▿}_y^b+T_{13}{\▿}_z^b\\ {\▿}_y^n=T_{21}{\▿}_x^b+T_{22}{\▿}_y^b+T_{23}{\▿}_z^b\\ {\▿}_z^n=T_{31}{\▿}_x^b+T_{32}{\▿}_y^b+T_{33}{\▿}_z^b\end{array}\right.---\left(28\right)$

$C_b^n=T=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(29\right)$

$\mathrm{\ρ}=w_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{V_y^n}{R+h}\\ \frac{V_x^n}{R+h}\\ {\mathrm{\ρ}}_z\\ \end{array}\right]---\left(30\right)$

e.状态方程

由步骤二确立的器件误差和步骤四公式(24)、(26)、(27)确立的导航误差作为状态量组成状态矢量，如下：

$X=[{\mathrm{\δ\θ}}_x^n,{\mathrm{\δ\θ}}_y^n,\mathrm{\δh},{\mathrm{\δV}}_x^1,{\mathrm{\δV}}_y^1,{\mathrm{\δV}}_z^1,{\mathrm{\ψ}}_x^n,{\mathrm{\ψ}}_y^n,{\mathrm{\ψ}}_z^n,{\mathrm{\ϵ}}_x^b,{\mathrm{\ϵ}}_y^b,{\mathrm{\ϵ}}_z^b,{\▿}_x^b,{\▿}_y^b,{\▿}_z^b,$ (31)

${\mathrm{\δK}}_{\mathrm{Ax}},{\mathrm{\δK}}_{\mathrm{Az}},{\mathrm{\δK}}_{\mathrm{Gy}},D_{\mathrm{xy}},D_{\mathrm{zy}},{\mathrm{\δK}}_{\mathrm{dy}},{\mathrm{\δv}}_{\mathrm{dx}},{\mathrm{\δv}}_{\mathrm{dy}},{\mathrm{\δv}}_{\mathrm{dz}}{]}^T$

$F_1=\left[\begin{array}{ccc}-\frac{V_z^n}{R+h}& 0& \frac{V_y^n}{{(R+h)}^2}\\ 0& -\frac{V_z^n}{R+h}& -\frac{V_x^n}{{(R+h)}^2}\\ -V_y^n& V_x^n& 0\end{array}\right]---\left(32\right)$

$F_2=\left[\begin{array}{ccc}& & \\ 0& -g& 0\\ g& 0& 0\\ 0& 0& \frac{{2\mathrm{gR}}^2}{{(R+h)}^3}\end{array}\right]---\left(33\right)$

$F_3=\left[\begin{array}{ccc}0& -\frac{1}{R+h}& 0\\ \frac{1}{R+h}& 0& 0\\ 0& 0& 1\\ & & \end{array}\right]---\left(34\right)$

$F_4=\left[\begin{array}{ccc}0& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_y\\ -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& 0& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_x\\ {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_y& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_x& 0\end{array}\right]---\left(35\right)$

$F_5=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& -f_z^n& f_y^n& 0& 0& 0\\ f_z^n& 0& {-f}_x^n& 0& 0& 0\\ -f_y^n& f_x^n& 0& 0& 0& 0\end{array}\right]---\left(36\right)$

$F_6=\left[\begin{array}{cccccc}0& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_y& T_{11}& T_{12}& T_{13}\\ -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& 0& {({\mathrm{\ω}}_{\mathrm{en}}^n+{2\mathrm{\ω}}_{\mathrm{ie}}^n)}_x& T_{21}& T_{22}& T_{23}\\ {({\mathrm{\ω}}_{\mathrm{en}}^n+{2\mathrm{\ω}}_{\mathrm{ie}}^n)}_y& -{({\mathrm{\ω}}_{\mathrm{en}}^n+{2\mathrm{\ω}}_{\mathrm{ie}}^n)}_x& 0& T_{31}& T_{32}& T_{33}\\ 0& 0& 0& {-\mathrm{\β}}_{\mathrm{\ϵx}}& 0& 0\\ 0& 0& 0& 0& -{\mathrm{\β}}_{\mathrm{\ϵy}}& 0\\ 0& 0& 0& 0& 0& -{\mathrm{\β}}_{\mathrm{\ϵz}}\end{array}\right]---\left(37\right)$

$F_7=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ T_{11}& T_{12}& T_{13}& T_{11}{f}_x^b& T_{13}{f}_z^b& 0\\ T_{21}& T_{22}& T_{23}& T_{21}{f}_x^b& T_{23}{f}_z^b& 0\\ T_{31}& T_{32}& T_{33}& T_{31}{f}_x^b& T_{33}{f}_z^b& 0\end{array}\right]---\left(38\right)$

$F_8=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& {-T}_{12}{w}_{\mathrm{ibx}}^b\\ 0& 0& 0& 0& 0& -T_{22}{w}_{\mathrm{ibx}}^b\\ 0& 0& 0& 0& 0& -T_{32}{w}_{\mathrm{ibx}}^b\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]---\left(39\right)$

$F_9=\left[\begin{array}{cc}{F}_1& F_3\\ F_2& F_4\end{array}\right]---\left(40\right)$

$F_{11}=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& {-\mathrm{\β}}_{\mathrm{dx}}& 0& 0\\ 0& 0& 0& 0& {-\mathrm{\β}}_{\mathrm{dy}}& 0\\ 0& 0& 0& 0& 0& {-\mathrm{\β}}_{\mathrm{dz}}\end{array}\right]---\left(42\right)$

最后得到状态转移矩阵：

$F=\left[\begin{array}{cccc}{F}_9& F_5& F_7& 0_{6\×6}\\ 0_{6\×6}& F_6& F_8& 0_{6\×6}\\ 0_{6\×6}& 0_{6\×6}& F_{10}& 0_{6\×6}\\ 0_{6\×6}& 0_{6\×6}& 0_{6\×6}& F_{11}\end{array}\right]---\left(43\right)$

连续Kalman滤波状态方程为

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)---\left(44\right)$

步骤四：建立惯性/多普勒雷达组合导航量测方程；

${\mathrm{\δV}}_{\mathrm{DOP}}^n={\tilde{V}}_{\mathrm{INU}}^n-{\tilde{V}}_{\mathrm{DOP}}^n$

$=V_{\mathrm{INU}}^n+{\mathrm{\δV}}^1-(\mathrm{\δ\θ}\×)V_{\mathrm{INU}}^n-{\tilde{C}}_b^n(V_{\mathrm{DOP}}^b+{\mathrm{\δV}}_{\mathrm{DOP}}^b)$

$=V_{\mathrm{INU}}^n+{\mathrm{\δV}}^1-(\mathrm{\δ\θ}\×)V_{\mathrm{INU}}^n-[I-(\mathrm{\φ}\×)]C_b^n(V_{\mathrm{DOP}}^b+{\mathrm{\δV}}_{\mathrm{DOP}}^b)$

$\≈V_{\mathrm{INU}}^n-C_b^n{V}_{\mathrm{DOP}}^b+{\mathrm{\δV}}^1-(\mathrm{\δ\θ}\×)V_{\mathrm{INU}}^n-C_b^n{\mathrm{\δV}}_{\mathrm{DOP}}^b+(\mathrm{\φ}\×)C_b^n{V}_{\mathrm{DOP}}^b---\left(45\right)$

$\≈{\mathrm{\δV}}^1+[(\mathrm{\φ}-\mathrm{\δ\θ})\×]C_b^n{V}_{\mathrm{INU}}^b-C_b^n{\mathrm{\δV}}_{\mathrm{DOP}}^b$

$={\mathrm{\δV}}^1-(C_b^n{V}_{\mathrm{DOP}}^b\×)\mathrm{\ψ}-C_b^n{\mathrm{\δV}}_{\mathrm{DOP}}^b$

$=Z$

$\left\{\begin{array}{c}{Z}_x^n=(T_{31}{V}_{\mathrm{DOPx}}^b+T_{32}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_y^n-{(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V}_{\mathrm{DOPz}}^b)}_{}{\mathrm{\ψ}}_z^n\\ +{\mathrm{\δV}}_x^1-T_{11}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{xy}}-T_{13}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{zy}}-T_{12}{V}_{\mathrm{DOPy}}^b{\mathrm{\δK}}_y\\ -T_{11}{\mathrm{\δv}}_{\mathrm{dx}}-T_{12}{\mathrm{\δv}}_{\mathrm{dy}}-T_{13}{\mathrm{\δv}}_{\mathrm{dz}}\\ Z_y^n=-(T_{31}{V}_{\mathrm{DOPx}}^b+T_{31}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_x^n+(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_z^n\\ +{\mathrm{\δV}}_y^1-T_{21}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{xy}}-T_{23}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{zy}}-T_{22}{V}_{\mathrm{DOPy}}^b{\mathrm{\δK}}_y\\ -T_{21}{\mathrm{\δv}}_{\mathrm{dx}}-T_{22}{\mathrm{\δv}}_{\mathrm{dy}}-T_{23}{\mathrm{\δv}}_{\mathrm{dz}}\\ Z_z^n=(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{C}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_x^n-(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_y^n\\ +{\mathrm{\δV}}_z^1-T_{31}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{xy}}-T_{33}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{zy}}-T_{32}{V}_{\mathrm{DOPy}}^b{\mathrm{\δK}}_y\\ -T_{31}{\mathrm{\δv}}_{\mathrm{dx}}-T_{32}{\mathrm{\δv}}_{\mathrm{dy}}-T_{33}{\mathrm{\δv}}_{\mathrm{dz}}\end{array}\right.---\left(46\right)$

$H_1=\left[\begin{array}{ccc}0& (T_{31}{V}_{\mathrm{DOPx}}^b+T_{32}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b)& -(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V_{\mathrm{DOPz}}^b}_{})\\ -(T_{31}{V}_{\mathrm{DOPx}}^b+T_{32}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b)& 0& (T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b)\\ (T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V}_{\mathrm{DOPz}}^b)& -(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b)& 0\end{array}\right]---\left(47\right)$

$H_2=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(48\right)$

$H_3=\left[\begin{array}{ccc}-T_{11}{V}_{\mathrm{DOPy}}^b& -T_{13}{V}_{\mathrm{DOPy}}^b& -T_{12}{V}_{\mathrm{DOPy}}^b\\ {-T}_{21}{V}_{\mathrm{DOPy}}^b& -T_{23}{V}_{\mathrm{DOPy}}^b& -T_{22}{V}_{\mathrm{DOPy}}^b\\ -T_{31}{V}_{\mathrm{DOPy}}^b& -T_{33}{V}_{\mathrm{DOPy}}^b& -T_{32}{V}_{\mathrm{DOPy}}^b\end{array}\right]---\left(49\right)$

$H_4=\left[\begin{array}{ccc}-T_{11}& -T_{12}& -T_{13}\\ -T_{21}& -T_{22}& -T_{23}\\ -T_{31}& -T_{32}& -T_{33}\end{array}\right]---\left(50\right)$

H＝[0_3×3 H₂ H₂ 0_3×9 H₃ H₄]                  (51)

最终确立量测方程

Z＝HX+V                                        (52)

步骤五：将卡尔曼滤波连续系统离散化；

将连续系统离散化

${\mathrm{\Φ}}_{k+1,k}=I+\mathrm{TF}+\frac{{\left(\mathrm{TF}\right)}^2}{2}+\frac{{\left(\mathrm{TF}\right)}^3}{6}---\left(53\right)$

$Q_k=\underset{i=1}{\overset{\∞}{\mathrm{\Σ}}}{M}_i\frac{\mathrm{\Δ}{T}^i}{i}---\left(54\right)$

其中 $M_1=Q\left(t\right),M_{i+1}={\mathrm{FM}}_i+M_i^T{F}^T$

步骤六：根据惯导与多普勒雷提供的导航信息，进行Kalman滤波信息融合解算，通过状态估计，得到关于导航参数的最优或次优估计，采用闭环反馈方式对惯导和多普勒雷达同时修正。

a.用卡尔曼滤波估计的惯导系统和多普勒雷达误差对测量数据补偿

1.对加速度计数据补偿

2.对陀螺数据补偿

对多普勒雷达数据补偿

b.用卡尔曼滤波估计的速度误差对速度及时修正

Vⁿ＝[I+(δθ×)](Vⁿ-δV¹)                    (58)

c.用卡尔曼滤波估计的姿态误差对姿态矩阵及时修正补偿

T＝{I+[(ψ+δθ)×]}T                        (59)

d.用卡尔曼滤波估计的位置误差对位置矩阵及时修正补偿

$C_e^n=[I+(\mathrm{\δ\θ}\×)]C_e^n---\left(60\right)$

优点及功效：本发明的优点在于：建立完善的系统误差模型，充分利用多普勒信息与惯导信息，提出了用于机载惯性/多普勒雷达组合导航的信息融合方案，有效解决了由于误差模型不完善引起的导航误差问题，提高了系统的精度。

(四)附图说明

图1为坐标系示意图。

图2为惯性/多普勒雷达组合导航算法框图。

图3为Kalman滤波器基本原理示意图。

图4为本发明流程框图

图中符号说明如下：

x、y、z分别为飞机的轴向；

为加速度计零偏；ε陀螺零偏；D为多普勒雷达的失准角；K为器件刻度因子；δθ为位置误差；h为高度；Vⁿ为速度；ψⁿ为姿态误差角；

为t_k-1时刻的状态估计值；为

是状态

的kalman滤波估计；Ф_k，k-1为t_k-1时刻至t_k时刻的一步转移阵；Г_k-1为系统噪声驱动阵；H_k为量测阵；R_k为量测噪声方差；Q_k为系统噪声方差阵；K_k为滤波增益；Z_k量测值；P_k-1为估计均方差阵；P_k，k-1一步预测误差方差阵；

(五)具体实施方式

下面结合附图，对本发明做进一步的详细说明。

见图1、图2、图3及图4，本发明一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法，该方法具体布骤如下：

步骤一：建立惯性器件、多普勒雷达的误差模型；

惯性/多普勒雷达组合系统的由加速度计、陀螺、多普勒雷达等器件组成。每个器件都存在器件误差、安装误差，这些误差都会引起组合导航误差。因此，建立其误差模型为步骤二中kalman状态量的选取和步骤三状态方程的确立奠定了基础。

a.加速度计误差方程

加速度计零偏的误差模型可用一阶马尔科夫过程模型方程表示：

$\left\{\begin{array}{c}{\stackrel{\·}{\▿}}_x^b=-{\mathrm{\β}}_{\▿x}{\▿}_x^b+W_{\▿x}\\ {\stackrel{\·}{\▿}}_y^b=-{\mathrm{\β}}_{\▿y}{\▿}_y^b+W_{\▿y}\\ {\stackrel{\·}{\▿}}_z^b=-{\mathrm{\β}}_{\▿z}{\▿}_z^b+W_{\▿z}\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{Ax}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{Ay}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{Az}}=0\end{array}\right.---\left(3\right)$

上述方程中，

分别为加速度计x、y、z轴向在载体坐标系下的零偏；

分别为加速度计x、y、z轴向理想值；

分别为加速度计x、y、z轴向测量值；δK_Ax、δK_Ay、δK_Az分别为加速度计x、y、z轴向刻度因子误差；Aij为加速度计j轴向偏向i轴向的失准角；

分别为加速度计x、y、z轴向噪声；

分别为加速度计x、y、z轴向相关时间常数；

a.陀螺误差模型

陀螺零偏的误差模型可用一阶马尔科夫过程模型方程表示：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_x^b=-{\mathrm{\β}}_{\mathrm{\ϵx}}{\mathrm{\ϵ}}_x^b+W_{\mathrm{\ϵx}}\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y^b=-{\mathrm{\β}}_{\mathrm{\ϵy}}{\mathrm{\ϵ}}_y^b+W_{\mathrm{\ϵy}}\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z^b=-{\mathrm{\β}}_{\mathrm{\ϵz}}{\mathrm{\ϵ}}_z^b+W_{\mathrm{\ϵz}}\end{array}\right.---\left(5\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{gx}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{gy}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{gz}}=0\end{array}\right.---\left(6\right)$

上述方程中，

分别为陀螺x、y、z轴向在载体坐标系下的零偏；分别为陀螺理想值；

分别为陀螺测量值；δK_gx、δK_gy、δK_gz分别为陀螺x、y、z轴向刻度因子误差；M_ij为陀螺j轴向偏向i轴向的失准角；W_εx、W_εy、W_εz分别为陀螺x、y、z轴向噪声；β_εx、β_εy、β_εz分别为陀螺x、y、z轴向相关时间常数；

b.多普勒雷达误差方程

多普勒雷达零偏的误差模型可用一阶马尔科夫过程模型方程表示：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{dx}}=-{\mathrm{\β}}_{\mathrm{dx}}{\mathrm{\δv}}_{\mathrm{dx}}+W_{\mathrm{dx}}\\ \mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{dy}}={-\mathrm{\β}}_{\mathrm{dy}}{\mathrm{\δv}}_{\mathrm{dy}}+W_{\mathrm{dy}}\\ \mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{dz}}=-{\mathrm{\β}}_{\mathrm{dz}}{\mathrm{\δv}}_{\mathrm{dz}}+W_{\mathrm{dz}}\end{array}\right.---\left(8\right)$

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{K_{\mathrm{di}}}=0\\ {\stackrel{\·}{D}}_{\mathrm{ij}}=0\end{array}\right.i=x,y,z;j=x,y,z---\left(9\right)$

上述方程中，δv_dx、δv_dy、δv_dz分别为多普勒雷达x、y、z轴向在载体坐标系下的零偏；

分别为多普勒雷达理想值；

分别为多普勒雷达测量值；δK_dx、δK_dy、δK_dz分别为多普勒雷达x、y、z轴向刻度因子误差；D_ij为多普勒雷达j轴向偏向i轴向的失准角；W_dx、W_dy、W_dz分别为多普勒雷达x、y、z轴向噪声；β_dx、β_dy、β_dz分别为多普勒雷达x、y、z轴向相关时间常数；

步骤二：基于惯性/多普勒组合导航在机载固定翼飞机上的应用，确定状态变量；

建立如图1所示坐标系。

本发明适用于机载固定翼飞机，该飞行器的特点是动力沿纵轴方向，横轴和垂轴方向无动力。因此，纵轴方向速度是载体的主要速度，风力等外部环境会造成横轴和垂轴方向很小的速度，即 $V_y^b>>V_x^b,V_y^b>>V_z^b.$

由于组合导航误差参数较多，由步骤一可知，每个器件有12个误差项，如何在器件误差中选取合适的状态量，既能保证较高的导航精度又减少计算量，是一个至关重要的问题。

对多普勒雷达而言，由刻度因子误差δK_d和失准角D_ij引起的误差分别为

和

因此由飞行器的特点可知δK_dx、δK_dz、D_xy、D_yz、D_zx、D_xz引起的误差可忽略，只需考虑δK_dy、D_xy、D_zy引起的误差。

对惯导系统而言，失准角引起的误差远小于刻度因子引起的误差，因此不考虑失准角引起的误差。由飞行器的特点可知，航向角和俯仰角相对变化较小，飞机横滚角相对变化较大，即

较大，

较小，只需考虑陀螺由刻度因子δK_gy以及加速度计由刻度因子δK_Ay、δK_Ay引起的误差。

因此惯导系统与多普勒雷达器件需选取δK_dy、D_xy、D_zy、δv_dx、δv_dy、δv_dz、δKgy、

${\mathrm{\ϵ}}_y^b,{\mathrm{\ϵ}}_z^b,{\▿}_x^b,{\▿}_y^b,{\▿}_z^b,$ δK_Az、δK_Ax作为状态量。

步骤三：建立惯性/多普勒雷达组合导航状态方程

由导航过程可以解算出姿态、位置、速度，推导其误差方程才能掌握组合导航误差传播机理。速度、姿态、位置误差方程在状态方程中至关重要。误差方程的建立的完善与否将直接影响kalman滤波方程，因此根据步骤二中选取的状态量建立一个完善的速度、姿态、位置误差方程。

a Kalman滤波的基本过程如下：

首先，给定动态系统的一阶线性状态方程和量测方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(10\right)$

Z(t)＝H(t)X(t)+V(t)                    (11)

进而将状态方程(10)和量测方程(11)离散化可得：

X_k＝Ф_k，k-1X_k-1+Г_k-1W_k-1             (12)

Z_k＝H_kX_k+V_k                            (13)

状态预测估计方程为：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(14\right)$

方差预测方程为：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(15\right)$

状态预测估计方程为：

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

方差迭代方程：

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

$=(I-K_k{H}_k)P_{k/k-1}---\left(17\right)$

滤波增益方程为：

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

初始条件为：

${\hat{X}}_0=E\left(X_0\right)={\mathrm{\μ}}_0,\mathrm{var}{\tilde{X}}_0=\mathrm{var}{X}_0=P_0---\left(19\right)$

验前统计量为：

E[W_k]＝0，Cov[W_k，W_j]＝E[W_kW_j ^T]＝Q_kδ_kj    (20)

E[V_k]＝0，Cov[V_k，V_j]＝E[V_kV_j ^T]＝R_kδ_kj    (21)

Cov[W_k，V_j]＝E[W_kV_j ^T]＝0                   (22)

${\mathrm{\δ}}_{\mathrm{kj}}=\left\{\begin{array}{c}0,k\≠j\\ 1,k=j\end{array}\right.---\left(23\right)$

b姿态误差方程

由公式 ${\stackrel{\·}{\mathrm{\ψ}}}^n={\mathrm{\ψ}}^n\×({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)-{\mathrm{T\δK}}_g{w}_{\mathrm{ib}}^b-{\mathrm{\ϵ}}^n,$ 展开可得：

$\left\{\begin{array}{c}\stackrel{\·}{{\mathrm{\ψ}}_x^n}={({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_z{\mathrm{\ψ}}_y^n-{({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_y{\mathrm{\ψ}}_z^n-T_{12}{\mathrm{\δK}}_{\mathrm{Gy}}{w}_{\mathrm{ibx}}^b-{\mathrm{\ϵ}}_x^n\\ \stackrel{\·}{{\mathrm{\ψ}}_y^n}={({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_z{\mathrm{\ψ}}_x^n+{({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_x{\mathrm{\ψ}}_z^n-T_{22}{\mathrm{\δK}}_{\mathrm{Gy}}{w}_{\mathrm{iby}}^b-{\mathrm{\ϵ}}_y^n\\ \stackrel{\·}{{\mathrm{\ψ}}_z^n}={({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_y{\mathrm{\ψ}}_x^n-{({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_x{\mathrm{\ψ}}_y^n-T_{32}{\mathrm{\δK}}_{\mathrm{Gy}}{w}_{\mathrm{ibz}}^b-{\mathrm{\ϵ}}_z^n\end{array}\right.---\left(24\right)$

式中：

$\left\{\begin{array}{c}{\mathrm{\ϵ}}_x^n=T_{11}{\mathrm{\ϵ}}_x^b+T_{12}{\mathrm{\ϵ}}_y^b+T_{13}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_y^n=T_{21}{\mathrm{\ϵ}}_x^b+T_{22}{\mathrm{\ϵ}}_y^b+T_{23}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_z^n=T_{31}{\mathrm{\ϵ}}_x^b+T_{32}{\mathrm{\ϵ}}_y^b+T_{33}{\mathrm{\ϵ}}_z^b\end{array}\right.---\left(25\right)$

其中为i(x、y、z)轴向平台坐标系相对计算地理坐标系之间的姿态误差角；

为地球自转角速率；为位移角速度；T为姿态矩阵；

c位置误差方程

由公式 ${\stackrel{\·}{\mathrm{\δ\θ}}}^n=\mathrm{\δ\ρ}-{\mathrm{\ω}}_{\mathrm{en}}^n\·{\mathrm{\δ\θ}}^n,$ ${\stackrel{\‾}{V}}^n=[I-(\mathrm{\δ\θ}\×)]V^n+{\mathrm{\δV}}^1,$ $\mathrm{\δ}\stackrel{\·}{h}={\mathrm{\δV}}_z^n$ 可得：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{\mathrm{\θ}}}_x^n=-\frac{V_z^n}{R+h}\mathrm{\δ}{\mathrm{\θ}}_x^n+\frac{V_y^n}{{(R+h)}^2}\mathrm{\δh}-\frac{1}{R+h}\mathrm{\δ}{V}_y^1\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\θ}}}_y^n=-\frac{V_z^n}{R+h}{\mathrm{\δ\θ}}_y^n-\frac{V_x^n}{{(R+h)}^2}\mathrm{\δh}+\frac{1}{R+h}{\mathrm{\δV}}_x^1\\ \mathrm{\δ}\stackrel{\·}{h}=-V_y^n{\mathrm{\δ\θ}}_x^n+V_x^n{\mathrm{\δ\θ}}_y^n+{\mathrm{\δV}}_z^1\end{array}\right.---\left(26\right)$

其中

为i(x、y、z)轴向位置误差；h为高度；

为i(x、y、z)轴向速度；为i(x、y、z)轴向速度；R为地球半径；

d速度误差方程

由公式 $\mathrm{\δ}{\stackrel{\·}{V}}^1=-g^n\×{\mathrm{\δ\θ}}_i^n-({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×{\mathrm{\δV}}^1+f^n\×{\mathrm{\ψ}}_i^n+\mathrm{\δ}{f}^n+\mathrm{\δ}{g}^n,$ 展开可得：

$\left\{\begin{array}{c}{\mathrm{\δ}\stackrel{\·}{V}}_x^1=-\mathrm{g\δ}{\mathrm{\θ}}_y-f_z^n{\mathrm{\ψ}}_y^n+f_y^n{\mathrm{\ψ}}_z^n+{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_z{\mathrm{\δV}}_y^1\\ -{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_y{\mathrm{\δV}}_z^1+T_{11}{\mathrm{\δK}}_{\mathrm{Ax}}{f}_x^b+T_{13}\mathrm{\δ}{K}_{\mathrm{Az}}{f}_z^b+{\▿}_x^n\\ \mathrm{\δ}{\stackrel{\·}{V}}_y^1=\mathrm{g\δ}{\mathrm{\θ}}_x+f_z^n{\mathrm{\ψ}}_x^n-f_x^n{\mathrm{\ψ}}_z^n-{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_z{\mathrm{\δV}}_x^1\\ +{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_x{\mathrm{\δV}}_z^1+T_{21}\mathrm{\δ}{K}_{\mathrm{Ax}}{f}_y^b+T_{23}\mathrm{\δ}{K}_{\mathrm{Az}}{f}_z^b+{\▿}_y^n\\ \mathrm{\δ}{\stackrel{\·}{V}}_z^1=\frac{{2\mathrm{gR}}^2}{{(R+h)}^3}\mathrm{\δh}-f_y^n{\mathrm{\ψ}}_x^n+f_x^n{\mathrm{\ψ}}_y^n+{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_y{\mathrm{\δV}}_x^1\\ -{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_x{\mathrm{\δV}}_y^1+T_{31}{\mathrm{\δK}}_{\mathrm{Ax}}{f}_x^b+T_{33}\mathrm{\δ}{K}_{\mathrm{Az}}{f}_z^b+{\▿}_z^n\end{array}\right.---\left(27\right)$

式中

$\left\{\begin{array}{c}{\▿}_x^n=T_{11}{\▿}_x^b+T_{12}{\▿}_y^b+T_{13}{\▿}_z^b\\ {\▿}_y^n=T_{21}{\▿}_x^b+T_{22}{\▿}_y^b+T_{23}{\▿}_z^b\\ {\▿}_z^n=T_{31}{\▿}_x^b+T_{32}{\▿}_y^b+T_{33}{\▿}_z^b\end{array}\right.---\left(28\right)$

$C_b^n=T=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(29\right)$

$\mathrm{\ρ}=w_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{V_y^n}{R+h}\\ \frac{V_x^n}{R+h}\\ {\mathrm{\ρ}}_z\\ \end{array}\right]---\left(30\right)$

e状态方程

由步骤二确立的器件误差和步骤四公式(24)、(26)、(27)确立的导航误差作为状态量组成状态矢量，如下：

$X=[{\mathrm{\δ\θ}}_x^n,{\mathrm{\δ\θ}}_y^n,\mathrm{\δh},{\mathrm{\δV}}_x^1,{\mathrm{\δV}}_y^1,{\mathrm{\δV}}_z^1,{\mathrm{\ψ}}_x^n,{\mathrm{\ψ}}_y^n,{\mathrm{\ψ}}_z^n,{\mathrm{\ϵ}}_x^b,{\mathrm{\ϵ}}_y^b,{\mathrm{\ϵ}}_z^b,{\▿}_x^b,{\▿}_y^b,{\▿}_z^b,$ (31)

${\mathrm{\δK}}_{\mathrm{Ax}},{\mathrm{\δK}}_{\mathrm{Az}},{\mathrm{\δK}}_{\mathrm{Gy}},D_{\mathrm{xy}},D_{\mathrm{zy}},{\mathrm{\δK}}_{\mathrm{dy}},{\mathrm{\δv}}_{\mathrm{dx}},{\mathrm{\δv}}_{\mathrm{dy}},{\mathrm{\δv}}_{\mathrm{dz}}{]}^T$

$F_1=\left[\begin{array}{ccc}-\frac{V_z^n}{R+h}& 0& \frac{V_y^n}{{(R+h)}^2}\\ 0& -\frac{V_z^n}{R+h}& -\frac{V_x^n}{{(R+h)}^2}\\ -V_y^n& V_x^n& 0\end{array}\right]---\left(32\right)$

$F_2=\left[\begin{array}{ccc}& & \\ 0& -g& 0\\ g& 0& 0\\ 0& 0& \frac{{2\mathrm{gR}}^2}{{(R+h)}^3}\end{array}\right]---\left(33\right)$

$F_3=\left[\begin{array}{ccc}0& -\frac{1}{R+h}& 0\\ \frac{1}{R+h}& 0& 0\\ 0& 0& 1\\ & & \end{array}\right]---\left(34\right)$

$F_4=\left[\begin{array}{ccc}0& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_y\\ -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& 0& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_x\\ {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_y& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_x& 0\end{array}\right]---\left(35\right)$

$F_5=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& -f_z^n& f_y^n& 0& 0& 0\\ f_z^n& 0& {-f}_x^n& 0& 0& 0\\ -f_y^n& f_x^n& 0& 0& 0& 0\end{array}\right]---\left(36\right)$

$F_6=\left[\begin{array}{cccccc}0& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_y& T_{11}& T_{12}& T_{13}\\ -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& 0& {({\mathrm{\ω}}_{\mathrm{en}}^n+{2\mathrm{\ω}}_{\mathrm{ie}}^n)}_x& T_{21}& T_{22}& T_{23}\\ {({\mathrm{\ω}}_{\mathrm{en}}^n+{2\mathrm{\ω}}_{\mathrm{ie}}^n)}_y& -{({\mathrm{\ω}}_{\mathrm{en}}^n+{2\mathrm{\ω}}_{\mathrm{ie}}^n)}_x& 0& T_{31}& T_{32}& T_{33}\\ 0& 0& 0& {-\mathrm{\β}}_{\mathrm{\ϵx}}& 0& 0\\ 0& 0& 0& 0& -{\mathrm{\β}}_{\mathrm{\ϵy}}& 0\\ 0& 0& 0& 0& 0& -{\mathrm{\β}}_{\mathrm{\ϵz}}\end{array}\right]---\left(37\right)$

$F_7=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ T_{11}& T_{12}& T_{13}& T_{11}{f}_x^b& T_{13}{f}_z^b& 0\\ T_{21}& T_{22}& T_{23}& T_{21}{f}_x^b& T_{23}{f}_z^b& 0\\ T_{31}& T_{32}& T_{33}& T_{31}{f}_x^b& T_{33}{f}_z^b& 0\end{array}\right]---\left(38\right)$

$F_8=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& {-T}_{12}{w}_{\mathrm{ibx}}^b\\ 0& 0& 0& 0& 0& -T_{22}{w}_{\mathrm{ibx}}^b\\ 0& 0& 0& 0& 0& -T_{32}{w}_{\mathrm{ibx}}^b\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]---\left(39\right)$

$F_9=\left[\begin{array}{cc}{F}_1& F_3\\ F_2& F_4\end{array}\right]---\left(40\right)$

$F_{11}=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& {-\mathrm{\β}}_{\mathrm{dx}}& 0& 0\\ 0& 0& 0& 0& {-\mathrm{\β}}_{\mathrm{dy}}& 0\\ 0& 0& 0& 0& 0& {-\mathrm{\β}}_{\mathrm{dz}}\end{array}\right]---\left(42\right)$

最后得到状态转移矩阵：

$F=\left[\begin{array}{cccc}{F}_9& F_5& F_7& 0_{6\×6}\\ 0_{6\×6}& F_6& F_8& 0_{6\×6}\\ 0_{6\×6}& 0_{6\×6}& F_{10}& 0_{6\×6}\\ 0_{6\×6}& 0_{6\×6}& 0_{6\×6}& F_{11}\end{array}\right]---\left(43\right)$

连续Kalman滤波状态方程为

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)---\left(44\right)$

步骤四：建立惯性/多普勒雷达组合导航量测方程

选取惯导系统解算的速度与多普勒雷达所测速度之差作为观测量，但是惯导系统解算的速度与多普勒雷达测量的速度不在一个坐标系，因此需要转化到同一坐标系下。

${\mathrm{\δV}}_{\mathrm{DOP}}^n={\tilde{V}}_{\mathrm{INU}}^n-{\tilde{V}}_{\mathrm{DOP}}^n$

$=V_{\mathrm{INU}}^n+{\mathrm{\δV}}^1-(\mathrm{\δ\θ}\×)V_{\mathrm{INU}}^n-{\tilde{C}}_b^n(V_{\mathrm{DOP}}^b+{\mathrm{\δV}}_{\mathrm{DOP}}^b)$

$=V_{\mathrm{INU}}^n+{\mathrm{\δV}}^1-(\mathrm{\δ\θ}\×)V_{\mathrm{INU}}^n-[I-(\mathrm{\φ}\×)]C_b^n(V_{\mathrm{DOP}}^b+{\mathrm{\δV}}_{\mathrm{DOP}}^b)$

$\≈V_{\mathrm{INU}}^n-C_b^n{V}_{\mathrm{DOP}}^b+{\mathrm{\δV}}^1-(\mathrm{\δ\θ}\×)V_{\mathrm{INU}}^n-C_b^n{\mathrm{\δV}}_{\mathrm{DOP}}^b+(\mathrm{\φ}\×)C_b^n{V}_{\mathrm{DOP}}^b---\left(45\right)$

$\≈{\mathrm{\δV}}^1+[(\mathrm{\φ}-\mathrm{\δ\θ})\×]C_b^n{V}_{\mathrm{INU}}^b-C_b^n{\mathrm{\δV}}_{\mathrm{DOP}}^b$

$={\mathrm{\δV}}^1-(C_b^n{V}_{\mathrm{DOP}}^b\×)\mathrm{\ψ}-C_b^n{\mathrm{\δV}}_{\mathrm{DOP}}^b$

$=Z$

由上式展开，得；

$\left\{\begin{array}{c}{Z}_x^n=(T_{31}{V}_{\mathrm{DOPx}}^b+T_{32}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_y^n-{(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V}_{\mathrm{DOPz}}^b)}_{}{\mathrm{\ψ}}_z^n\\ +{\mathrm{\δV}}_x^1-T_{11}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{xy}}-T_{13}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{zy}}-T_{12}{V}_{\mathrm{DOPy}}^b{\mathrm{\δK}}_y\\ -T_{11}{\mathrm{\δv}}_{\mathrm{dx}}-T_{12}{\mathrm{\δv}}_{\mathrm{dy}}-T_{13}{\mathrm{\δv}}_{\mathrm{dz}}\\ Z_y^n=-(T_{31}{V}_{\mathrm{DOPx}}^b+T_{31}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_x^n+(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_z^n\\ +{\mathrm{\δV}}_y^1-T_{21}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{xy}}-T_{23}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{zy}}-T_{22}{V}_{\mathrm{DOPy}}^b{\mathrm{\δK}}_y\\ -T_{21}{\mathrm{\δv}}_{\mathrm{dx}}-T_{22}{\mathrm{\δv}}_{\mathrm{dy}}-T_{23}{\mathrm{\δv}}_{\mathrm{dz}}\\ Z_z^n=(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{C}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_x^n-(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_y^n\\ +{\mathrm{\δV}}_z^1-T_{31}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{xy}}-T_{33}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{zy}}-T_{32}{V}_{\mathrm{DOPy}}^b{\mathrm{\δK}}_y\\ -T_{31}{\mathrm{\δv}}_{\mathrm{dx}}-T_{32}{\mathrm{\δv}}_{\mathrm{dy}}-T_{33}{\mathrm{\δv}}_{\mathrm{dz}}\end{array}\right.---\left(46\right)$

$H_1=\left[\begin{array}{ccc}0& (T_{31}{V}_{\mathrm{DOPx}}^b+T_{32}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b)& -(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V_{\mathrm{DOPz}}^b}_{})\\ -(T_{31}{V}_{\mathrm{DOPx}}^b+T_{32}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b)& 0& (T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b)\\ (T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V}_{\mathrm{DOPz}}^b)& -(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b)& 0\end{array}\right]---\left(47\right)$

$H_2=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(48\right)$

$H_3=\left[\begin{array}{ccc}-T_{11}{V}_{\mathrm{DOPy}}^b& -T_{13}{V}_{\mathrm{DOPy}}^b& -T_{12}{V}_{\mathrm{DOPy}}^b\\ {-T}_{21}{V}_{\mathrm{DOPy}}^b& -T_{23}{V}_{\mathrm{DOPy}}^b& -T_{22}{V}_{\mathrm{DOPy}}^b\\ -T_{31}{V}_{\mathrm{DOPy}}^b& -T_{33}{V}_{\mathrm{DOPy}}^b& -T_{32}{V}_{\mathrm{DOPy}}^b\end{array}\right]---\left(49\right)$

$H_4=\left[\begin{array}{ccc}-T_{11}& -T_{12}& -T_{13}\\ -T_{21}& -T_{22}& -T_{23}\\ -T_{31}& -T_{32}& -T_{33}\end{array}\right]---\left(50\right)$

H＝[0_3×3 H₂ H₂ 0_3×9 H₃ H₄]                    (51)

最终确立量测方程

Z＝HX+V                                         (52)

步骤五：将卡尔曼滤波连续系统离散化；

对于实际的物理系统，其状态方程一般是连续的微分方程，因此在进行滤波处理前，首先要对状态方程进行离散化处理。设描述物理系统的连续状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

Kalman滤波要求状态方程中的驱动源噪声W(t)具有如下性质：

E[W(t)]＝0，Cov[W(t)W^T(t)]＝Q(t)δ(t-τ)

对(11)式进行离散化处理，得到离散系统状态方程：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+W\left(t\right)$

其中Ф_k，k-1为一步状态转移矩阵，根据下式计算得到：

${\mathrm{\Φ}}_{k+1,k}=I+\mathrm{TF}+\frac{{\left(\mathrm{TF}\right)}^2}{2}+\frac{{\left(\mathrm{TF}\right)}^3}{6}---\left(53\right)$

离散状态方程中的系统噪声具有以下性质：

E[W_k]＝0，Cov[W_kW_j]＝Q_kδ_kj

其中：

$Q_k=\underset{i=1}{\overset{\∞}{\mathrm{\Σ}}}{M}_i\frac{\mathrm{\Δ}{T}^i}{i}---\left(54\right)$

上式中， $M_1=Q\left(t\right),M_{i+1}={\mathrm{FM}}_i+M_i^T{F}^T$

步骤六：根据惯导与多普勒雷提供的导航信息，进行Kalman滤波信息融合解算，通过状态估计，得到关于导航参数的最优或次优估计，采用闭环反馈方式对惯导和多普勒雷达同时修正。

由步骤四、步骤五建立的kalman滤波方程与纯惯导程序组成组合导航程序，对组合导航信息进行信息融合解算，得到关于导航参数的最优或次优估计，为保持组合导航精度，采用闭环反馈方式对状态方程的状态量修正补偿，即惯导系统和多普勒雷达、速度、位置、姿态进行同时修正。具体补偿如下：

a)用卡尔曼滤波估计的惯导系统和多普勒雷达误差对测量数据补偿由步骤一的器件误差模型，可对组合导航器件进行补偿

一、加速度计数据补偿

二、陀螺数据补偿

三、对多普勒雷达数据补偿

b)用卡尔曼滤波估计的速度误差对速度及时修正

由公式Vⁿ＝[I-(δθ×)]Vⁿ+δV¹可补偿：

Vⁿ＝[I+(δθ×)](Vⁿ-δV¹)                                (58)

c)用卡尔曼滤波估计的姿态误差对姿态矩阵及时修正补偿

由公式T＝{I-[(ψ+δθ)×]}T可补偿：

T＝{I+[(ψ+δθ)×]}T                                   (59)

d)用卡尔曼滤波估计的位置误差对位置矩阵及时修正补偿

由公式 $C_e^n=[I-(\mathrm{\δ\θ}\×)]C_e^n$ 可补偿：

$C_e^n=[I+(\mathrm{\δ\θ}\×)]C_e^n---\left(60\right)$

Claims (1)

1.一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法，其特征在于：该方法具体布骤如下：

步骤一：建立惯性器件、多普勒雷达的误差模型；

a.加速度计误差方程

加速度计零偏的误差模型用一阶马尔科夫过程模型方程表示：

$\left\{\begin{array}{c}{\stackrel{\·}{\▿}}_x^b=-{\mathrm{\β}}_{\▿x}{\▿}_x^b+W_{\▿x}\\ {\stackrel{\·}{\▿}}_y^b=-{\mathrm{\β}}_{\▿y}{\▿}_y^b+W_{\▿y}\\ {\stackrel{\·}{\▿}}_z^b=-{\mathrm{\β}}_{\▿z}{\▿}_z^b+W_{\▿z}\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{Ax}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{Ay}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{Az}}=0\end{array}\right.---\left(3\right)$

上述方程中，

分别为加速度计x、y、z轴向在载体坐标系下的零偏；

分别为加速度计x、y、z轴向理想值；

分别为加速度计x、y、z轴向测量值；δK_Ax、δK_Ay、δK_Az分别为加速度计x、y、z轴向刻度因子误差；A_ij为加速度计j轴向偏向i轴向的失准角；分别为加速度计x、y、z轴向噪声；

分别为加速度计x、y、z轴向过程的相关时间；

b.陀螺误差模型

陀螺零偏的误差模型用一阶马尔科夫过程模型方程表示：

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ϵ}}}_x^b=-{\mathrm{\β}}_{\mathrm{\ϵx}}{\mathrm{\ϵ}}_x^b+W_{\mathrm{\ϵx}}\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y^b=-{\mathrm{\β}}_{\mathrm{\ϵy}}{\mathrm{\ϵ}}_y^b+W_{\mathrm{\ϵy}}\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z^b=-{\mathrm{\β}}_{\mathrm{\ϵz}}{\mathrm{\ϵ}}_z^b+W_{\mathrm{\ϵz}}\end{array}\right.---\left(5\right)$

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{gx}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{gy}}=0\\ {\stackrel{\·}{\mathrm{\δ}K}}_{\mathrm{gz}}=0\end{array}\right.---\left(6\right)$

上述方程中，

分别为陀螺x、y、z轴向在载体坐标系下的零偏；

分别为x、y、z轴向陀螺理想值；

分别为x、y、z轴向陀螺测量值；δK_gx、δK_gy、δK_gz分别为陀螺x、y、z轴向刻度因子误差；M_ij为陀螺j轴向偏向i轴向的失准角；W_εx、W_εy、W_εz分别为陀螺x、y、z轴向噪声；1/β_εx、1/β_εy、1/β_εz分别为陀螺x、y、z轴向过程的相关时间；

c.多普勒雷达误差方程

多普勒雷达零偏的误差模型用一阶马尔科夫过程模型方程表示：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{dx}}=-{\mathrm{\β}}_{\mathrm{dx}}{\mathrm{\δv}}_{\mathrm{dx}}+W_{\mathrm{dx}}\\ \mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{dy}}={-\mathrm{\β}}_{\mathrm{dy}}{\mathrm{\δv}}_{\mathrm{dy}}+W_{\mathrm{dy}}\\ \mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{dz}}=-{\mathrm{\β}}_{\mathrm{dz}}{\mathrm{\δv}}_{\mathrm{dz}}+W_{\mathrm{dz}}\end{array}\right.---\left(8\right)$

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{K_{\mathrm{di}}}=0\\ {\stackrel{\·}{D}}_{\mathrm{ij}}=0\end{array}\right.i=x,y,z;j=x,y,z---\left(9\right)$

上述方程中，δv_dx、δv_dy、δv_dz分别为多普勒雷达x、y、z轴向在载体坐标系下的零偏；

分别为x、y、z轴向多普勒雷达理想值；

分别为x、y、z轴向多普勒雷达测量值；δK_dx、δK_dy、δK_dz分别为多普勒雷达x、y、z轴向刻度因子误差；D_ij为多普勒雷达j轴向偏向i轴向的失准角；W_dx、W_dy、W_dz分别为多普勒雷达x、y、z轴向噪声；1/β_dx、1/β_dy、1/β_dz分别为多普勒雷达x、y、z轴向过程的相关时间；

步骤二：基于惯性/多普勒组合导航在机载固定翼飞机上的应用，确定状态变量；

机载固定翼飞机的特点是动力沿纵轴方向，横轴和垂轴方向无动力，因此，纵轴方向速度是载体的主要速度，风力外部环境会造成横轴和垂轴方向很小的速度，即

对多普勒雷达而言，由刻度因子误差δK_d和失准角D_ij引起的误差分别为

和

因此由飞行器的特点知道δK_dx、δK_dz、D_xy、D_yz、D_zx、D_xz引起的误差忽略，只需考虑δK_dy、D_xy、D_zy引起的误差；

对惯导系统而言，失准角引起的误差远小于刻度因子引起的误差，因此不考虑失准角引起的误差；由飞行器的特点知道，航向角和俯仰角相对变化较小，飞机横滚角相对变化较大，即

较大，

较小，只需考虑陀螺由刻度因子δK_gy以及加速度计由刻度因子δK_Ay、δK_Ay引起的误差；

因此惯导系统与多普勒雷达器件需选取δK_dy、D_xy、D_zy、δv_dx、δv_dy、δv_dz、δK_gy、 ${\mathrm{\ϵ}}_y^b,{\mathrm{\ϵ}}_z^b,{\▿}_x^b,{\▿}_y^b,{\▿}_z^b,$ δK_Az、δK_Ax作为状态量；

步骤三：建立惯性/多普勒雷达组合导航状态方程；

a.Kalman滤波的基本过程如下：

首先，给定动态系统的一阶线性状态方程和量测方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(10\right)$

Z(t)＝H(t)X(t)+V(t)           (11)

进而将状态方程(10)和量测方程(11)离散化得：

X_k＝Ф_k，k-1X_k-1+Г_k-1W_k-1    (12)

Z_k＝H_kX_k+V_k                   (13)

状态预测估计方程为：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(14\right)$

方差预测方程为：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(15\right)$

状态预测估计方程为：

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

方差迭代方程：

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

$=(I-K_k{H}_k)P_{k/k-1}---\left(17\right)$

滤波增益方程为：

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

初始条件为：

${\hat{X}}_0=E\left(X_0\right)={\mathrm{\μ}}_0,\mathrm{var}{\tilde{X}}_0=\mathrm{var}{X}_0=P_0---\left(19\right)$

验前统计量为：

E[W_k]＝0，Cov[W_k，W_j]＝E[W_kW_j ^T]＝Q_kδ_kj         (20)

E[V_k]＝0，Cov[V_k，V_j]＝E[V_kV_j ^T]＝R_kδ_kj         (21)

Cov[W_k，V_j]＝E[W_kV_j ^T]＝0                        (22)

${\mathrm{\δ}}_{\mathrm{kj}}=\left\{\begin{array}{c}0,k\≠j\\ 1,k=j\end{array}\right.---\left(23\right)$

b.姿态误差方程

$\left\{\begin{array}{c}\stackrel{\·}{{\mathrm{\ψ}}_x^n}={({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_z{\mathrm{\ψ}}_y^n-{({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_y{\mathrm{\ψ}}_z^n-T_{12}{\mathrm{\δK}}_{\mathrm{Gy}}{w}_{\mathrm{ibx}}^b-{\mathrm{\ϵ}}_x^n\\ \stackrel{\·}{{\mathrm{\ψ}}_y^n}={({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_z{\mathrm{\ψ}}_x^n+{({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_x{\mathrm{\ψ}}_z^n-T_{22}{\mathrm{\δK}}_{\mathrm{Gy}}{w}_{\mathrm{iby}}^b-{\mathrm{\ϵ}}_y^n\\ \stackrel{\·}{{\mathrm{\ψ}}_z^n}={({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_y{\mathrm{\ψ}}_x^n-{({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)}_x{\mathrm{\ψ}}_y^n-T_{32}{\mathrm{\δK}}_{\mathrm{Gy}}{w}_{\mathrm{ibz}}^b-{\mathrm{\ϵ}}_z^n\end{array}\right.---\left(24\right)$

式中：

$\left\{\begin{array}{c}{\mathrm{\ϵ}}_x^n=T_{11}{\mathrm{\ϵ}}_x^b+T_{12}{\mathrm{\ϵ}}_y^b+T_{13}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_y^n=T_{21}{\mathrm{\ϵ}}_x^b+T_{22}{\mathrm{\ϵ}}_y^b+T_{23}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_z^n=T_{31}{\mathrm{\ϵ}}_x^b+T_{32}{\mathrm{\ϵ}}_y^b+T_{33}{\mathrm{\ϵ}}_z^b\end{array}\right.---\left(25\right)$

其中为i(x、y、z)轴向平台坐标系相对计算地理坐标系之间的姿态误差角；

为地球自转角速率；

为位移角速度；T为姿态矩阵；

c.位置误差方程

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{\mathrm{\θ}}}_x^n=-\frac{V_z^n}{R+h}\mathrm{\δ}{\mathrm{\θ}}_x^n+\frac{V_y^n}{{(R+h)}^2}\mathrm{\δh}-\frac{1}{R+h}\mathrm{\δ}{V}_y^1\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\θ}}}_y^n=-\frac{V_z^n}{R+h}{\mathrm{\δ\θ}}_y^n-\frac{V_x^n}{{(R+h)}^2}\mathrm{\δh}+\frac{1}{R+h}{\mathrm{\δV}}_x^1\\ \mathrm{\δ}\stackrel{\·}{h}=-V_y^n{\mathrm{\δ\θ}}_x^n+V_x^n{\mathrm{\δ\θ}}_y^n+{\mathrm{\δV}}_z^1\end{array}\right.---\left(26\right)$

其中

为i(x、y、z)轴向位置误差；h为高度；

为i(x、y、z)轴向速度；

为i(x、y、z)轴向速度误差；R为地球半径；

d.速度误差方程

$\left\{\begin{array}{c}{\mathrm{\δ}\stackrel{\·}{V}}_x^1=-\mathrm{g\δ}{\mathrm{\θ}}_y-f_z^n{\mathrm{\ψ}}_y^n+f_y^n{\mathrm{\ψ}}_z^n+{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_z{\mathrm{\δV}}_y^1\\ -{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_y{\mathrm{\δV}}_z^1+T_{11}{\mathrm{\δK}}_{\mathrm{Ax}}{f}_x^b+T_{13}\mathrm{\δ}{K}_{\mathrm{Az}}{f}_z^b+{\▿}_x^n\\ \mathrm{\δ}{\stackrel{\·}{V}}_y^1=\mathrm{g\δ}{\mathrm{\θ}}_x+f_z^n{\mathrm{\ψ}}_x^n-f_x^n{\mathrm{\ψ}}_z^n-{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_z{\mathrm{\δV}}_x^1\\ +{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_x{\mathrm{\δV}}_z^1+T_{21}\mathrm{\δ}{K}_{\mathrm{Ax}}{f}_y^b+T_{23}\mathrm{\δ}{K}_{\mathrm{Az}}{f}_z^b+{\▿}_y^n\\ \mathrm{\δ}{\stackrel{\·}{V}}_z^1=\frac{{2\mathrm{gR}}^2}{{(R+h)}^3}\mathrm{\δh}-f_y^n{\mathrm{\ψ}}_x^n+f_x^n{\mathrm{\ψ}}_y^n+{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_y{\mathrm{\δV}}_x^1\\ -{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_x{\mathrm{\δV}}_y^1+T_{31}{\mathrm{\δK}}_{\mathrm{Ax}}{f}_x^b+T_{33}\mathrm{\δ}{K}_{\mathrm{Az}}{f}_z^b+{\▿}_z^n\end{array}\right.---\left(27\right)$

式中

$\left\{\begin{array}{c}{\▿}_x^n=T_{11}{\▿}_x^b+T_{12}{\▿}_y^b+T_{13}{\▿}_z^b\\ {\▿}_y^n=T_{21}{\▿}_x^b+T_{22}{\▿}_y^b+T_{23}{\▿}_z^b\\ {\▿}_z^n=T_{31}{\▿}_x^b+T_{32}{\▿}_y^b+T_{33}{\▿}_z^b\end{array}\right.---\left(28\right)$

$C_b^n=T=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(29\right)$

$\mathrm{\ρ}=w_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{V_y^n}{R+h}\\ \frac{V_x^n}{R+h}\\ {\mathrm{\ρ}}_z\\ \end{array}\right]---\left(30\right)$

e.状态方程

由步骤二确立的器件误差和步骤四公式(24)、(26)、(27)确立的导航误差作为状态量组成状态矢量，如下：

$X=[{\mathrm{\δ\θ}}_x^n,{\mathrm{\δ\θ}}_y^n,\mathrm{\δh},{\mathrm{\δV}}_x^1,{\mathrm{\δV}}_y^1,{\mathrm{\δV}}_z^1,{\mathrm{\ψ}}_x^n,{\mathrm{\ψ}}_y^n,{\mathrm{\ψ}}_z^n,{\mathrm{\ϵ}}_x^b,{\mathrm{\ϵ}}_y^b,{\mathrm{\ϵ}}_z^b,{\▿}_x^b,{\▿}_y^b,{\▿}_z^b,$ (31)

${\mathrm{\δK}}_{\mathrm{Ax}},{\mathrm{\δK}}_{\mathrm{Az}},{\mathrm{\δK}}_{\mathrm{Gy}},D_{\mathrm{xy}},D_{\mathrm{zy}},{\mathrm{\δK}}_{\mathrm{dy}},{\mathrm{\δv}}_{\mathrm{dx}},{\mathrm{\δv}}_{\mathrm{dy}},{\mathrm{\δv}}_{\mathrm{dz}}{]}^T$

$F_1=\left[\begin{array}{ccc}-\frac{V_z^n}{R+h}& 0& \frac{V_y^n}{{(R+h)}^2}\\ 0& -\frac{V_z^n}{R+h}& -\frac{V_x^n}{{(R+h)}^2}\\ -V_y^n& V_x^n& 0\end{array}\right]---\left(32\right)$

$F_2=\left[\begin{array}{ccc}& & \\ 0& -g& 0\\ g& 0& 0\\ 0& 0& \frac{{2\mathrm{gR}}^2}{{(R+h)}^3}\end{array}\right]---\left(33\right)$

$F_3=\left[\begin{array}{ccc}0& -\frac{1}{R+h}& 0\\ \frac{1}{R+h}& 0& 0\\ 0& 0& 1\\ & & \end{array}\right]---\left(34\right)$

$F_4=\left[\begin{array}{ccc}0& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_y\\ -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& 0& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_x\\ {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_y& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_x& 0\end{array}\right]---\left(35\right)$

$F_5=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& -f_z^n& f_y^n& 0& 0& 0\\ f_z^n& 0& {-f}_x^n& 0& 0& 0\\ -f_y^n& f_x^n& 0& 0& 0& 0\end{array}\right]---\left(36\right)$

$F_6=\left[\begin{array}{cccccc}0& {({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_y& T_{11}& T_{12}& T_{13}\\ -{({\mathrm{\ω}}_{\mathrm{en}}^n+2{\mathrm{\ω}}_{\mathrm{ie}}^n)}_z& 0& {({\mathrm{\ω}}_{\mathrm{en}}^n+{2\mathrm{\ω}}_{\mathrm{ie}}^n)}_x& T_{21}& T_{22}& T_{23}\\ {({\mathrm{\ω}}_{\mathrm{en}}^n+{2\mathrm{\ω}}_{\mathrm{ie}}^n)}_y& -{({\mathrm{\ω}}_{\mathrm{en}}^n+{2\mathrm{\ω}}_{\mathrm{ie}}^n)}_x& 0& T_{31}& T_{32}& T_{33}\\ 0& 0& 0& {-\mathrm{\β}}_{\mathrm{\ϵx}}& 0& 0\\ 0& 0& 0& 0& -{\mathrm{\β}}_{\mathrm{\ϵy}}& 0\\ 0& 0& 0& 0& 0& -{\mathrm{\β}}_{\mathrm{\ϵz}}\end{array}\right]---\left(37\right)$

$F_7=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ T_{11}& T_{12}& T_{13}& T_{11}{f}_x^b& T_{13}{f}_z^b& 0\\ T_{21}& T_{22}& T_{23}& T_{21}{f}_x^b& T_{23}{f}_z^b& 0\\ T_{31}& T_{32}& T_{33}& T_{31}{f}_x^b& T_{33}{f}_z^b& 0\end{array}\right]---\left(38\right)$

$F_8=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& {-T}_{12}{w}_{\mathrm{ibx}}^b\\ 0& 0& 0& 0& 0& -T_{22}{w}_{\mathrm{ibx}}^b\\ 0& 0& 0& 0& 0& -T_{32}{w}_{\mathrm{ibx}}^b\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]---\left(39\right)$

$F_9=\left[\begin{array}{cc}{F}_1& F_3\\ F_2& F_4\end{array}\right]---\left(40\right)$

$F_{11}=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& {-\mathrm{\β}}_{\mathrm{dx}}& 0& 0\\ 0& 0& 0& 0& {-\mathrm{\β}}_{\mathrm{dy}}& 0\\ 0& 0& 0& 0& 0& {-\mathrm{\β}}_{\mathrm{dz}}\end{array}\right]---\left(42\right)$

最后得到状态转移矩阵：

$F=\left[\begin{array}{cccc}{F}_9& F_5& F_7& 0_{6\×6}\\ 0_{6\×6}& F_6& F_8& 0_{6\×6}\\ 0_{6\×6}& 0_{6\×6}& F_{10}& 0_{6\×6}\\ 0_{6\×6}& 0_{6\×6}& 0_{6\×6}& F_{11}\end{array}\right]---\left(43\right)$

连续Kalman滤波状态方程为

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)---\left(44\right)$

步骤四：建立惯性/多普勒雷达组合导航量测方程；

${\mathrm{\δV}}_{\mathrm{DOP}}^n={\tilde{V}}_{\mathrm{INU}}^n-{\tilde{V}}_{\mathrm{DOP}}^n$

$=V_{\mathrm{INU}}^n+{\mathrm{\δV}}^1-(\mathrm{\δ\θ}\×)V_{\mathrm{INU}}^n-{\tilde{C}}_b^n(V_{\mathrm{DOP}}^b+{\mathrm{\δV}}_{\mathrm{DOP}}^b)$

$=V_{\mathrm{INU}}^n+{\mathrm{\δV}}^1-(\mathrm{\δ\θ}\×)V_{\mathrm{INU}}^n-[I-(\mathrm{\φ}\×)]C_b^n(V_{\mathrm{DOP}}^b+{\mathrm{\δV}}_{\mathrm{DOP}}^b)$

$\≈V_{\mathrm{INU}}^n-C_b^n{V}_{\mathrm{DOP}}^b+{\mathrm{\δV}}^1-(\mathrm{\δ\θ}\×)V_{\mathrm{INU}}^n-C_b^n{\mathrm{\δV}}_{\mathrm{DOP}}^b+(\mathrm{\φ}\×)C_b^n{V}_{\mathrm{DOP}}^b---\left(45\right)$

$\≈{\mathrm{\δV}}^1+[(\mathrm{\φ}-\mathrm{\δ\θ})\×]C_b^n{V}_{\mathrm{INU}}^b-C_b^n{\mathrm{\δV}}_{\mathrm{DOP}}^b$

$={\mathrm{\δV}}^1-(C_b^n{V}_{\mathrm{DOP}}^b\×)\mathrm{\ψ}-C_b^n{\mathrm{\δV}}_{\mathrm{DOP}}^b$

$=Z$

$\left\{\begin{array}{c}{Z}_x^n=(T_{31}{V}_{\mathrm{DOPx}}^b+T_{32}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_y^n-{(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V}_{\mathrm{DOPz}}^b)}_{}{\mathrm{\ψ}}_z^n\\ +{\mathrm{\δV}}_x^1-T_{11}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{xy}}-T_{13}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{zy}}-T_{12}{V}_{\mathrm{DOPy}}^b{\mathrm{\δK}}_y\\ -T_{11}{\mathrm{\δv}}_{\mathrm{dx}}-T_{12}{\mathrm{\δv}}_{\mathrm{dy}}-T_{13}{\mathrm{\δv}}_{\mathrm{dz}}\\ Z_y^n=-(T_{31}{V}_{\mathrm{DOPx}}^b+T_{31}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_x^n+(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_z^n\\ +{\mathrm{\δV}}_y^1-T_{21}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{xy}}-T_{23}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{zy}}-T_{22}{V}_{\mathrm{DOPy}}^b{\mathrm{\δK}}_y\\ -T_{21}{\mathrm{\δv}}_{\mathrm{dx}}-T_{22}{\mathrm{\δv}}_{\mathrm{dy}}-T_{23}{\mathrm{\δv}}_{\mathrm{dz}}\\ Z_z^n=(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{C}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_x^n-(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b){\mathrm{\ψ}}_y^n\\ +{\mathrm{\δV}}_z^1-T_{31}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{xy}}-T_{33}{V}_{\mathrm{DOPy}}^b{D}_{\mathrm{zy}}-T_{32}{V}_{\mathrm{DOPy}}^b{\mathrm{\δK}}_y\\ -T_{31}{\mathrm{\δv}}_{\mathrm{dx}}-T_{32}{\mathrm{\δv}}_{\mathrm{dy}}-T_{33}{\mathrm{\δv}}_{\mathrm{dz}}\end{array}\right.---\left(46\right)$

$H_1=\left[\begin{array}{ccc}0& (T_{31}{V}_{\mathrm{DOPx}}^b+T_{32}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b)& -(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V_{\mathrm{DOPz}}^b}_{})\\ -(T_{31}{V}_{\mathrm{DOPx}}^b+T_{32}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b)& 0& (T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b)\\ (T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V}_{\mathrm{DOPz}}^b)& -(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b)& 0\end{array}\right]---\left(47\right)$

$H_2=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]---\left(48\right)$

$H_3=\left[\begin{array}{ccc}-T_{11}{V}_{\mathrm{DOPy}}^b& -T_{13}{V}_{\mathrm{DOPy}}^b& -T_{12}{V}_{\mathrm{DOPy}}^b\\ {-T}_{21}{V}_{\mathrm{DOPy}}^b& -T_{23}{V}_{\mathrm{DOPy}}^b& -T_{22}{V}_{\mathrm{DOPy}}^b\\ -T_{31}{V}_{\mathrm{DOPy}}^b& -T_{33}{V}_{\mathrm{DOPy}}^b& -T_{32}{V}_{\mathrm{DOPy}}^b\end{array}\right]---\left(49\right)$

$H_4=\left[\begin{array}{ccc}-T_{11}& -T_{12}& -T_{13}\\ -T_{21}& -T_{22}& -T_{23}\\ -T_{31}& -T_{32}& -T_{33}\end{array}\right]---\left(50\right)$

H＝[0_3×3 H₂ H₂ 0_3×9 H₃ H₄]          (51)

最终确立量测方程

Z＝HX+V                  (52)

步骤五：将卡尔曼滤波连续系统离散化；

将连续系统离散化

${\mathrm{\Φ}}_{k+1,k}=I+\mathrm{TF}+\frac{{\left(\mathrm{TF}\right)}^2}{2}+\frac{{\left(\mathrm{TF}\right)}^3}{6}---\left(53\right)$

$Q_k=\underset{i=1}{\overset{\∞}{\mathrm{\Σ}}}{M}_i\frac{\mathrm{\Δ}{T}^i}{i}---\left(54\right)$

其中 $M_1=Q\left(t\right),M_{i+1}={\mathrm{FM}}_i+M_i^T{F}^T;$

步骤六：根据惯导与多普勒雷提供的导航信息，进行Kalman滤波信息融合解算，通过状态估计，得到关于导航参数的最优或次优估计，采用闭环反馈方式对惯导和多普勒雷达同时修正；

a.用卡尔曼滤波估计的惯导系统和多普勒雷达误差对测量数据补偿

1.对加速度计数据补偿

2.对陀螺数据补偿

对多普勒雷达数据补偿

b.用卡尔曼滤波估计的速度误差对速度及时修正

Vⁿ＝[I+(δθ×)](Vⁿ-δV¹)                      (58)

c.用卡尔曼滤波估计的姿态误差对姿态矩阵及时修正补偿

T＝{I+[(ψ+δθ)×]}T                      (59)

d.用卡尔曼滤波估计的位置误差对位置矩阵及时修正补偿

$C_e^n=[I+(\mathrm{\δ\θ}\×)]C_e^n.---\left(60\right)$

Images