CN109724599A - 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 - Google Patents

Abstract

本发明属于组合导航技术领域，具体涉及一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法。为了处理大机动引起的厚尾分布的系统噪声，本发明首先采用学生t分布对一步预测概率密度函数进行建模，并对SINS/DVL组合导航系统模型进行高斯分层；然后针对DVL输出存在野值的情况，采用贝塔‑伯努利分布对区分野值点的二进制变量进行建模，剔除量测野值；进而通过变分贝叶斯方法对状态向量x_k、辅助随机变量ξ_k、尺度矩阵Σ_k、Bernoulli变量λ_k和beta变量π_k的近似后验概率密度进行联合估计；最后将估计的SINS/DVL导航误差与惯导解算导航参数进行输出校正，完成组合导航。本发明能够处理系统噪声呈厚尾分布和量测噪声存在野值的SINS/DVL组合导航，导航精度高，鲁棒性强。

Description

一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法

技术领域

本发明属于组合导航技术领域，具体涉及一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法。

背景技术

捷联惯导系统通过陀螺仪和加速度计输出来确定姿态、速度和位置，以其具有隐蔽性、连续性和全球性，常作为海洋运载设备的主导航系统，但是存在速度误差和位置误差随时间累积的缺点。多普勒计程仪通过超声波打到海底产生多普勒效应原理来进行测速，但是不能提供姿态信息。SINS/DVL组合导航系统利用KF技术，将SINS和DVL的输出进行信息融合，以获得优于单一导航设备的性能。

在系统噪声和量测噪声都满足高斯分布，且对应的统计特性已知的条件下，根据建立的SINS/DVL组合导航模型，能够利用KF实现导航误差的最优估计。然而，在大机动情况下，如转弯、加速、颠簸等情况，陀螺仪输出误差必然会增大，从而导致惯性导航系统误差显著增加。此时，将带误差的惯导解算结果用于建模将导致系统模型不确定，使得系统噪声的统计特性发生改变，呈厚尾分布。同时，由于水下环境复杂，DVL受海湾、海沟、洋流的影响，在使用过程中容易产生野值，使得量测噪声存在野值。因此，亟需一种鲁棒的SINS/DVL组合导航算法来解决系统噪声呈厚尾分布和量测噪声存在野值的情况。

为了处理上述问题，研究者们进行了一系列自适应卡尔曼滤波算法(AKFs)的研究，例如Sage-Huge AKF(SHAKF)，多模AKF(MMAKF)，基于学生t分布的鲁棒KF(RTKF)和基于量测野值检查的鲁棒KF(ODKF)等。其中SHAKF利用最大后验准则来估计系统噪声的统计特性，但是容易导致滤波发散。MMAKF同时运行不同的系统噪声的多个Kalman滤波算法，但是它的计算量极大。RTKF通过将系统模型建立为学生t分布来估计一步预测协方差阵和量测噪声协方差阵，但是使用了量测中可剔除的野值点进行量测量测更新，会降低估计的准确度。ODKF针对量测设立了一个野值检查指标量，但是该算法只适用于系统模型为高斯分布的情况，当系统模型存在不确定时，容易产生野值点的误判。针对SINS/DVL组合导航系统大机动情况下的系统噪声呈厚尾分布和量测噪声存在野值的情况，本发明提出了一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法，以提高SINS/DVL在大机动情况下的组合导航精度。

发明内容

本发明的目的是提供一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法。

一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法，该方法包括以下步骤：

步骤1：建立SINS/DVL组合导航的状态方程和量测方程；

步骤2：建立学生t分布的一步预测概率密度p(x_k|z_1:k-1)和带野值检测的似然概率密度p(z_k|x_k)模型；

步骤3：利用变分贝叶斯方法近似组合导航模型的后验概率密度，获得修正的一步预测协方差矩阵、修正的量测噪声协方差矩阵和量测野值检查结果；

步骤4：利用野值的检测结果，执行相应的量测更新过程；

步骤5：将估计的导航误差对惯导解算的输出进行输出校正，完成SINS/DVL组合导航。

设SINS和DVL的安装误差已经补偿，利于SINS的误差方程来建立步骤1所述的SINS/DVL组合导航的状态方程，SINS的误差方程包括姿态误差方程、速度误差方程以及位置误差方程，其中姿态误差方程为：

其中i表示惯性系；b表示载体坐标系；n表示由东北天地理坐标系构成的导航坐标系；e表示地球坐标系；为平台失准角，为纵摇角误差，为横摇角误差，为航向角误差，表示导航系相对惯性系的旋转角速度在导航系下的投影；为对应的误差项；为地球自转角速度，为地球自转角速度对应的误差项；为导航系相对地球系的旋转角速度，为导航系相对地球系的旋转角速度的误差项；为载体系到导航系的姿态转换矩阵；陀螺漂移误差由随机常值漂移ε^b和陀螺仪随机噪声两部分组成；速度误差方程为：

其中表示载体的对地速度在n系下的投影；表示载体的速度误差；f^b表示加速度计输出的比力，δf^b表示加速度计输出的比力的测量误差，δf^b由加速度计零偏和加速度计随机噪声组成；位置误差方程为：

其中，L表示纬度，λ表示经度，h表示高度；δL、δλ和δh为对应的纬度误差、经度误差和高度误差，δL、δλ和δh为对应的纬度误差、经度误差和高度误差，δL为对应的纬度误差，δλ对应的经度误差，δh为对应的高度误差，R_N表示卯酉圈的曲率半径，R_M表示子午圈的曲率半径；所以状态方程如下：

其中，为状态向量；为系统噪声；A为状态转移矩阵；B为系统噪声驱动阵，(·)_m,n表示对矩阵(·)取前m行n列；本发明采用DVL提供的外速度信息作为观测量，步骤1所述的量测方程如下：

其中，Z是量测量，η为量测噪声，和为计程仪提供的速度，H＝[0_2×3I_2×20_2×7]为量测矩阵；将建立的系统方程和量测方程进行离散化，可得离散化后的状态空间模型为：

x_k＝F_kx_k-1+G_kw_k-1

z_k＝H_kx_k+v_k

其中t_k＝kT_s；k为离散时间指数；T_s为采样时间周期；离散后的状态转移矩阵为F_k＝I₇+T_sA(t_k)；系统噪声矩阵为G_k＝T_sB(t_k)；量测矩阵为H_k＝H；在没有野值的情况下，量测噪声v_k满足高斯分布，且均值为零，标称协方差为R_k，w_k厚尾分布的系统噪声，且均值为0，标称的协方差为Q_k。

步骤2所述的学生t分布的一步预测概率密度p(x_k|z_1:k-1)表示为下式：

其中，St(·；μ,Σ,ν)表示均值为μ，尺度矩阵为Σ，自由度为ν的学生t分布的概率密度函数，由于学生t分布可以被看作无数个混合高斯分布，则可以将上式表示为：

其中，ξ_k是满足伽玛分布的辅助随机变量，G(·；α,β)表示受形式参数α和比率参数β影响的伽玛分布的概率密度函数，根据上式可以将一步预测概率密度函数高斯分层为：

步骤3所述的利用变分贝叶斯方法近似组合导航模型的后验概率密度p(Θ_k|z_1:k)表示为下式：

Bn(λ_k；π_k)Be(π_k；e₀,f₀)p(z_1:k-1)

其中，IW(·；u_k,U_k)表示自由度参数为u_k，逆尺度矩阵为U_k的IW分布概率密度函数，且Σ_k是n维的正定矩阵，Bn(·,π)表示受参数π控制的Bernoulli分布的概率密度函数；π_k是由参数e₀和f₀决定的贝塔(beta)分布，B(·；e₀,f₀)表示由参数e₀和f₀决定的beta分布的概率密度函数；其中B(·,·)为beta函数；对上式两边取自然对数可得：

根据变分贝叶斯方法，这些近似的后验概率密度函数满足方程表示为下式：

其中表示Θ_k中除了φ以外的其他所有元素，E[·]表示期望运算；

当φ＝λ_k时，得下式：

其中：λ_k是Bernoulli参数；

当φ＝π_k时，近似后验密度的对数形式为：

根据上式，则q⁽ⁱ⁺¹⁾(π_k)仍然满足beta分布：

其中：

当φ＝ξ_k时，得近似后验密度的对数形式为：

其中：根据上式，则q⁽ⁱ⁺¹⁾(ξ_k)满足Gamma分布：

其中：

当φ＝Σ_k时，得其近似后验密度的对数形式为：

则q⁽ⁱ⁺¹⁾(Σ_k)仍然满足IW分布：

其中：

当φ＝x_k时，得其近似后验密度的对数形式为：

其中：

则q⁽ⁱ⁺¹⁾(x_k)可以近似为高斯分布：

其中均值矢量和误差协方差矩阵可以用卡尔曼滤波的来更新：

根据各个后验分布特性，可以获得以下期望：

其中ψ(·)是digamma函数，在更新完之后，计算期望和

步骤4所述的利用野值的检测结果，执行相应的量测更新过程包括：当趋于1时，则按完整的步骤3进行更新；

当趋于0时，即将量测作为无用野值点进行处理，此时忽略量测量，将q⁽ⁱ⁺¹⁾(x_k)进行时间更新：

本发明的有益效果在于：

(1)通过学生t分布建模一步预测概率密度函数，获得了修正的一步预测协方差矩阵，减小了大机动情况下系统模型不确定性对状态更新的影响；

(2)通过beta-Bernoulli分布的二进制指示变量对每个量测进行检测，通过检测结果对量测中无用的野值点进行剔除，减小了量测野值对SINS/DVL组合导航状态估计的影响，提高了SINS/DVL组合导航的导航精度和鲁棒性。

附图说明

图1是抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法流程图。

图2是本发明的组合导航原理框图。

图3是舰船运行轨迹图。

图4(a)是舰船东向速度变化图。

图4(b)是舰船北向速度变化图。

图5(a)是舰船俯仰角姿态变化图。

图5(b)是舰船横滚角姿态变化图。

图5(c)是舰船航向角姿态变化图。

图6是DVL输出的速度误差的概率密度图。

图7(a)是SINS/DVL组合导航的纵摇姿态误差曲线图。

图7(b)是SINS/DVL组合导航的横摇姿态误差曲线图。

图7(c)是SINS/DVL组合导航的航向姿态误差曲线图。

图8(a)是SINS/DVL组合导航的东向速度误差曲线图。

图8(b)是SINS/DVL组合导航的北向速度误差曲线图。

图9(a)是SINS/DVL组合导航的纬度位置误差曲线图。

图9(b)是SINS/DVL组合导航的经度位置误差曲线图。

图10(a)是本发明的东向速度量测野值检测结果。

图10(b)是本发明的北向速度量测野值检测结果。

具体实施方式

下面结合附图与具体实施例对本发明作进一步详细描述。

表1本发明鲁棒卡尔曼滤波算法执行步骤表

(1)首先完成捷联惯导系统的初始对准，然后在舰船航行的情况下，采集加速度计、陀螺仪和多普勒计程仪的输出数据，且在航行过程中进行多次转弯运动。同时通过高精度组合导航系统采集同步的速度、位置和姿态参考信息。

(2)对器件的安装误差和杆臂误差进行补偿，获得误差补偿后的多普勒计程仪的速度输出和补偿后的参考信息。

(3)通过GPS获得初始位置信息，将采集的陀螺仪输出、加速度计输出和多普勒计程仪输出用于验证SINS/DVL组合导航方法。组合导航的原理框图如图2所示，通过陀螺仪输出和加速度计输出进行惯导解算，获得导航参数；然后通过建立的系统模型和量测模型，利用表1中的鲁棒卡尔曼滤波算法执行步骤对导航参数误差进行估计；最后将估计的误差进行输出校正，完成SINS/DVL组合导航。

本发明的目的是这样实现的：步骤如下：

步骤一：建立SINS/DVL组合导航的状态方程和量测方程；

步骤二：建立学生t分布的一步预测概率密度p(x_k|z_1:k-1)和带野值检测的似然概率密度p(z_k|x_k)模型；

步骤三：利用变分贝叶斯方法近似组合导航模型的后验概率密度，获得修正的一步预测协方差矩阵、修正的量测噪声协方差矩阵和量测野值检查结果；

步骤四：利用野值的检测结果，执行相应的量测更新过程；

步骤五：将估计的导航误差对惯导解算的输出进行输出校正，完成SINS/DVL组合导航。

本发明还包括这样一些结构特征：

1.步骤一具体为：

设SINS和DVL的安装误差已经补偿，利于SINS的误差方程来建立SINS/DVL组合导航的状态方程，其姿态误差方程为：

其中i表示惯性系；b表示载体坐标系；n表示由东北天地理坐标系构成的导航坐标系；e表示地球坐标系。为平台失准角，分别为纵摇角误差、横摇角误差和航向角误差。表示导航系相对惯性系的旋转角速度在导航系下的投影；为对应的误差项；和为地球自转角速度及其对应误差项；和为导航系相对地球系的旋转角速度及其误差项；为载体系到导航系的姿态转换矩阵；陀螺漂移误差由随机常值漂移ε^b和陀螺仪随机噪声两部分组成。

速度误差方程为：

其中表示载体的对地速度在n系下的投影；表示载体的速度误差；f^b和δf^b分别表示加速度计输出的比力及其测量误差，δf^b由加速度计零偏和加速度计随机噪声组成。

位置误差方程为：

L、λ和h分别为表示纬度、经度和高度；δL、δλ和δh为对应的纬度误差、经度误差和高度误差。R_N和R_M分别表示卯酉圈和子午圈的曲率半径。

本发明针对水面航行的运载体建模，忽略天向通道的影响，则可根据以上三个方程建立捷联惯导系统的状态方程如下：

其中为状态向量；为系统噪声；A为状态转移矩阵；B为系统噪声驱动阵，可以通过式(1-5)进行求取。(·)_m,n表示对矩阵(·)取前m行n列。

本发明采用DVL提供的外速度信息作为观测量，建立量测方程如下：

其中Z是量测量，η为量测噪声，和为计程仪提供的速度，H＝[0_2×3I_2×20_2×7]为量测矩阵。

将建立的系统方程和量测方程进行离散化，可得离散化后的状态空间模型为：

x_k＝F_kx_k-1+G_kw_k-1

z_k＝H_kx_k+v_k

其中t_k＝kT_s；k为离散时间指数；T_s为采样时间周期；离散后的状态转移矩阵为F_k＝I₇+T_sA(t_k)；系统噪声矩阵为G_k＝T_sB(t_k)；量测矩阵为H_k＝H。在没有野值的情况下，量测噪声v_k满足高斯分布，且均值为零，标称协方差为R_k。w_k厚尾分布的系统噪声，且均值为0，标称的协方差为Q_k。

2.步骤二具体为：

为了处理量测噪声中的野值，引入一个贝塔-伯努利(Beta-Bernoulli)分布的二进制变量λ_k来表征量测是否为野值点。当λ_k＝1则量测z_k为标称的量测值，若λ_k＝0则量测z_k为野值点。则似然概率密度函数可以表示为：

这里可以利用beta-Bernoulli分层先验来表示，λ_k是受π_k控制的伯努利(Bernoulli)变量：

Bn(·,π)表示受参数π控制的Bernoulli分布的概率密度函数；π_k是由参数e₀和f₀决定的贝塔(beta)分布：

B(·；e₀,f₀)表示由参数e₀和f₀决定的beta分布的概率密度函数；其中B(·,·)为beta函数。

在大机动情况下，系统噪声呈厚尾分布，则可以用基于学生t分布的概率密度函数来表示一步预测概率密度函数p(x_k|z_1:k-1)：

St(·；μ,Σ,ν)表示均值为μ，尺度矩阵为Σ，自由度为ν的学生t分布的概率密度函数。由于学生t分布可以被看作无数个混合高斯分布，则可以将上式表示为：

其中ξ_k是满足伽玛(Garmma)分布的辅助随机变量，G(·；α,β)表示受形式参数α和比率参数β影响的Gamma分布的概率密度函数。根据上式可以将一步预测概率密度函数高斯分层为：

由于标称的协方差矩阵P_k|k-1受不准确的系统噪声协方差阵Q_k-1的影响，不适合选作尺度矩阵Σ_k，因此需要通过变分贝叶斯方法自适应估计出尺度矩阵Σ_k。根据文献，可以将尺度矩阵建模为逆威沙特(Inverse Wishart，IW)分布：

p(Σ_k)＝IW(Σ_k；u_k,U_k)

IW(·；u_k,U_k)表示自由度参数为u_k，逆尺度矩阵为U_k的IW分布概率密度函数，且Σ_k是n维的正定矩阵。为了获得Σ_k的先验信息，将p(Σ_k)的期望设为标称的P_k|k-1：

令：

u_k＝n+τ+1

其中τ≥0为调整参数，则有：

U_k＝τP_k|k-1

3.步骤三具体为：

为了对状态向量进行估计，需要计算联合后验概率密度函数p(Θ_k|z_1:k)。对于高斯分层的状态空间模型，其后验概率密度函数是没有解析解的。因此，可以利用变分贝叶斯方法将p(Θ_k|z_1:k)近似为：

p(Θ_k|z_1:k)≈q(x_k)q(ξ_k)q(Σ_k)q(λ_k)q(π_k)

其中q(φ)为近似的φ的后验概率密度函数，φ为Θ_k中的元素。根据变分贝叶斯方法，这些近似的后验概率密度函数满足如下方程：

其中表示Θ_k中除了φ以外的其他所有元素，E[·]表示期望运算。由于变分参数之间不可避免地存在耦合关系，所以需要采用定点迭代的方法来获得上式的近似解。

利用高斯分层的状态空间模型条件独立的性质，将联合概率密度函数p(Θ_k,z_1:k)表示为：

p(Θ_k,z_1:k)＝p(z_k|x_k)p(x_k|ξ_k,z_1:k-1)p(ξ_k)p(Σ_k)p(λ_k|π_k)p(π_k|e₀,f)p(z_1:k-1)

通过步骤二中建立的概率密度模型，将上式展开可得：

Bn(λ_k；π_k)Be(π_k；e₀,f₀)p(z_1:k-1)

对上式两边取自然对数可得：

当φ＝λ_k时，将上式代入logq(φ)的近似概率密度条件可得：

其中：

由于λ_k是Bernoulli参数，则满足：

其中A是一个标准化常数，使得概率之和为1，即p⁽ⁱ⁺¹⁾(λ_k＝1)+p⁽ⁱ⁺¹⁾(λ_k＝0)＝1。当φ＝π_k时，同理可得其近似后验密度的对数形式为：

根据上式，则q⁽ⁱ⁺¹⁾(π_k)仍然满足beta分布：

其中：

当φ＝ξ_k时，同理可得其近似后验密度的对数形式为：

其中：

根据上式，则q⁽ⁱ⁺¹⁾(ξ_k)满足Gamma分布：

其中：

当φ＝Σ_k时，同理可得其近似后验密度的对数形式为：

则q⁽ⁱ⁺¹⁾(Σ_k)仍然满足IW分布：

其中：

当φ＝x_k时，同理可得其近似后验密度的对数形式为：

其中：

则q⁽ⁱ⁺¹⁾(x_k)可以近似为高斯分布：

其中均值矢量和误差协方差矩阵可以用卡尔曼滤波的来更新：

根据各个后验分布特性，可以获得以下期望：

其中ψ(·)是digamma函数。在更新完式(50)之后，可以计算期望和

4.步骤四具体为：

当趋于1时，则按完整的步骤三进行更新；

当趋于0时，即将量测作为无用野值点进行处理，此时忽略量测量，将q⁽ⁱ⁺¹⁾(x_k)进行简单的时间更新：

根据步骤三和步骤四，可得本发明的鲁棒KF的具体执行步骤如表1所示。

具体实施步骤如下：

步骤一：选取状态量建立状态方程和量测方程如下：

对状态方程和量测方程进行离散化处理，可得离散后的模型为：

x_k＝F_kx_k-1+G_kw_k-1

z_k＝H_kx_k+v_k

步骤二：根据表1中的鲁棒卡尔曼滤波执行步骤和步骤一中的状态空间模型，对步骤一中的状态量进行估计；首先进行一步预测时间更新：

执行量测更新：在对迭代初值进行初始化后，分别进行N此近似后验概率密度q⁽ⁱ⁺¹⁾(λ_k)、q⁽ⁱ⁺¹⁾(π_k)、q⁽ⁱ⁺¹⁾(ξ_k)、q⁽ⁱ⁺¹⁾(Σ_k)、q⁽ⁱ⁺¹⁾(x_k)的更新；

通过λ_k判断量测是否为野值点：判断若λ_k≤1×10^-10，则量测为野值，此时不进行量测更新，且终止此次定点迭代过程，则：

i＝N

在定点迭代结束后，将第N次的迭代结果作为量测更新的结果：

步骤三：如图10所示，将步骤二中估计出的导航误差与惯导解算输出的导航参数进行输出校正，从而输出SINS/DVL组合导航的速度、位置和姿态信息。至此完成SINS/DVL组合导航计算。

为了验证本发明公开的抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法在大机动条件下系统噪声呈厚尾分布和量测噪声存在野值的情况下的导航性能，进行了一次海试实验验证。实验条件和实验结果如下：

(1)实验条件

实验设备包括光纤捷联惯导系统(SINS)、多普勒计程仪、GPS接收机、光学惯性导航系统(PHINS)。其中光纤捷联惯导系统由陀螺仪(随机常值漂移0.01°/h，噪声为)和加速度计(零偏10^-4g，噪声)组成，其输出频率均为100Hz。计程仪的精度为0.2m/s。将PHINS/GPS组合导航系统输出的姿态、速度和位置作为参考，姿态均方根误差为0.01°，速度误差为0.1m/s，位置误差为5-15m。实验时长为4000s，实验过程中的运行轨迹如图3所示，速度变化曲线如图4所示，姿态变化曲线如图5所示。从图3-图4可以看出，舰船在4000s内进行了转弯、加速等较大的机动状态。图6为DVL速度误差的概率密度图，可以看出DVL输出存在野值点。因此，该实验室数据可以用来验证本发明提出的组合导航方法性能。

将本发明与现有的KF、RTKF、ODKF进行SINS/DVL组合导航性能对比。设滤波估计的状态初值为x_0|0＝0_12×1，误差协方差初值为P_0|0＝diag([0_.5°；0_.5°；0_.5°；0_.1m/s；0_.1m/s；1°/h；1°/h；0_.01°/h；0_.01°/h；0_.01°/h；10^-4g；10^-4g])²，标称的系统噪声协方差阵为Q_k＝diag([2_.5×10^-3g；2_.5×10^-3g；0_.01°/h；0_.01°/h；0_.01°/h])²，量测噪声协方差阵为R_k＝diag([0.1m/s 0.1m/s])²。自由度参数ν＝5和转换参数τ＝5，迭代次数N＝3，Beta-Bernoulli参数设为e₀＝0.9和f₀＝0.1。

(2)实验结果

表2 SINS/DVL组合导航误差统计结果表

实验结果如图7-图9所示，其均方根误差如表2所示。图10为本发明的野值检查结果。从图7-表2中看出，所有滤波算法构成的组合导航方法所得的姿态误差大小基本一致，但是本发明的鲁棒KF的速度计算误差和位置计算误差均小于其它三种方法的速度计算误差和位置计算误差。图10可以看出本发明能够有效检测出量测中的野值点。实验结果表明，本发明在处理大机动情况下系统噪声呈厚尾分布和量测噪声存在野值的SINS/DVL组合导航中具有更高的导航精度和更好的鲁棒性，本发明更适用于船用的SINS/DVL组合导航。

综上所述，本发明公开了一种抗野值的鲁棒卡尔曼滤波的SINS/DVL组合导航方法，能够处理大机动引起的系统噪声呈厚尾分布和DVL存在野值的SINS/DVL组合导航，具有导航精度高和鲁棒性强的特点。

Claims (5)

1.一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法，其特征在于，该方法包括以下步骤：

步骤1：建立SINS/DVL组合导航的状态方程和量测方程；

步骤2：建立学生t分布的一步预测概率密度p(x_k|z_1:k-1)和带野值检测的似然概率密度p(z_k|x_k)模型；

步骤3：利用变分贝叶斯方法组合导航模型的后验概率密度，获得修正的一步预测协方差矩阵、修正的量测噪声协方差矩阵和量测野值检查结果；

步骤4：利用野值的检测结果，执行相应的量测更新过程；

步骤5：将估计的导航误差对惯导解算的输出进行输出校正，完成SINS/DVL组合导航。

2.根据权利要求1所述的一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法，其特征在于：设SINS和DVL的安装误差已经补偿，利于SINS的误差方程来建立步骤1所述的SINS/DVL组合导航的状态方程，SINS的误差方程包括姿态误差方程、速度误差方程以及位置误差方程，其中姿态误差方程为：

其中i表示惯性系；b表示载体坐标系；n表示由东北天地理坐标系构成的导航坐标系；e表示地球坐标系；为平台失准角，为纵摇角误差，为横摇角误差，为航向角误差，表示导航系相对惯性系的旋转角速度在导航系下的投影；为对应的误差项；为地球自转角速度，为地球自转角速度对应的误差项；为导航系相对地球系的旋转角速度，为导航系相对地球系的旋转角速度的误差项；为载体系到导航系的姿态转换矩阵；陀螺漂移误差由随机常值漂移ε^b和陀螺仪随机噪声两部分组成；速度误差方程为：

其中表示载体的对地速度在n系下的投影；表示载体的速度误差；f^b表示加速度计输出的比力，δf^b表示加速度计输出的比力的测量误差，δf^b由加速度计零偏▽^b和加速度计随机噪声组成；位置误差方程为：

其中，L表示纬度，λ表示经度，h表示高度；δL为对应的纬度误差，δλ对应的经度误差，δh为对应的高度误差，R_N表示卯酉圈的曲率半径，R_M表示子午圈的曲率半径；所以状态方程表示为下式：

其中，为状态向量；为系统噪声；A为状态转移矩阵；B为系统噪声驱动阵，(·)_m,n表示对矩阵(·)取前m行n列；采用DVL提供的外速度信息作为观测量，步骤1所述的量测方程表示为下式：

其中，Z是量测量，η为量测噪声，和为计程仪提供的速度，H＝[0_2×3 I_2×2 0_2×7]为量测矩阵；将建立的系统方程和量测方程进行离散化，得离散化后的状态空间模型为：

x_k＝F_kx_k-1+G_kw_k-1

z_k＝H_kx_k+v_k

其中t_k＝kT_s；k为离散时间指数；T_s为采样时间周期；离散后的状态转移矩阵为F_k＝I₇+T_sA(t_k)；系统噪声矩阵为G_k＝T_sB(t_k)；量测矩阵为H_k＝H；在没有野值的情况下，量测噪声v_k满足高斯分布，且均值为零，标称协方差为R_k，标称的协方差为Q_k。

3.根据权利要求2所述的一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法，其特征在于，步骤2所述的学生t分布的一步预测概率密度p(x_k|z_1:k-1)表示为下式：

其中，St(·；μ,Σ,ν)表示均值为μ，尺度矩阵为Σ，自由度为ν的学生t分布的概率密度函数，由于学生t分布被看作无数个混合高斯分布，则将上式表示为：

其中，ξ_k是满足伽玛分布的辅助随机变量，G(·；α,β)表示受形式参数α和比率参数β影响的伽玛分布的概率密度函数，根据上式将一步预测概率密度函数高斯分层为：

4.根据权利要求3所述的一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法，其特征在于，步骤3所述的利用变分贝叶斯方法组合导航模型的后验概率密度p(Θ_k|z_1:k)表示为下式：

Bn(λ_k；π_k)Be(π_k；e₀,f₀)p(z_1:k-1)

其中，IW(·；u_k,U_k)表示自由度参数为u_k，逆尺度矩阵为U_k的IW分布概率密度函数，且Σ_k是n维的正定矩阵，Bn(·,π)表示受参数π控制的Bernoulli分布的概率密度函数；π_k是由参数e₀和f₀决定的贝塔beta分布，B(·；e₀,f₀)表示由参数e₀和f₀决定的beta分布的概率密度函数；其中B(·,·)为beta函数；对上式两边取自然对数得：

根据变分贝叶斯方法，这些后验概率密度函数满足方程表示为下式：

其中表示Θ_k中除了φ以外的所有元素，E[·]表示期望运算；

当φ＝λ_k时，得下式：

其中：λ_k是Bernoulli参数；

当φ＝π_k时，后验密度的对数形式为：

根据上式，则q⁽ⁱ⁺¹⁾(π_k)仍然满足beta分布：

其中：

当φ＝ξ_k时，得后验密度的对数形式为：

其中：根据上式，则q⁽ⁱ⁺¹⁾(ξ_k)满足Gamma分布：

其中：

当φ＝Σ_k时，得其后验密度的对数形式为：

则q⁽ⁱ⁺¹⁾(Σ_k)仍然满足IW分布：

其中：

当φ＝x_k时，得其后验密度的对数形式为：

其中：

则q⁽ⁱ⁺¹⁾(x_k)为高斯分布：

其中均值矢量和误差协方差矩阵用卡尔曼滤波的来更新：

根据各个后验分布特性，获得以下期望：

其中ψ(·)是digamma函数，在更新完之后，计算期望和

5.根据权利要求4所述的一种抗野值的鲁棒卡尔曼滤波SINS/DVL组合导航方法，其特征在于，步骤4所述的利用野值的检测结果，执行相应的量测更新过程包括：

当趋于1时，则按完整的步骤3进行更新；

当趋于0时，即将量测作为无用野值点进行处理，此时忽略量测量，将q⁽ⁱ⁺¹⁾(x_k)进行时间更新：