CN103776453B - 一种多模型水下航行器组合导航滤波方法 - Google Patents

Abstract

本发明公开了一种多模型水下航行器组合导航滤波方法，本发明首先根据水下航行器组合导航系统建立SINS/DVL/TAN/MCP组合导航系统的状态方程、观测方程及噪声模型；并根据系统方程与噪声模型确定模型集；从组合导航系统中选择特征变量，建立贝叶斯网络。根据多模型滤波算法结构，采用贝叶斯网络参数修正多模型估计中的模型切换概率，采用加权和方式计算所有滤波器的估计融合。由导航计算机根据组合导航系统的滤波模型及算法流程，完成组合导航的数据处理及解算工作。本发明能够提高复杂环境下组合导航系统的滤波精度，增强水下航行器自主导航定位性能。

Description

一种多模型水下航行器组合导航滤波方法

技术领域

本发明涉及利用一种多模型水下航行器组合导航滤波方法，实现对水下航行器的自主导航，属于多传感器信息融合技术领域。该专利内容同样适用于惯性组合导航、目标跟踪与识别、图像处理、模式识别等其他多传感器信息融合及多源数据处理应用领域。

背景技术

水下航行器由于水下环境复杂、工作时间长，可靠性和定位精度要求高，单一的导航方法很难满足要求，一般采用两种或两种以上非相似导航方法的组合，在以往的组合导航滤波器设计中，通常假设其中的各项参数保持不变，但由于存在洋流干扰、海水温度和盐度等环境变化以及载体机动等情况，系统的模型会随时间发生变化。这导致使用传统的Kalman、EKF滤波估计精度会下降甚至出现发散，难以实现系统高精度导航跟踪定位功能。基于混合系统的多模型估计是一种强有力的自适应估计方法，尤其适用于结构或参数易发生变化的系统，它的主要思想是在对象和扰动的数学模型不完全确定或者模型变化而不确定的情况下，设计多个模型来逼近系统复杂的时变或非线性过程，从而使在建模条件下分析得到的系统性能保持或接近最优。多模型估计中，当前计算模式随着模型的Markov链转移，由先验的Markov转移概率和量测信息共同确定模型转移概率，求得的模型转移概率与估计结果在下一时刻的估计过程中进行输入交互，进而对下一时刻的状态估计产生影响。实际应用表明，当系统模式发生变化时，由于滤波系统的惯性使得多模型估计算法对于实际系统模式切换的辨识有一定的滞后，模型转移概率通过先验知识确定，并没有利用当前时刻量测中的信息，由先验信息选定的滤波参数是模式切换与模式未切换情况下的折中。本发明是针对上述情况，解决传统多模型算法由于人为先验知识不足造成的系统模式切换滞后以及估计精度降低等问题，提高水下航行器组合导航算法估计性能。

发明内容

本发明提出了一种多模型水下航行器组合导航滤波方法，该方法的主要思路是在多模型滤波框架下，有机的把贝叶斯网络和多模型滤波算法结合起来，降低多模型算法中真实模式识别对算法中先验知识的依赖性，解决多模型算法中模型转换存在滞后、模型概率易发生跳变等问题，提高复杂环境下组合导航系统的滤波精度，增强水下航行器自主导航定位性能。

本发明提供了一种多模型水下航行器组合导航滤波方法，包括下列步骤：

(1)建立SINS/DVL/TAN/MCP组合导航系统的状态方程、观测方程及噪声模型；

(2)根据步骤(1)获取的系统方程和噪声模型，建立由n个模型构成的模型集；

(3)根据步骤(2)所获取的模型集，构建交互式多模型滤波结构，采用n个并行的模型Kalman滤波器进行滤波，获取模型子滤波器的状态估计、协方差阵以及残差和残差协方差阵；

(4)根据步骤(1)所述组合导航系统，选择特征变量建立贝叶斯网络，通过贝叶斯网络参数对模型切换概率进行修正；

(5)根据步骤(3)得到模型子滤波状态估计协方差阵P_i(k)和步骤(4)得到的模式切换概率μ_i(k)，进行概率加权融合得到系统状态估计和协方差阵P(k)。

(6)返回步骤(3)，从第(3)步重复以上步骤。

(7)完成组合导航的数据处理与解算工作。

优选的，步骤(3)所述模型子滤波器的状态估计、协方差阵以及残差和残差协方差阵获取方法如下：

3.1)模型转移概率预测：将k-1时刻的模型转移概率μ_j(k-1)和Markov转移概率π_ji进行交互，得到k时刻预测的模型转移概率下标i，j分别表示第i，j个模型。

3.2)重新初始化：根据k-1时刻模型子滤波器的状态估计k-1时刻模型子滤波器的协方差阵估计P_j(k-1)，预测的模型转移概率计算模型子滤波器的状态与协方差阵P_oi(k-1)。

${\hat{X}}_{\mathrm{oi}}(k-1)=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\hat{X}}_j(k-1){\widehat{\mathrm{\μ}}}_{j|i}(k-1)$

$P_{\mathrm{oi}}(k-1)=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\widehat{\mathrm{\μ}}}_{j|i}(k-1)\{P_j(k-1)+[{\hat{X}}_j(k-1)-{\hat{X}}_{\mathrm{oi}}(k-1)\left]{[{\hat{X}}_j(k-1)-{\hat{X}}_{\mathrm{oi}}(k-1)]}^T\right\}$

3.3)模型滤波：根据步骤32)获取的的重初始化状态和协方差阵P_oi(k-1)，在获得新的量测z_k之后，利用Kalman滤波器进行状态估计更新，得到模型子滤波状态估计协方差阵P_i(k)、残差估值ε_i(k)和残差方差S_i(k)。

优选的，步骤(4)所述的通过贝叶斯网络参数对模型切换概率进行修正方法如下：

4.1)MM模型概率更新：根据步骤33)中获取的残差估值ε_i(k)和残差方差S_i(k)，状态维数τ，采用假设检验方法，计算模型的似然函数Λ_i(k)，通过似然函数计算多模型概率 ${\mathrm{\μ}}_i^{\mathrm{MM}}\left(k\right).$

${\mathrm{\Λ}}_i\left(k\right)=N[{\mathrm{\ϵ}}_i\left(k\right);0,S_i\left(k\right)]={\left[{\left(2\mathrm{\π}\right)}^{\mathrm{\τ}}\right|S_i\left(k\right)\left|\right]}^{-1/2}\exp \{-\frac{1}{2}{\mathrm{\ϵ}}_i^T{S}_i^{-1}\left(k\right){\mathrm{\ϵ}}_j\}$

${\mathrm{\μ}}_i^{\mathrm{MM}}\left(k\right)=P\left\{m_i\left(k\right)\right|Z\left(k\right)\}=\frac{{\mathrm{\Λ}}_i\left(k\right)\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\π}}_{\mathrm{ji}}{\widehat{\mathrm{\μ}}}_j(k-1)}{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Λ}}_i\left(k\right)\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\π}}_{\mathrm{ji}}{\widehat{\mathrm{\μ}}}_j(k-1)}$

4.2)贝叶斯网络概率更新：通过贝叶斯网络特征变量X_i的父节点的条件概率Pa(X_i)确定贝叶斯网络概率

${\mathrm{\μ}}_i^{\mathrm{BN}}\left(k\right)={\mathrm{\Π}}_{i=1}^{n}P\left(X_i\right|\mathrm{Pa}\left(X_i\right))$

4.3)概率加权：设定权值系数η,根据步骤41)获取的多模型模型概率和步骤4.2)获取的贝叶斯网络概率计算改进多模型算法的模式切换概率μ_i(k)。

${\mathrm{\μ}}_i\left(k\right)={\mathrm{\η\μ}}_i^{\mathrm{MM}}\left(k\right)+(1-\mathrm{\η}){\mathrm{\μ}}_i^{\mathrm{BN}}\left(k\right)$

${\mathrm{\μ}}_i\left(k\right)=\frac{{\mathrm{\μ}}_i\left(k\right)}{\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\μ}}_i\left(k\right)}$

相对于现有技术，本发明相对于现有技术的优点为：

根据水下航行器组合导航系统建立SINS/DVL/TAN/MCP组合导航系统的状态方程、观测方程及噪声模型；根据噪声模型的特点，从组合导航系统中选择特征变量，建立贝叶斯网络，确定多模型滤波算法结构。根据上述算法结构，采用贝叶斯网络参数修正多模型估计中的模型切换概率，采用加权和计算所有滤波器的估计融合。由导航计算机根据组合导航系统的滤波模型及算法流程，完成组合导航的数据处理及解算工作。本发明能够提高复杂环境下组合导航系统的滤波精度，增强水下航行器组合导航的自适应能力。

对以上发明的有益效果说明如下：

在同等条件下，导航计算机根据传感器数据和系统模型，采用不同的滤波算法进行滤波。将本发明提出的贝叶斯网络多模型(BN-IMM)估计方法与普通交互式多模型(IMM)滤波方法进行组合导航系统数据处理并对结果进行对比。图5为普通交互式多模型算法模型切换概率曲线，图6为本发明的贝叶斯网络多模型算法模型切换概率曲线。由图5、图6可以看出，交互式多模型算法中模型概率切换出现了滞后现象，模型概率变化幅度很大，模型转换过程时间较长；贝叶斯网络多模型算法由于引入了特征变量直接对系统的模式进行辨识，降低了对前一时刻模型概率和观测量的依赖程度，模型转换速度快，模型概率变化幅度较小。图7给出了两种滤波方法在经度、纬度和高度方向上的数据比较曲线，普通交互式多模型算法的最大经度估计误差达到82.5m，最大纬度估计误差达到72.8m，最大高度估计误差达到81.6m；贝叶斯网络多模型算法的最大经度估计误差为65.8m，最大纬度估计误差为59.5m，最大高度估计误差为53.7m，可以看出，贝叶斯网络多模型算法性能优于普通的交互式多模型滤波算法。

附图说明

图1贝叶斯网络增强型多模型估计算法结构图(模型个数n=3)；

图2组合导航系统贝叶斯网络；

图3水下航行器组合导航系统；

图4水下航行器航迹仿真曲线；

图5多模型估计模型概率；

图6贝叶斯网络增强型多模型估计模型概率；

图7交互式多模型算法与贝叶斯网络多模型算法位置估计误差。

具体实施方式

下面结合说明书附图对本发明做详尽描述：

如图3所示，首先根据SINS/DVL/TAN/MCP组合导航系统的特点建立系统状态方程、观测方程及噪声模型；根据组合导航系统的特点建立如图2所示的组合导航系统贝叶斯网络，结合交互式多模型滤波算法得到如图1所示的贝叶斯网络增强型多模型估计算法结构，根据上述算法结构，采用贝叶斯网络参数修正多模型估计中的模型切换概率，采用加权和计算所有滤波器的估计融合。按照图4所示的水下航行器航迹仿真曲线，由导航计算机根据组合导航系统的滤波模型及算法流程，完成组合导航的数据处理及结算工作。

(1)建立SINS/DVL/TAN/MCP组合导航系统的状态方程、观测方程及噪声模型

选择状态变量X(t)＝[δV_EδV_NδV_Uφ_Eφ_Nφ_UδLδλδhε_bxε_byε_bz ]，δV_E、δV_N、δV_U为速度误差；φ_E、φ_N、φ_U为失准角；δL、δλ、δh为位置误差；ε_bx、ε_by、ε_bz为陀螺的常值漂移；为加速度计常值偏置。以东北天坐标系为导航坐标系(n)，右前上坐标系为载体坐标系(b)。列写15维的系统状态方程如式(1)所示：

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

W(t)为状态噪声，方差为Q(t)，F(t)为状态转移矩阵。

设V_DE、V_DN和V_DU是由载体坐标系转换到导航坐标系下的DVL的速度，V_SE、V_SN和V_SU为SINS输出的速度；和为MCP和SINS所测得的航向角；L_TN、λ_TN和h_TN为TAN所测量的经度、纬度和高度，L_SN、λ_SN和h_SN为SINS得到的位置信息。将捷联惯导输出与其它传感器测量之差作为集中滤波器的量测值，其中DVL输出的速度是在载体坐标系(b)系中得到的，需要变换到导航坐标系(n)中。观测方程如式(2)所示：

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

V(t)为观测噪声向量，方差为R(t)，H(t)为观测矩阵。

(2)根据步骤(1)获取的系统方程和噪声模型，建立模型集；

水下航行器航迹仿真曲线如图4所示，假设过程噪声Q和量测噪声R在不同机动状态时会发生变化，设匀速状态时过程噪声和量测噪声分别为Q₀和R₀，上浮/下潜时过程噪声和量测噪声分别为3Q₀和6R₀，转弯机动时过程噪声和量测噪声分别为6Q₀和12R₀，采用的模型集包含三个模型，它们的过程噪声和量测噪声也分别取值为Q₀，R₀，3Q₀、6R₀和6Q₀、12R₀。

(3)根据步骤(2)所获取的模型集，构建交互式多模型滤波结构，采用一组并行的各模型Kalman滤波器进行滤波，获取模型子滤波器的状态估计、协方差阵以及残差和残差协方差阵，具体实施方法如下：

31)模型转移概率预测：将k-1时刻的模型转移概率μ_j(k-1)和Markov转移概率π_ji进行交互，得到k时刻的预测模型转移概率下标i，j分别表示第i，j个模型。

32)重新初始化：根据k-1时刻模型子滤波器的状态估计k-1时刻模型子滤波器的协方差阵估计P_j(k-1)，预测的模型转移概率计算模型子滤波器的状态与协方差阵P_oi(k-1)。

${\hat{X}}_{\mathrm{oi}}(k-1)=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\hat{X}}_j(k-1){\widehat{\mathrm{\μ}}}_{j|i}(k-1)$

$P_{\mathrm{oi}}(k-1)=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\widehat{\mathrm{\μ}}}_{j|i}(k-1)\{P_j(k-1)+[{\hat{X}}_j(k-1)-{\hat{X}}_{\mathrm{oi}}(k-1)\left]{[{\hat{X}}_j(k-1)-{\hat{X}}_{\mathrm{oi}}(k-1)]}^T\right\}$

33)模型滤波：根据步骤32)获取的重初始化状态和协方差阵P_oi(k-1)，在获得新的量测z_k之后，利用Kalman滤波器进行状态估计更新，得到模型子滤波状态估计协方差阵P_i(k)、残差估值ε_i(k)和残差方差S_i(k)。

(4)根据步骤(1)所述组合导航系统，选择特征变量建立贝叶斯网络，通过贝叶斯网络参数对模型切换概率进行修正，具体实施方法如下：

41)多模型模型概率更新：根据步骤33)中获取的残差估值ε_i(k)和残差方差S_i(k)，状态维数τ，采用假设检验方法，计算模型的似然函数Λ_i(k)，通过似然函数计算多模型概率 ${\mathrm{\μ}}_i^{\mathrm{MM}}\left(k\right).$

${\mathrm{\Λ}}_i\left(k\right)=N[{\mathrm{\ϵ}}_i\left(k\right);0,S_i\left(k\right)]={\left[{\left(2\mathrm{\π}\right)}^{\mathrm{\τ}}\right|S_i\left(k\right)\left|\right]}^{-1/2}\exp \{-\frac{1}{2}{\mathrm{\ϵ}}_i^T{S}_i^{-1}\left(k\right){\mathrm{\ϵ}}_j\}$

${\mathrm{\μ}}_i^{\mathrm{MM}}\left(k\right)=P\left\{m_i\left(k\right)\right|Z\left(k\right)\}=\frac{{\mathrm{\Λ}}_i\left(k\right)\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\π}}_{\mathrm{ji}}{\widehat{\mathrm{\μ}}}_j(k-1)}{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Λ}}_i\left(k\right)\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\π}}_{\mathrm{ji}}{\widehat{\mathrm{\μ}}}_j(k-1)}$

42)贝叶斯网络概率更新：通过贝叶斯网络特征变量X_i的父节点的条件概率Pa(X_i)确定贝叶斯网络概率

${\mathrm{\μ}}_i^{\mathrm{BN}}\left(k\right)={\mathrm{\Π}}_{i=1}^{n}P\left(X_i\right|\mathrm{Pa}\left(X_i\right))$

43)概率加权：概率加权：设定权值系数η,根据步骤41)获取的多模型模型概率和步骤42)获取的贝叶斯网络概率计算改进多模型算法的模式切换概率μ_i(k)。

${\mathrm{\μ}}_i\left(k\right)={\mathrm{\η\μ}}_i^{\mathrm{MM}}\left(k\right)+(1-\mathrm{\η}){\mathrm{\μ}}_i^{\mathrm{BN}}\left(k\right)$

${\mathrm{\μ}}_i\left(k\right)=\frac{{\mathrm{\μ}}_i\left(k\right)}{\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\μ}}_i\left(k\right)}$

(5)根据步骤(3)得到模型子滤波状态估计协方差阵P_i(k)和步骤(4)得到的模式切换概率μ_i(k)，进行概率加权融合得到系统状态估计和协方差阵P(k)：

$\hat{X}\left(k\right)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\hat{X}}_i\left(k\right){\mathrm{\μ}}_i\left(k\right)$

$P\left(k\right)=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\μ}}_i\left(k\right)\{P_i\left(k\right)+[{\hat{X}}_i(k-1)-\hat{X}(k-1)\left]{[{\hat{X}}_i(k-1)-\hat{X}(k-1)]}^T\right\}$

(6)返回步骤(3)，从第(3)步重复以上步骤。

(7)完成组合导航的数据处理与解算工作。

在Matlab下进行了仿真，仿真参数设置如下：

陀螺随机常值漂移为0.03°/h，白噪声随机漂移为0.03°/h；

加速度计偏置误差为0.002g，白噪声随机漂移为0.002g；

DVL测速误差的均方根为0.05m/s；

捷联惯导初始水平方向失准角为6′，航向误差角为10′；

初始速度V₀=0m/s，初始速度误差为0.1m/s；

初始位置为北纬32°，东经118°，高度0m，初始位置误差为0m。

仿真时间3600s，在410s-830s,2870s-3290s加速和1130s-2570s转弯机动过程中，由于IMM算法中模型概率变化幅度很大，引起的位置估计结果误差也存在很大的波动。IMM算法的最大经度估计误差达到75.3m，最大纬度估计误差达到69.5m，最大高度估计误差达到75m；BN-IMM算法的最大经度估计误差为47.8m，最大纬度估计误差为43.5m，最大高度估计误差为44.7m，可以看出，基于贝叶斯网络的多模型估计算法性能优于IMM算法。

在同等条件下，导航计算机根据传感器数据和系统模型，采用不同的滤波算法进行滤波。将本发明提出的贝叶斯网络多模型估计方法与普通交互式多模型滤波方法进行组合导航系统数据处理并对结果进行对比。图5为普通交互式多模型算法模型切换概率曲线，图6为贝叶斯网络多模型算法模型切换概率曲线。由图5、图6可以看出，交互式多模型算法中模型概率切换出现了滞后现象，模型概率变化幅度很大，模型转换过程时间较长；贝叶斯网络多模型算法由于引入了特征变量直接对系统的模式进行辨识，降低了对前一时刻模型概率和观测量的依赖程度，模型转换速度快，模型概率变化幅度较小。图7给出了两种滤波方法在经度、纬度和高度方向上的数据比较曲线，普通交互式多模型算法的最大经度估计误差达到82.5m，最大纬度估计误差达到72.8m，最大高度估计误差达到81.6m；贝叶斯网络多模型算法的最大经度估计误差为65.8m，最大纬度估计误差为59.5m，最大高度估计误差为53.7m，可以看出，贝叶斯网络多模型算法性能优于普通的交互式多模型滤波算法。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (2)

1.一种多模型水下航行器组合导航滤波方法，包括下列步骤：

(1)建立SINS/DVL/TAN/MCP组合导航系统的状态方程、观测方程及噪声模型；

(2)根据步骤(1)获取的状态方程、观测方程和噪声模型，建立由n个模型构成的模型集；

(3)根据步骤(2)所获取的模型集，构建交互式多模型滤波结构，采用n个并行的模型Kalman滤波器进行滤波，获取模型子滤波器的状态估计、协方差阵以及残差和残差协方差阵，具体包括如下步骤：

3.1)模型转移概率预测：将k-1时刻的模型转移概率μ_j(k-1)和Markov转移概率π_ji进行交互，得到k时刻的预测模型转移概率下标i，j分别表示第i，j个模型；

3.2)重新初始化：根据k-1时刻模型子滤波器的状态估计k-1时刻模型子滤波器的协方差阵估计P_j(k-1)，预测的模型转移概率通过如下公式计算模型子滤波器的状态与协方差阵P_oi(k-1)：

${\hat{X}}_{oi}(k-1)=\underset{j=1}{\overset{n}{\Σ}}{\hat{X}}_j(k-1){\widehat{\mathrm{\μ}}}_{j|i}(k-1)$

$P_{oi}(k-1)=\underset{j=1}{\overset{n}{\Σ}}{\widehat{\mathrm{\μ}}}_{j|i}(k-1)\{P_j(k-1)+\[{\hat{X}}_j(k-1)-{\hat{X}}_{oi}(k-1)\]{\[{\hat{X}}_j(k-1)-{\hat{X}}_{oi}(k-1)\]}^T\};$

3.3)模型滤波：根据步骤3.2)获取的重初始化状态和协方差阵P_oi(k-1)，在获得新的量测z_k之后，利用Kalman滤波器进行状态估计更新，得到模型子滤波状态估计协方差阵P_i(k)、残差估值ε_i(k)和残差方差S_i(k)；

(4)根据步骤(1)所述组合导航系统，选择特征变量建立贝叶斯网络，通过贝叶斯网络参数对模型切换概率进行修正，具体包括如下步骤：

4.1)多模型模型概率更新：根据步骤3.3)中获取的残差估值ε_i(k)和残差方差S_i(k)，状态维数τ，采用假设检验方法，计算模型的似然函数Λ_i(k)，通过似然函数计算多模型概率

${\mathrm{\Λ}}_i\left(k\right)=N\[{\mathrm{\ϵ}}_i\left(k\right);0,S_i\left(k\right)\]={\[{\left(2\mathrm{\π}\right)}^{\mathrm{\τ}}\left|S_i\left(k\right)\right|\]}^{-1/2}\exp \{-\frac{1}{2}{\mathrm{\ϵ}}_i^T{S}_i^{-1}\left(k\right){\mathrm{\ϵ}}_j\}$

${\mathrm{\μ}}_i^{MM}\left(k\right)=P\left\{m_i\left(k\right)\right|Z\left(k\right)\}=\frac{{\mathrm{\Λ}}_i\left(k\right)\underset{j=1}{\overset{n}{\Σ}}{\mathrm{\π}}_{ji}{\widehat{\mathrm{\μ}}}_j(k-1)}{\underset{i=1}{\overset{n}{\Σ}}{\mathrm{\Λ}}_i\left(k\right)\underset{j=1}{\overset{n}{\Σ}}{\mathrm{\π}}_{ji}{\widehat{\mathrm{\μ}}}_j(k-1)};$

4.2)贝叶斯网络概率更新：通过贝叶斯网络特征变量X_i的父节点的条件概率Pa(X_i)确定贝叶斯网络概率

${\mathrm{\μ}}_i^{BN}\left(k\right)={\Π}_{i=1}^{n}P\left(X_i|Pa\left(X_i\right)\right);$

4.3)概率加权：设定权值系数η,根据步骤4.1)获取的多模型模型概率和步骤4.2)获取的贝叶斯网络概率计算改进多模型算法的模式切换概率μ_i(k)：

${\mathrm{\μ}}_i\left(k\right)={\mathrm{\η\μ}}_i^{MM}\left(k\right)+(1-\mathrm{\η}){\mathrm{\μ}}_i^{BN}\left(k\right)$

${\mathrm{\μ}}_i\left(k\right)=\frac{{\mathrm{\μ}}_i\left(k\right)}{\underset{i=1}{\overset{n}{\Σ}}{\mathrm{\μ}}_i\left(k\right)}.$

(5)根据步骤(3)得到模型子滤波状态估计协方差阵P_i(k)和步骤(4)得到的模式切换概率μ_i(k)，进行概率加权融合得到系统状态估计和协方差阵P(k)：

$\hat{X}\left(k\right)=\underset{i=1}{\overset{n}{\Σ}}{\hat{X}}_i\left(k\right){\mathrm{\μ}}_i\left(k\right)$

$P\left(k\right)=\underset{i=1}{\overset{n}{\Σ}}{\mathrm{\μ}}_i\left(k\right)\{P_i\left(k\right)+\[{\hat{X}}_i(k-1)-\hat{X}(k-1)\]{\[{\hat{X}}_i(k-1)-\hat{X}(k-1)\]}^T\};$

(6)返回步骤(3)，从第(3)步重复以上步骤；

(7)完成组合导航的数据处理与解算工作。

2.根据权利要求1所述的多模型水下航行器组合导航滤波方法，其特征在于，所述步骤4.2)中，贝叶斯网络特征变量包括电机的转速、推进器的力矩以及传感器输出。