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

摘要

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

主权项

1.一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法，其特征在于：该方法具体布骤如下：步骤一：建立惯性器件、多普勒雷达的误差模型；a.加速度计误差方程加速度计零偏的误差模型用一阶马尔科夫过程模型方程表示：$\left\{\begin{array}{c}{\stackrel{·}{▿}}_x^b=-{\beta }_{▿x}{▿}_x^b+W_{▿x}\\ {\stackrel{·}{▿}}_y^b=-{\beta }_{▿y}{▿}_y^b+W_{▿y}\\ {\stackrel{·}{▿}}_z^b=-{\beta }_{▿z}{▿}_z^b+W_{▿z}\end{array}\right.---\left(2\right)$$\left\{\begin{array}{c}{\stackrel{·}{\delta K}}_{\mathrm{Ax}}=0\\ {\stackrel{·}{\delta K}}_{\mathrm{Ay}}=0\\ {\stackrel{·}{\delta 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{·}{ϵ}}_x^b=-{\beta }_{\mathrm{ϵx}}{ϵ}_x^b+W_{\mathrm{ϵx}}\\ {\stackrel{·}{ϵ}}_y^b=-{\beta }_{\mathrm{ϵy}}{ϵ}_y^b+W_{\mathrm{ϵy}}\\ {\stackrel{·}{ϵ}}_z^b=-{\beta }_{\mathrm{ϵz}}{ϵ}_z^b+W_{\mathrm{ϵz}}\end{array}\right.---\left(5\right)$$\left\{\begin{array}{c}{\stackrel{·}{\delta K}}_{\mathrm{gx}}=0\\ {\stackrel{·}{\delta K}}_{\mathrm{gy}}=0\\ {\stackrel{·}{\delta 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}\delta {\stackrel{·}{v}}_{\mathrm{dx}}=-{\beta }_{\mathrm{dx}}{\mathrm{\delta v}}_{\mathrm{dx}}+W_{\mathrm{dx}}\\ \delta {\stackrel{·}{v}}_{\mathrm{dy}}={-\beta }_{\mathrm{dy}}{\mathrm{\delta v}}_{\mathrm{dy}}+W_{\mathrm{dy}}\\ \delta {\stackrel{·}{v}}_{\mathrm{dz}}=-{\beta }_{\mathrm{dz}}{\mathrm{\delta v}}_{\mathrm{dz}}+W_{\mathrm{dz}}\end{array}\right.---\left(8\right)$$\left\{\begin{array}{c}\delta \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、${ϵ}_y^b,{ϵ}_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}={\Phi }_{k,k-1}{\hat{X}}_{k-1}---\left(14\right)$方差预测方程为：$P_{k/k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{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)={\mu }_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)${\delta }_{\mathrm{kj}}=\left\{\begin{array}{c}0,k\ne j\\ 1,k=j\end{array}\right.---\left(23\right)$b.姿态误差方程$\left\{\begin{array}{c}\stackrel{·}{{\psi }_x^n}={({\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)}_z{\psi }_y^n-{({\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)}_y{\psi }_z^n-T_{12}{\mathrm{\delta K}}_{\mathrm{Gy}}{w}_{\mathrm{ibx}}^b-{ϵ}_x^n\\ \stackrel{·}{{\psi }_y^n}={({\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)}_z{\psi }_x^n+{({\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)}_x{\psi }_z^n-T_{22}{\mathrm{\delta K}}_{\mathrm{Gy}}{w}_{\mathrm{iby}}^b-{ϵ}_y^n\\ \stackrel{·}{{\psi }_z^n}={({\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)}_y{\psi }_x^n-{({\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)}_x{\psi }_y^n-T_{32}{\mathrm{\delta K}}_{\mathrm{Gy}}{w}_{\mathrm{ibz}}^b-{ϵ}_z^n\end{array}\right.---\left(24\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(25\right)$其中为i(x、y、z)轴向平台坐标系相对计算地理坐标系之间的姿态误差角；为地球自转角速率；为位移角速度；T为姿态矩阵；c.位置误差方程$\left\{\begin{array}{c}\delta {\stackrel{·}{\theta }}_x^n=-\frac{V_z^n}{R+h}\delta {\theta }_x^n+\frac{V_y^n}{{(R+h)}^2}\mathrm{\delta h}-\frac{1}{R+h}\delta V_y^1\\ \delta {\stackrel{·}{\theta }}_y^n=-\frac{V_z^n}{R+h}{\mathrm{\delta \theta }}_y^n-\frac{V_x^n}{{(R+h)}^2}\mathrm{\delta h}+\frac{1}{R+h}{\mathrm{\delta V}}_x^1\\ \delta \stackrel{·}{h}=-V_y^n{\mathrm{\delta \theta }}_x^n+V_x^n{\mathrm{\delta \theta }}_y^n+{\mathrm{\delta 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}{\delta \stackrel{·}{V}}_x^1=-\mathrm{g\delta }{\theta }_y-f_z^n{\psi }_y^n+f_y^n{\psi }_z^n+{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_z{\mathrm{\delta V}}_y^1\\ -{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_y{\mathrm{\delta V}}_z^1+T_{11}{\mathrm{\delta K}}_{\mathrm{Ax}}{f}_x^b+T_{13}\delta K_{\mathrm{Az}}{f}_z^b+{▿}_x^n\\ \delta {\stackrel{·}{V}}_y^1=\mathrm{g\delta }{\theta }_x+f_z^n{\psi }_x^n-f_x^n{\psi }_z^n-{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_z{\mathrm{\delta V}}_x^1\\ +{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_x{\mathrm{\delta V}}_z^1+T_{21}\delta K_{\mathrm{Ax}}{f}_y^b+T_{23}\delta K_{\mathrm{Az}}{f}_z^b+{▿}_y^n\\ \delta {\stackrel{·}{V}}_z^1=\frac{{2\mathrm{gR}}^2}{{(R+h)}^3}\mathrm{\delta h}-f_y^n{\psi }_x^n+f_x^n{\psi }_y^n+{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_y{\mathrm{\delta V}}_x^1\\ -{(w_{\mathrm{en}}^n+{2w}_{\mathrm{ie}}^n)}_x{\mathrm{\delta V}}_y^1+T_{31}{\mathrm{\delta K}}_{\mathrm{Ax}}{f}_x^b+T_{33}\delta 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)$$\rho =w_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{V_y^n}{R+h}\\ \frac{V_x^n}{R+h}\\ {\rho }_z\\ \end{array}\right]---\left(30\right)$e.状态方程由步骤二确立的器件误差和步骤四公式(24)、(26)、(27)确立的导航误差作为状态量组成状态矢量，如下：$X=[{\mathrm{\delta \theta }}_x^n,{\mathrm{\delta \theta }}_y^n,\mathrm{\delta h},{\mathrm{\delta V}}_x^1,{\mathrm{\delta V}}_y^1,{\mathrm{\delta V}}_z^1,{\psi }_x^n,{\psi }_y^n,{\psi }_z^n,{ϵ}_x^b,{ϵ}_y^b,{ϵ}_z^b,{▿}_x^b,{▿}_y^b,{▿}_z^b,$(31)${\mathrm{\delta K}}_{\mathrm{Ax}},{\mathrm{\delta K}}_{\mathrm{Az}},{\mathrm{\delta K}}_{\mathrm{Gy}},D_{\mathrm{xy}},D_{\mathrm{zy}},{\mathrm{\delta K}}_{\mathrm{dy}},{\mathrm{\delta v}}_{\mathrm{dx}},{\mathrm{\delta v}}_{\mathrm{dy}},{\mathrm{\delta 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& {({\omega }_{\mathrm{en}}^n+2{\omega }_{\mathrm{ie}}^n)}_z& -{({\omega }_{\mathrm{en}}^n+2{\omega }_{\mathrm{ie}}^n)}_y\\ -{({\omega }_{\mathrm{en}}^n+2{\omega }_{\mathrm{ie}}^n)}_z& 0& {({\omega }_{\mathrm{en}}^n+2{\omega }_{\mathrm{ie}}^n)}_x\\ {({\omega }_{\mathrm{en}}^n+2{\omega }_{\mathrm{ie}}^n)}_y& {({\omega }_{\mathrm{en}}^n+2{\omega }_{\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& {({\omega }_{\mathrm{en}}^n+2{\omega }_{\mathrm{ie}}^n)}_z& -{({\omega }_{\mathrm{en}}^n+2{\omega }_{\mathrm{ie}}^n)}_y& T_{11}& T_{12}& T_{13}\\ -{({\omega }_{\mathrm{en}}^n+2{\omega }_{\mathrm{ie}}^n)}_z& 0& {({\omega }_{\mathrm{en}}^n+{2\omega }_{\mathrm{ie}}^n)}_x& T_{21}& T_{22}& T_{23}\\ {({\omega }_{\mathrm{en}}^n+{2\omega }_{\mathrm{ie}}^n)}_y& -{({\omega }_{\mathrm{en}}^n+{2\omega }_{\mathrm{ie}}^n)}_x& 0& T_{31}& T_{32}& T_{33}\\ 0& 0& 0& {-\beta }_{\mathrm{ϵx}}& 0& 0\\ 0& 0& 0& 0& -{\beta }_{\mathrm{ϵy}}& 0\\ 0& 0& 0& 0& 0& -{\beta }_{\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& {-\beta }_{\mathrm{dx}}& 0& 0\\ 0& 0& 0& 0& {-\beta }_{\mathrm{dy}}& 0\\ 0& 0& 0& 0& 0& {-\beta }_{\mathrm{dz}}\end{array}\right]---\left(42\right)$最后得到状态转移矩阵：$F=\left[\begin{array}{cccc}{F}_9& F_5& F_7& 0_{6\times 6}\\ 0_{6\times 6}& F_6& F_8& 0_{6\times 6}\\ 0_{6\times 6}& 0_{6\times 6}& F_{10}& 0_{6\times 6}\\ 0_{6\times 6}& 0_{6\times 6}& 0_{6\times 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{\delta V}}_{\mathrm{DOP}}^n={\tilde{V}}_{\mathrm{INU}}^n-{\tilde{V}}_{\mathrm{DOP}}^n$$=V_{\mathrm{INU}}^n+{\mathrm{\delta V}}^1-(\mathrm{\delta \theta }\times )V_{\mathrm{INU}}^n-{\tilde{C}}_b^n(V_{\mathrm{DOP}}^b+{\mathrm{\delta V}}_{\mathrm{DOP}}^b)$$=V_{\mathrm{INU}}^n+{\mathrm{\delta V}}^1-(\mathrm{\delta \theta }\times )V_{\mathrm{INU}}^n-[I-(\phi \times )]C_b^n(V_{\mathrm{DOP}}^b+{\mathrm{\delta V}}_{\mathrm{DOP}}^b)$$\approx V_{\mathrm{INU}}^n-C_b^n{V}_{\mathrm{DOP}}^b+{\mathrm{\delta V}}^1-(\mathrm{\delta \theta }\times )V_{\mathrm{INU}}^n-C_b^n{\mathrm{\delta V}}_{\mathrm{DOP}}^b+(\phi \times )C_b^n{V}_{\mathrm{DOP}}^b---\left(45\right)$$\approx {\mathrm{\delta V}}^1+[(\phi -\mathrm{\delta \theta })\times ]C_b^n{V}_{\mathrm{INU}}^b-C_b^n{\mathrm{\delta V}}_{\mathrm{DOP}}^b$$={\mathrm{\delta V}}^1-(C_b^n{V}_{\mathrm{DOP}}^b\times )\psi -C_b^n{\mathrm{\delta 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){\psi }_y^n-{(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{V}_{\mathrm{DOPz}}^b)}_{}{\psi }_z^n\\ +{\mathrm{\delta 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{\delta K}}_y\\ -T_{11}{\mathrm{\delta v}}_{\mathrm{dx}}-T_{12}{\mathrm{\delta v}}_{\mathrm{dy}}-T_{13}{\mathrm{\delta v}}_{\mathrm{dz}}\\ Z_y^n=-(T_{31}{V}_{\mathrm{DOPx}}^b+T_{31}{V}_{\mathrm{DOPy}}^b+T_{33}{V}_{\mathrm{DOPz}}^b){\psi }_x^n+(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b){\psi }_z^n\\ +{\mathrm{\delta 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{\delta K}}_y\\ -T_{21}{\mathrm{\delta v}}_{\mathrm{dx}}-T_{22}{\mathrm{\delta v}}_{\mathrm{dy}}-T_{23}{\mathrm{\delta v}}_{\mathrm{dz}}\\ Z_z^n=(T_{21}{V}_{\mathrm{DOPx}}^b+T_{22}{V}_{\mathrm{DOPy}}^b+T_{23}{C}_{\mathrm{DOPz}}^b){\psi }_x^n-(T_{11}{V}_{\mathrm{DOPx}}^b+T_{12}{V}_{\mathrm{DOPy}}^b+T_{13}{V}_{\mathrm{DOPz}}^b){\psi }_y^n\\ +{\mathrm{\delta 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{\delta K}}_y\\ -T_{31}{\mathrm{\delta v}}_{\mathrm{dx}}-T_{32}{\mathrm{\delta v}}_{\mathrm{dy}}-T_{33}{\mathrm{\delta 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)步骤五：将卡尔曼滤波连续系统离散化；将连续系统离散化${\Phi }_{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{\infty }{\Sigma }}{M}_i\frac{\Delta 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{\delta \theta }\times )]C_e^n.---\left(60\right)$