CN114459476A - 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法 - Google Patents

Abstract

基于虚拟速度量测的水下无人潜航器测流DVL/SINS组合导航方法，首先，获取安装在水下无人潜航器上的多普勒测速仪的速度量测数据以及惯性测量元件所输出的陀螺仪和加速度计的输出数据；然后，利用DVL的输出、IMU的输出以及SINS捷联解算获得的历史速度数据构建一组虚拟速度量测，该虚拟速度量测为对地速度；最终，将所构建的虚拟速度量测与IMU、DVL传感器的输出一同输入至卡尔曼滤波器中，完成多传感器融合后的对水下无人潜航器的定位与导航。本发明算法有效解决了因DVL探测范围受限导致中层水域工作的水下无人潜航器无法获得对地速度的问题，与普通的卡尔曼滤波组合导航算法相比，其速度估计精度更高，因而对定位精度有较大的提升。

Description

基于虚拟速度量测的水下无人潜航器测流DVL/SINS组合导航 方法

技术领域

本发明属于水下无人潜航器组合导航领域，尤其涉及SINS系统与DVL系统的组合导航，适用于在中层水域工作的水下无人潜航器，在GPS拒止条件下，完成高精度的定位与导航任务。

背景技术

水下无人潜航器(Autonomous Underwater Vehicle,AUV)的研究一直受到广泛的关注，AUV在众多水下的作业任务中都发挥着很重要的作用。AUV的工作效率受到导航和定位精度的影响，对于水下工作的AUV，其卫星信号收到遮挡，无法利用卫星进行导航与定位。目前，较为常见的导航方式包括基于惯性基传感器的航位推算式导航，基于声学的水声测距定位以及基于地球物理学的匹配定位。其中，基于水声测距的定位需要提前布置声学阵或信标，而基于地球物理学的匹配定位需要提前获得水下地形和地磁等相关数据库信息，这使得这两种导航方式的使用有诸多限制。对于一个未知的水下环境，这两种方式不能使用。

惯性导航(StrapdownInertialnavigationsystem，SINS)是一种基于加速度计和陀螺仪输出的数据进行积分运算，进而获得系统的姿态、位置信息的技术，其定位原理无需外界信号，具有隐蔽性强、短期精度高等特点，但是其仪表误差会随积分累积，在长时间的水下作业中，无法保证其定位精度。多普勒测速仪(Doppler velocity log,DVL)是一种利用发射的声波和接收的水底反射波之间的多普勒频移测量船舶相对于水底的速度的传感器，可以安装在载体底部，提供较高精度的速度量测。目前，SINS/DVL组合导航系统是一种主流的组合导航方案并被广泛应用。然而，多普勒测速仪的精度与最大深度相关，其发射频率越高，精度就越高，但是其可以穿透的最大深度会变低，当超过其最大深度限制时，DVL无法获得对地速度，只能获得其对水层速度。此时，普通的SINS/DVL算法精度会受到影响。

专利文献(CN112097763A)利用卡尔曼滤波器实现了IMU/DVL/磁力计的水下组合导航方案，通过卡尔曼滤波的方案使得系统的定位精度得到提高。但是，该专利中使用的DVL速度为载体对地速度，没有考虑洋流对于DVL量测输出值的影响。当水下运载体工作在中层水域时，无法获得准确的载体对地速度。此时若不对未知洋流进行同时的估计和处理，定位效果将大幅下降。专利文献(CN110567454A)利用DVL所获得的波束信息作为量测量进行SINS/DVL紧组合卡尔曼滤波组合导航，在其中考虑了波束信息受洋流影响时的处理方法。但是紧组合中需要的DVL波束信息不易获取。本发明所采用的基于虚拟速度量测的水下无人潜航器测流DVL/SINS组合导航方法，在使用相同传感器信息的前提下，对DVL和IMU的速度变化信息进行了额外的提取与融合操作，利用洋流速度相对稳定特性来构建一个新的虚拟速度，在构建的虚拟速度中，耦合的未知洋流速度被暂时屏蔽，来实现速度误差与洋流速度的解耦，从而提高DVL量测值受到洋流干扰的时候组合导航的整体定位精度。本发明在实施过程中，只需要获取解算完成后的DVL速度量测，且不需要其它额外的传感器，计算难度低，与其他方法相比具有较大优势。

发明内容

发明目的：本发明要解决的技术问题在于，为保证DVL拥有较高的测速精度，DVL的工作频率需要很高，其探测的最大深度受到限制，当AUV在中层水域工作时，其距离海底的距离大于DVL最大探测深度，此时DVL测量的速度为对流速度，其值等于AUV对地的速度加上一个未知洋流的速度。此时，需要将未知洋流当成一个额外的状态量加入到卡尔曼滤波器的估计中。然而，洋流状态会与SINS的速度误差状态量互相耦合，导致卡尔曼滤波器中速度误差状态量的估计速度和精度下降。因此，需要对组合导航方案进行一定的改进以解决测流DVL/SINS组合导航中所出现的未知状态量耦合问题。

技术方案：为实现本发明的目的，本发明提供基于虚拟速度量测的中层水域SINS/测流DVL组合导航方法，具体方法如下：

(1)获取安装在水下无人潜航器上的传感器实时数据，具体包括多普勒测速仪的速度量测数据以及惯性测量元件所输出的陀螺仪和加速度计的输出数据，并利用传感器数据执行姿态解算算法，获得SINS系统以及DVL系统的导航系下速度，具体步骤如下：

S1.1：选取当地地理坐标系作为导航坐标系，分别指向东、北、天，记导航坐标系为n；载体坐标系原点位于载体重心，三个轴分别指向载体的右、前、上，记载体坐标系为b；记惯性坐标系为i、地球坐标系为e，则导航坐标系下导航解算的微分方程如式(1)～(4)：

其中，q表示载体坐标系到导航坐标系的旋转四元数；ωnbb表示载体系b和导航系n之间的相对角速度在载体系b下的投影；ωien表示地球系e与惯性系i的相对角速度在导航系n下的投影；ωenn表示导航系n与地球系e之间的相对角速度在导航系n下的投影；Vn＝vEvNvUT是对地速度在导航系n下的投影；fb表示加速度计输出的比力信息；gn表示重力加速度在导航系n下的投影；L、λ和h分别表示纬度、经度和高度；R_n、R_e分别表示地球子午圈、卯酉圈曲率半径；

S1.2：已知初始时刻的姿态q(0)、速度v(0)和位置P(0)，通过对导航解算的微分方程(2)-(4)进行积分，实时递归计算得到当前时刻的姿态、速度和位置。

S1.3：DVL传感器安装在载体的底部，通过向下发送超声波以完成对载体速度的测量。高精度的DVL传感器分别在载体的前后左右四个方向发送超声波束，可获得AUV在载体坐标系下的三维速度Vdvlb＝[vdvlrightvdvlfrontvdvlup]。然后，利用S1.2中SINS系统解算获得的AUV姿态，可获取到导航坐标系下的三维速度：

(2)利用步骤(1)中获得的IMU输出、SINS系统以及DVL系统的导航系下速度，构建一组AUV的虚拟对地速度量测；

步骤(2)中t时刻虚拟速度量测的构建方程具体为：

式中，

代表构建的t时刻不含洋流干扰的由虚拟东向速度量测和虚拟北向速度量测组成的虚拟速度量测向量，

为由SINS在t-1时刻经过捷联解算算法获得的东向速度和北向速度组成的向量，

为从t-1时刻到t时刻构建的速度变化量，其计算方式如下：

式中，

为DVL测量误差协方差矩阵，根据DVL器件的测速精度设置，而

为SINS系统的速度估计误差协方差矩阵，从滤波器中提取；ΔV_est的计算方式如下：

(8)式中，f^b(t)为加速度计在t时刻的比力输出值，

为SINS系统t时刻解算获得的姿态矩阵，φ为SINS系统计算导航系与实际的地理导航系之间的失准角，b_a为SINS中加速度计的常值零偏估计值。

(3)将步骤(2)中构建的水下无人潜航器虚拟对地速度量测与步骤(1)中获得SINS系统、DVL系统速度输出一同输入至卡尔曼滤波器中，进行信息融合，完成对水下无人潜航器的定位与导航，具体步骤如下；

S3.1：选取SINS系统姿态失准角、SINS系统导航系速度误差、SINS系统位置误差、陀螺常值误差和、速度计常值误差以及水流对地速度作为17维状态变量，选取DVL系统与SINS系统导航系速度误差、SINS系统与高度计高度测量误差以及SINS系统与虚拟速度量测导航系速度误差作为5维量测变量，获得离散化的kalman滤波状态方程和量测方程；

S3.2：给定状态估计和估计误差方差的初始值X0和P0，根据k时刻的观测值Z(k)，实时递推计算得到k时刻的状态估计Xkk；

S3.3：利用步骤3.2中得到的状态估计对SINS输出的导航参数进行修正。

作为本发明进一步改进，ΔV_est的计算方式如下：

式中，f^b(t)为加速度计在t时刻的比力输出值，

为SINS系统t时刻解算获得的姿态矩阵，φ为SINS系统计算导航系与实际的地理导航系之间的失准角，b_a为SINS中加速度计的常值零偏估计值。

作为本发明进一步改进，S3.1中状态变量的选取如下：

S3.1中离散化的kalman滤波状态方程为：

Xk+1＝FkXk+Γ(k)W(k)Zk+1＝Hk+1Xk+1+V(k+1) (10)

其中，Xk+1为k+1时刻的状态估计，Xk为k时刻的状态估计，Zk+1为k+1时刻的观测值，Fk为k时刻的状态转移矩阵，Γ(k)为系统过程噪声输入矩阵，Hk+1为k+1时刻的观测矩阵，W(k)是状态转移中的随机过程噪声，V(k+1)是随机量测噪声；

S3.1中量测变量表示如下：

其中，

分别代表由SINS东向、北向速度输出，

分别代表DVL东向、北向速度输出，

分别代表步骤(2)中构建的虚拟东向速度、虚拟北向速度输出。

作为本发明进一步改进，S3.2中具体的卡尔曼滤波的递推求解如下：

Xk|k-1＝Φkk-1Xk-1Pk|k-1＝Φkk-1Pk-1ΦTkk-1+Γ(k-1)QkΓT(k-1)K(k)＝Pkk-1HTkH(k)Pkk-1HTk+R(k)-1Xkk＝Xkk-1+Kk[Zk-H(k)Xkk-1]Pk|k＝[I-K(k)H(k)]P(k|k-1) (12)

其中，I表示单位矩阵。

技术原理：常规的基于卡尔曼滤波的SINS/DVL组合导航中，其DVL所测速度为载体对地速度，当DVL工作在水跟踪模式下时，DVL所测速度为载体相对于水层的速度，而水层速度为未知估计量。通常将未知的水层速度也作为待估计的值加入到滤波器的状态量中。对于卡尔曼滤波器，其状态变量的收敛精度以及收敛速度直接由状态转移矩阵和量测矩阵所表示的滤波器结构决定。水层速度的状态量和SINS系统速度误差状态量反映在微分方程中的系数相近，即二者可观测度较低，收敛速度和收敛精度较差。常用的增加状态量可观测度的方法有增加量测和改变系数，增加量测方法通常需要引入新的传感器，在水下的情况寻找新量测输入通常难度较大或代价高昂。本发明利用DVL和SINS的输出构建了一个虚拟量测，在没有引入新的传感器的情况下改善了对应卡尔曼滤波器的可观测度，因此提高了SINS系统速度误差量的收敛速度和收敛精度，因此提高了定位精度。

有益效果：本发明采用的基于虚拟速度量测的中层水域AUV测流DVL/SINS组合导航方法，具有SINS速度误差收敛速度快，定位精度高的优点，并且无需增加新的传感器，增加的计算量也不大，无需对AUV的机动与轨迹进行限制，非常适用于中浅层水域的AUV导航。

附图说明

图1为本发明基于虚拟速度量测的中层水域AUV测流DVL/SINS组合导航框架图；

图2为本发明基于虚拟速度量测的DVL/SINS组合导航东向速度误差和普通DVL/SINS组合导航东向速度误差对比图；

图3为本发明基于虚拟速度量测的DVL/SINS组合导航北向速度误差和普通DVL/SINS组合导航北向速度误差对比图。

具体实施方式

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

针对中层水域因为未知洋流导致的SINS/DVL组合导航精度下降问题，本发明利用IMU输出、DVL输出和SINS系统历史信息构建了一种虚拟量测，并将虚拟量测输入至组合导航的滤波器中，使得滤波器的可观测性矩阵列满秩，以获取更高精度的速度测量值以及其他导航参数。

如图1所示，本发明所述的一种基于虚拟速度量测的SINS/测流DVL组合导航方法，包括以下步骤：

步骤1.获取安装在水下AUV上的传感器实时数据，具体包括多普勒测速仪的速度量测数据以及惯性测量元件所输出的陀螺仪和加速度计的输出数据，并利用传感器数据执行姿态解算算法，获得SINS系统以及DVL系统的导航系下速度；

首先定义如下坐标系：

导航坐标系：选取当地地理坐标系作为导航坐标系，分别指向东、北、天，记导航坐标系为n。

载体坐标系：载体坐标系原点位于载体重心，三个轴分别指向载体的右、前、上，记载体坐标系为b。

惯性坐标系：恒定不变的坐标系，记其为i。

地球坐标系：原点位于地球中心，x、y轴位于地球赤道平面，且两轴互相垂直，z轴指向本初子午线，记其为e。

1.1：惯性测量元件(Inertial measurement unit,IMU)被安装在载体AUV上，包括三轴陀螺仪以及三轴加速度计，可以输出载体的角速度信息ωibb和比力信息fb。

当完成初始对准后，可获得载体初始时刻的姿态Cnb(0)、速度Vn0以及位置P(0)，然后可根据微分方程以及IMU输出的信息递推求解载体的位姿：

导航坐标系下导航解算的微分方程为：

Lλh＝VNRn+hVEsecLRe+hVU (4)

其中，q表示载体坐标系到导航坐标系的旋转四元数；ωnbb表示载体系b和导航系n之间的相对角速度在载体系b下的投影；ωien表示地球系e与惯性系i的相对角速度在导航系n下的投影；ωenn表示导航系n与地球系e之间的相对角速度在导航系n下的投影；Vn＝vEvNvUT是对地速度在导航系n下的投影；fb表示加速度计输出的比力信息；gn表示重力加速度在导航系n下的投影；L、λ和h分别表示纬度、经度和高度；R_n、R_e分别表示地球子午圈、卯酉圈曲率半径；

1.2：DVL传感器安装在载体的底部，通过向下发送超声波以完成对载体速度的测量。高精度的DVL传感器分别在载体的前后左右四个方向发送超声波束，可获得AUV在载体坐标系下的三维速度Vdvlb＝[vdvlrightvdvlfrontvdvlup]。然后，利用S1.2中SINS系统解算获得的AUV姿态，可获取到导航坐标系下的三维速度：

步骤2：利用步骤(1)中获得的IMU输出、SINS系统以及DVL系统的导航系下速度，构建一组AUV的虚拟对地速度量测；

其中，虚拟量测的构建公式如下：

Vvirtualnt＝Vsinsnt-1+ΔVnt (6)

式中，Vsinsnt-1＝vsinsEt-1vsinsNt-1为SINS系统在t-1时刻经过解算获得的东向速度和北向速度，Vvirtualnt＝vvirtualEtvvirtualNt为利用DVL和SINS系统构建的t时刻不含洋流的虚拟速度量测，ΔVnt＝ΔvEtΔvNt为从t-1时刻到t时刻构建的速度变化量，其计算方式如下：

ΔVnt＝Psins-1(t)+Pdvl-1(t)-1[Psins-1tI-φ×Cbn(t)·fbt+Pdvl-1t(Vdvlnt-Vdvlnt-1)] (7)

式中，fbt为加速度计在t时刻的比力输出值，Cbn(t)为SINS系统t时刻解算获得的姿态矩阵，φ为SINS系统计算导航系与实际的地理导航系之间的失准角，可通过滤波器的状态量提取，Pdvl-1t为DVL测速得误差方差阵，可根据DVL器件的测速精度设置，而Psins-1t为SINS系统的速度估计误差，可从滤波器中提取。

步骤3：将步骤(2)中构建的AUV虚拟对地速度量测与步骤(1)中获得SINS系统、DVL系统速度输出一同输入至卡尔曼滤波器中，进行信息融合，完成对AUV的定位与导航。

3.1：选取SINS系统姿态失准角、SINS系统导航系速度误差、SINS系统位置误差、陀螺常值误差和、速度计常值误差以及水流对地速度作为状态变量，选取DVL系统与SINS系统导航系速度误差、SINS系统与高度计高度测量误差以及SINS系统与虚拟速度量测导航系速度误差作为量测变量，获得离散化的kalman滤波状态方程和量测方程；

17维状态变量的选取如下：

离散化后的卡尔曼滤波方程为：

Xk+1＝FkXk+Γ(k)W(k)Zk+1＝Hk+1Xk+1+V(k+1) (9)

量测变量表示如下：

Z(k+1)＝vSINSE(k+1)-vDVLE(k+1)vSINSN(k+1)-vDVLN(k+1)hSINS(k+1)-hpressure(k+1)vSINSN(k+1)-vvirtualE(k+1)vSINSN(k+1)-vvirtualN(k+1) (10)

其中，Xk+1为k+1时刻的状态估计，Xk为k时刻的状态估计，Zk+1为k+1时刻的观测值，Fk为k时刻的状态转移矩阵，Γ(k)为系统过程噪声输入矩阵，Hk+1为k+1时刻的观测矩阵，W(k)是状态转移中的随机过程噪声，V(k+1)是随机量测噪声；

3.2：给定状态估计和估计误差方差的初始值X0和P0，根据k时刻的观测值Z(k)，实时递推计算得到k时刻的状态估计Xkk；

3.2中具体的卡尔曼滤波的递推求解如下：

Xk|k-1＝Φkk-1Xk-1Pk|k-1＝Φkk-1Pk-1ΦTkk-1+Γ(k-1)QkΓT(k-1)K(k)＝Pkk-1HTkH(k)Pkk-1HTk+R(k)-1Xkk＝Xkk-1+Kk[Zk-H(k)Xkk-1]Pk|k＝[I-K(k)H(k)]P(k|k-1) (11)

其中，I表示单位矩阵。

3.3：利用步骤3.2中得到的状态估计对SINS输出的导航参数进行修正。

步骤4：利用步骤3中获得的误差估计修正导航参数，并保存该时刻的SINS速度输出以及DVL速度输出，用于下一时刻虚拟速度量测的构建，然后循环步骤(1)～(4)。

本发明的有益效果通过MATLAB仿真得以验证

通过仿真软件MATLAB生成AUV水下轨迹与传感器数据，并向传感器数据添加仪表误差，同时向捷联惯性导航系统中添加初始误差，向DVL输出值中添加洋流值，具体数据如下：

表1仿真误差及洋流设置

在表1的条件下，使用本发明所使用的基于虚拟量测的测流DVL/SINS组合导航方案与基于普通卡尔曼滤波的DVL/SINS组合导航进行对比，仿真持续时间3250秒。图2和图3分别为本发明的基于虚拟速度量测的DVL/SINS组合导航和普通DVL/SINS组合导航东向速度误差和北向速度误差对比图，从图中可以明显看出，本发明所使用的基于虚拟量测的的测流DVL/SINS组合导航方案与普通卡尔曼滤波相比，在速度误差的收敛速度和收敛精度上均有较大优势。本发明的基于虚拟速度量测的DVL/SINS组合导航具有更高的定位精度。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (4)

1.基于虚拟速度量测的中层水域SINS/测流DVL组合导航方法，其特征在于，具体方法如下：

(1)获取安装在水下无人潜航器上的传感器实时数据，具体包括多普勒测速仪的速度量测数据以及惯性测量元件所输出的陀螺仪和加速度计的输出数据，并利用传感器数据执行姿态解算算法，获得SINS系统以及DVL系统的导航系下速度，具体步骤如下：

S1.1：选取当地地理坐标系作为导航坐标系，分别指向东、北、天，记导航坐标系为n；载体坐标系原点位于载体重心，三个轴分别指向载体的右、前、上，记载体坐标系为b；记惯性坐标系为i、地球坐标系为e，则导航坐标系下导航解算的微分方程如式(1)～(4)：

其中，q表示载体坐标系到导航坐标系的旋转四元数；ωnbb表示载体系b和导航系n之间的相对角速度在载体系b下的投影；ωien表示地球系e与惯性系i的相对角速度在导航系n下的投影；ωenn表示导航系n与地球系e之间的相对角速度在导航系n下的投影；Vn＝vEvNvUT是对地速度在导航系n下的投影；fb表示加速度计输出的比力信息；gn表示重力加速度在导航系n下的投影；L、λ和h分别表示纬度、经度和高度；R_n、R_e分别表示地球子午圈、卯酉圈曲率半径；

S1.2：已知初始时刻的姿态q(0)、速度v(0)和位置P(0)，通过对导航解算的微分方程(2)-(4)进行积分，实时递归计算得到当前时刻的姿态、速度和位置。

S1.3：DVL传感器安装在载体的底部，通过向下发送超声波以完成对载体速度的测量。高精度的DVL传感器分别在载体的前后左右四个方向发送超声波束，可获得AUV在载体坐标系下的三维速度Vdvlb＝[vdvlrightvdvlfrontvdvlup]。然后，利用S1.2中SINS系统解算获得的AUV姿态，可获取到导航坐标系下的三维速度：

(2)利用步骤(1)中获得的IMU输出、SINS系统以及DVL系统的导航系下速度，构建一组AUV的虚拟对地速度量测；

步骤(2)中t时刻虚拟速度量测的构建方程具体为：

式中，

代表构建的t时刻不含洋流干扰的由虚拟东向速度量测和虚拟北向速度量测组成的虚拟速度量测向量，

为由SINS在t-1时刻经过捷联解算算法获得的东向速度和北向速度组成的向量，

为从t-1时刻到t时刻构建的速度变化量，其计算方式如下：

式中，

为DVL测量误差协方差矩阵，而

为SINS系统的速度估计误差协方差矩阵；

(3)将步骤(2)中构建的水下无人潜航器虚拟对地速度量测与步骤(1)中获得SINS系统、DVL系统速度输出一同输入至卡尔曼滤波器中，进行信息融合，完成对水下无人潜航器的定位与导航，具体步骤如下；

S3.1：选取SINS系统姿态失准角、SINS系统导航系速度误差、SINS系统位置误差、陀螺常值误差和、速度计常值误差以及水流对地速度作为17维状态变量，选取DVL系统与SINS系统导航系速度误差、SINS系统与高度计高度测量误差以及SINS系统与虚拟速度量测导航系速度误差作为5维量测变量，获得离散化的kalman滤波状态方程和量测方程；

S3.2：给定状态估计和估计误差方差的初始值X0和P0，根据k时刻的观测值Z(k)，实时递推计算得到k时刻的状态估计Xkk；

S3.3：利用步骤3.2中得到的状态估计对SINS输出的导航参数进行修正。

2.根据权利要求1所述的基于虚拟速度量测的水下无人潜航器测流DVL/SINS组合导航方法，其特征在于，

ΔV_est的计算方式如下：

式中，f^b(t)为加速度计在t时刻的比力输出值，

为SINS系统t时刻解算获得的姿态矩阵，φ为SINS系统计算导航系与实际的地理导航系之间的失准角，b_a为SINS中加速度计的常值零偏估计值。

3.根据权利要求1所述的基于虚拟速度量测的中层水域SINS/测流DVL组合导航方法，其特征在于，

S3.1中状态变量的选取如下：

S3.1中离散化的kalman滤波状态方程为：

Xk+1＝FkXk+Γ(k)W(k)Zk+1＝Hk+1Xk+1+V(k+1) (10)

其中，Xk+1为k+1时刻的状态估计，Xk为k时刻的状态估计，Zk+1为k+1时刻的观测值，Fk为k时刻的状态转移矩阵，Γ(k)为系统过程噪声输入矩阵，Hk+1为k+1时刻的观测矩阵，W(k)是状态转移中的随机过程噪声，V(k+1)是随机量测噪声；

S3.1中量测变量表示如下：

其中，

分别代表由SINS东向、北向速度输出，

分别代表DVL东向、北向速度输出，

分别代表步骤(2)中构建的虚拟东向速度、虚拟北向速度输出。

4.根据权利要求1所述的基于虚拟速度量测的中层水域SINS/测流DVL组合导航方法，其特征在于，

S3.2中具体的卡尔曼滤波的递推求解如下：

Xk|k-1＝Φkk-1Xk-1Pk|k-1＝Φkk-1Pk-1ΦTkk-1+Γ(k-1)QkΓT(k-1)K(k)＝Pkk-1HTkH(k)Pkk-1HTk+R(k)-1Xkk＝Xkk-1+Kk[Zk-H(k)Xkk-1]Pk|k＝[I-K(k)H(k)]P(k|k-1) (12)

其中，I表示单位矩阵。

Images