CN112525218A - 一种ins/dvl组合导航系统鲁棒智能协同校准方法 - Google Patents

Abstract

本发明公开了一种INS/DVL组合导航系统鲁棒智能协同校准方法，步骤一：建立包括状态方程和量测方程在内的传统INS/DVL系统模型；步骤二：采用基于声测距的交替校正算法对海洋无人运动平台进行校正；步骤三：利用基于统计相似度度量的鲁棒卡尔曼滤波器滤除输出异常值。本发明涉及的基于水声测距的交替校正算法不需要参考站点或者母船的支持，不仅扩大了海洋无人运动平台的工作范围，而且进一步提高了其定位精度；本发明鲁棒滤波器计算量小于现有的鲁棒滤波器，在高采样频率的导航应用中更具优势，而且对于异常值造成的非高斯场景其鲁棒性强于现有的鲁棒滤波器，本发明定位精度优于传统的INS/DVL组合导航系统。

Description

一种INS/DVL组合导航系统鲁棒智能协同校准方法

技术领域

本发明涉及一种INS/DVL组合导航系统鲁棒智能协同校准方法，特别是一种基于声学测距的领航者INS/DVL组合导航系统鲁棒智能协同校准方法，属于导航技术领域。

背景技术

随着对海洋探索的深入，海洋无人运动平台在海洋开发、海洋调查、生物监测等领域发挥着重要的作用。考虑到导航设备的成本问题，海洋无人运动平台通常以集群方式协同导航，由于领航者与辅助无人运动平台所携带的导航设备精度不同，导航系统的定位精度以及辅助无人运动平台的定位精度取决于领航者的定位精度。为了提高系统整体的定位精度，必须提高领航者的定位精度。

由于全球定位系统在水下环境无法使用，海洋无人运动平台通常使用惯性导航设备。虽然惯性导航设备能够提供位置、速度、姿态信息，但存在陀螺仪和加速度计的随机误差随时间累积的问题，故实际应用中使用测量误差不随时间累积的多普勒计程仪与惯性导航设备组成的组合导航系统。然而，对多普勒计程仪的标度因数误差以及惯性导航系统和多普勒计程仪之间的安装角误差进行精确校正和补偿是很困难的。此外，INS/DVL组合导航系统不能利用基于多普勒计程仪测量的载体坐标系速度的卡尔曼滤波估计来消除导航坐标系的累计位置误差。因此，传统的INS/DVL组合导航系统的位置误差仍会随时间积累。为了进一步提高组合导航系统的定位精度，提出了基于声测距的INS/DVL组合导航系统，这涉及到水下导航定位技术领域。

这种基于水声测距的INS/DVL组合导航方法的优点在于通过创新协方差矩阵建立评价指标，以此来确定不同领航者的定位性能。在此基础上，利用水声距离测量和定位性能较好的领航者位置信息，可以对滤波效果较差的领航者的位置进行校正。

由于水下环境复杂多变以及不同声设备之间的相互干扰，多普勒计程仪的输出中可能存在异常值，这使得INS/DVL组合导航系统的量测噪声呈现非高斯特性。为了滤除异常值提高导航系统的定位精度，这就涉及了一个重要的技术领域——滤波技术。

在现有的滤波技术中，标准卡尔曼滤波器(KF)的性能随着异常值的增大急剧下降。基于Huber方法的卡尔曼滤波器(HKF)通过最小化l₁和l₂的范数来减小包含数值大的异常值的测量值，以此提高滤波器在非高斯环境下的性能。之后，又提出了基于最大相关准则的卡尔曼滤波器(MCKF)，以一步预测误差和残差的核函数之和作为代价函数来处理状态和测量异常值。考虑到异常值污染噪声通常呈现重尾分布，许多鲁棒滤波器都是通过将重尾噪声视为学生t分布提出的。根据后验概率密度函数是否近似为高斯分布，可将鲁棒滤波器分为基于学生t的鲁棒滤波器(RSTKF)和学生t滤波器。对于包含异常值的系统来说，虽然鲁棒滤波器有比标准卡尔曼滤波器更高的估计精度，但它们仍存在一些缺点。HKF的影响函数并不能完全滤除异常值，这意味着在滤波过程中仍包含一些较大的异常值。对于MCKF，估计误差协方差矩阵的推导缺乏理论依据，并且在数值计算时，由于测量权重的过度减小，可能会出现非正定协方差矩阵。RSTKF不能推广到一般的非高斯分布情况，限制了其使用范围。对于复杂水下环境下的长时间自主导航，滤波器不能仅针对某一类非高斯噪声进行设计，必须保证滤波器的稳定性。现有的HKF、MCKF和RSTKF难以满足要求。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种INS/DVL组合导航系统鲁棒智能协同校准方法，为改进的INS/DVL组合导航系统提供一种基于统计相似度度量方法的鲁棒卡尔曼滤波器(SSMKF)，这种滤波器的优点在于其计算量小于现有的鲁棒滤波器。

为解决上述技术问题，本发明的一种INS/DVL组合导航系统鲁棒智能协同校准方法，包括以下步骤：

步骤一：建立包括状态方程和量测方程在内的传统INS/DVL系统模型，状态方程具体为：

其中，δp是由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量，

为n坐标系中的地速矢量，相应的误差矢量为

φ表示由俯仰、横滚、航向三组不对称角组成的不对称角矢量，ε^b为陀螺常值漂移矢量，

为加速度计常值漂移矢量，

为陀螺随机漂移矢量，

为加速度计随机漂移矢量，w是假设均值为0的白噪声，F是状态转移矩阵，G是噪声分配矩阵；

量测方程具体为：

其中，Z表示量测向量，

表示多普勒计程仪测得的速度矢量，

表示从b坐标系变换到n坐标系的方向余弦矩阵，V表示量测噪声；

步骤二：采用基于声测距的交替校正算法对海洋无人运动平台进行校正；

步骤三：利用基于统计相似度度量的鲁棒卡尔曼滤波器滤除输出异常值。

本发明还包括：

1.步骤二中采用基于声测距的交替校正算法对海洋无人运动平台进行校正具体为：

步骤1：建立评价相对误差大小的评价指标，利用滤波器的创新和创新协方差组成的似然函数构建描述卡尔曼滤波器滤波效果的评价指标，进而确定两个海洋无人运动平台位置误差的相对大小，评价指标为：

式中，

表示残差，

表示残差的协方差矩阵，

为卡尔曼滤波器获得的一步预测估计，

表示一步预测估计误差协方差矩阵，R_k是量测噪声协方差矩阵；

当

时，系统将利用水声测距信息和海洋无人运动平台2的位置信息来修正海洋无人运动平台1的位置，反之，则利用水声测距信息和海洋无人运动平台1的位置信息来修正海洋无人运动平台2的位置；

步骤2：根据评价指标的大小对海洋无人运动平台进行校正：

和

分别表示两个海洋无人运动平台从INS/DVL组合导航系统测得的位置信息，

和

分别表示滤波后得到的海洋无人运动平台1和海洋无人运动平台2的位置误差δp的估计误差协方差矩阵，d_k表示由水声调制解调器测得的两个海洋无人运动平台之间的相对距离，假设海洋无人运动平台2的位置误差小于海洋无人运动平台1，则系统可根据水声测距结果和海洋无人运动平台2的位置信息来修正海洋无人运动平台1的位置，可得如下所示的量测方程：

式中，θ_k表示假设均值为0的高斯噪声，Θ_k为其协方差矩阵；

对上式进行一阶泰勒级数展开，得到：

上式可写成如下形式：

用M_k代替

m_k代替

则得到：

当卡尔曼滤波器的估计结果反馈给惯性导航系统后，海洋无人运动平台1的位置可以看作一个均值为

协方差矩阵为

的随机量，可以得到

的先验概率密度函数：

式中，

表示均值为

协方差矩阵为

似然概率密度函数可近似如下：

可得到

的表达式：

当海洋无人运动平台1的位置误差小于海洋无人运动平台2时，则系统可根据水声测距结果和海洋无人运动平台1的位置信息来修正海洋无人运动平台2的位置，可得到

的表达式：

2.步骤三中利用基于统计相似度度量的鲁棒卡尔曼滤波器滤除输出异常值具体为：

构造基于统计相似度度量的鲁棒滤波器，异常值鲁棒滤波器由时间更新和量测更新两部分组成，其中时间更新如下：

式中，

和

表示在k时刻的后验状态估计及其估计误差协方差矩阵，Q_k为标称过程噪声协方差矩阵，

为一步预测状态，

为一步预测误差协方差矩阵；

在量测更新中，通过最大化

和

之间的统计相似度度量与

和

之间的统计相似度度量之和，得到状态的近似后验概率密度函数，其中

和

分别是一步预测误差协方差矩阵

和标称噪声协方差矩阵R_k+1的平方根：

将状态后验概率密度函数近似为高斯分布，将状态后验概率密度函数的最大化问题转化为对状态后验均值和协方差矩阵的最大化问题，然后，将由代价函数的Jensen不等式求得的下界最大化，建立新的代价函数，从而得到近似解：

B_k+1＝∫(Z_k+1-H_k+1X)(Z_k+1-H_k+1X)^T

式中，tr(·)表示求迹操作；

相似度函数f_x(t)取f_x(t)＝-0.5t，相似度函数f_z(t)取

ω是自由度参数，s是t的维数，根据最大点的定义可知：

其中，

进而得到：

其中，

其中，

并且将ξ_k+1用

代替，对

求解，得到

最优协方差矩阵是Hessian矩阵的负逆

其中，

是二阶项；

其中，

为保持

的负确定性，从Hessian矩阵

中减去二阶项

在这种情况下，

中的最大值

可以表示为：

本发明的有益效果：

基于声测距和鲁棒滤波器的INS/DVL组合导航系统能够有效地提高系统的定位精度，但传统的声测距需要参考站点或者母船的支持，传统的鲁棒滤波器存在不能完全滤除异常值、估计误差协方差矩阵的推导缺乏理论依据、不能推广到一般的非高斯分布场景等缺点，这都限制了其使用范围。为了解决上述的问题，本发明提供了一种基于声测距的交替校正方法、以及概率论与数理统计领域的基于统计相似度度量的鲁棒滤波器，除此之外，其状态后验概率密度函数近似为高斯分布，并采用不动点迭代算法计算状态后验概率密度函数的均值和协方差，证明了其数值稳定性和滤波稳定性。与现有技术相比，本发明的有益效果概述为以下两点：

一、本发明在鲁棒滤波器的基础上，使用了具有成熟数学理论体系作为支撑的统计相似度度量方法对其进行改善；

二、本发明涉及的基于水声测距的交替校正算法不需要参考站点或者母船的支持，不仅扩大了海洋无人运动平台的工作范围，而且进一步提高了其定位精度；

三、本发明涉及的统计相似度度量方法不仅使得基于此方法的鲁棒滤波器计算量小于现有的鲁棒滤波器，在高采样频率的导航应用中更具优势，而且对于异常值造成的非高斯场景其鲁棒性强于现有的鲁棒滤波器。更重要的是，改进的基于声测距的INS/DVL组合导航系统的定位精度优于传统的INS/DVL组合导航系统。

附图说明

图1为改进的领航者INS/DVL组合导航系统的流程图；

图2为海洋无人运动平台1上基于SSMKF的改进INS/DVL系统和传统的INS/DVL系统的定位误差；

图3为海洋无人运动平台2上基于SSMKF的改进INS/DVL系统和传统的INS/DVL系统的定位误差。

具体实施方式

下面结合附图对本发明具体实施方式做进一步说明。

本发明充分考虑基于声测距的INS/DVL组合导航系统的定位精度和鲁棒滤波器在含有输出异常值场景的优良性能，对其中包含的水声测距信息和鲁棒滤波器进行了研究。其中，基于声测距的INS/DVL组合导航系统需要参考站点或者母船的支持、现有的鲁棒滤波器无法在复杂的水下环境中完全滤除异常值，在现有的文献与专利中，还没有具体解决上述两个问题的方法。

针对基于声测距的INS/DVL组合导航系统需要参考站点或者母船的支持的问题，本发明在水声测距的基础上，使用了交替校正算法，使得海洋无人运动平台可以脱离参考站点或者母船进行工作，扩大了其工作范围。针对现有鲁棒滤波器无法在复杂水下环境完全滤除异常值的问题，本发明在鲁棒滤波器的基础上，引入了统计相似度度量算法，使得INS/DVL组合导航系统能够在复杂水文条件下完全滤除输出异常值，进一步提高了海洋无人运动平台的定位精度。

结合图1，本发明具体实施方式包括以下步骤：

步骤一：建立包括状态方程和量测方程在内的传统INS/DVL系统模型；

具体步骤如下：

第一步，建立系统的状态方程。为了推导出惯性导航系统的误差方程，我们定义了以下坐标系。将惯性导航系统的载体坐标系表示为b，惯性坐标系表示为i，地心地固坐标系表示为e，实际导航坐标系表示为n，计算得到的导航坐标系表示为n'。选取东北天坐标系作为导航坐标系n。通过推导可以得到惯性导航系统的误差模型：

式中，ε^b为陀螺常值漂移矢量，

为陀螺随机漂移矢量，

为加速度计常值漂移矢量，

为加速度计随机漂移矢量。φ表示由俯仰、横滚、航向三组不对称角组成的不对称角矢量。

表示从b坐标系变换到n坐标系的方向余弦矩阵。

表示在n坐标系中的n坐标系相对i坐标系的相对角速度矢量，相应的计算误差为

为n坐标系中的地速矢量，相应的误差矢量为

f^b为加速度计的输出。δgⁿ为重力误差矢量在n坐标系下的投影。δp是由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量。R_M和R_N分别表示子午圈和卯酉圈的曲率半径。

取不对称角φ，速度误差

位置误差δp，陀螺仪和加速度计的常值偏差ε^b、

为状态变量。根据式(1)～(5)，可得到如下状态方程：

式中，F是状态转移矩阵，G是噪声分配矩阵，w是假设均值为0的白噪声。

第二步，建立系统的量测方程。惯性导航设备和多普勒计程仪之间的安装角误差以及多普勒计程仪的标度因数会在设备安装完成之后进行标定和补偿。残余安装角误差和标度因数误差可以忽略不计。采用n坐标系下的惯性导航设备与多普勒计程仪的速度差作为观测矢量。可得到如下的量测方程：

式中，Z表示量测向量。

表示多普勒计程仪测得的速度矢量。V表示量测噪声。0_3×3为3×3维零矩阵。I₃表示单位对角矩阵。

将式(9)和(11)离散化，可得到如下的INS/DVL组合导航系统的离散时间状态空间模型：

X_k+1＝F_kX_k+G_kw_k (13)

Z_k+1＝H_k+1X_k+1+V_k+1 (14)

惯性导航系统作为一种主要的导航设备，能够提供速度、姿态和位置信息。多普勒计程仪作为辅助导航设备，提供b坐标系下的速度测量。利用惯性导航设备解算的速度与转换到n坐标系下的多普勒计程仪输出之间的差作为卡尔曼滤波器的测量向量。然后利用卡尔曼滤波器状态估计并反馈给惯导系统，以减小惯导系统的导航误差。

步骤二：采用基于声测距的交替校正算法对定位性能较差的海洋无人运动平台进行校正；

水声距离测量具有测量精度高、测量误差不随时间积累的特点。在水下协同编队中，采用INS/DVL组合导航系统，可以利用不同领航者之间的水声距离测量信息进一步提高领航者的定位精度。为了便于讨论，我们考虑一个经典案例，即对两个使用INS/DVL系统自主导航的海洋无人运动平台，利用声学调制解调器测量的两个海洋无人运动平台之间的相对距离信息进行误差交替校正。

具体步骤如下：

第一步，建立评价相对误差大小的评价指标。在交互多模型算法中，利用子滤波器的创新及其协方差矩阵来计算模型概率从而搜索最优子滤波器。受此启发，利用滤波器的创新和创新协方差组成的似然函数，来构建描述卡尔曼滤波器滤波效果的评价指标，进而确定两个海洋无人运动平台位置误差的相对大小。评价指标如下：

式中，

表示残差。

表示残差的协方差矩阵。

为卡尔曼滤波器获得的一步预测估计，

表示一步预测估计误差协方差矩阵。R_k是量测噪声协方差矩阵。

当

时，系统将利用水声测距信息和海洋无人运动平台2的位置信息来修正海洋无人运动平台1的位置。反之，则利用水声测距信息和海洋无人运动平台1的位置信息来修正海洋无人运动平台2的位置。

第二步，根据评价指标的大小对定位性能较差的海洋无人运动平台进行校正。

和

分别表示两个海洋无人运动平台从INS/DVL组合导航系统测得的位置信息。

和

分别表示滤波后得到的海洋无人运动平台1和海洋无人运动平台2的位置误差δp的估计误差协方差矩阵。d_k表示由水声调制解调器测得的两个海洋无人运动平台之间的相对距离。

当海洋无人运动平台2的位置误差小于海洋无人运动平台1时，则系统可根据水声测距结果和海洋无人运动平台2的位置信息来修正海洋无人运动平台1的位置。可得如下所示的量测方程：

式中，θ_k表示假设均值为0的高斯噪声，Θ_k为其协方差矩阵。

对式(16)进行一阶泰勒级数展开，得到：

上式可写成如下形式：

用M_k代替

m_k代替

则得到：

当卡尔曼滤波器的估计结果反馈给惯性导航系统后，海洋无人运动平台1的位置可以看作一个均值为

协方差矩阵为

的随机量。我们可以得到

的先验概率密度函数：

式中，

表示均值为

协方差矩阵为

根据式(19)，似然概率密度函数可近似如下：

利用式(20)和(21)的极大后验估计，可得到

的表达式：

当海洋无人运动平台1的位置误差小于海洋无人运动平台2时，则系统可根据水声测距结果和海洋无人运动平台1的位置信息来修正海洋无人运动平台2的位置。根据相同的推导过程，可得到

的表达式：

步骤三：利用基于统计相似度度量的鲁棒卡尔曼滤波器滤除输出异常值。

具体步骤如下：

第一步，统计相似度度量的定义。统计相似度度量s(a,b)可以用来表示两个随机向量a和b之间的相似度，定义如下：

s(a,b)＝E[f(||a-b||²)]＝∫∫f(||a-b||²)p(a,b)dadb (24)

式中，||·||表示欧氏范数，p(a,b)表示随机向量a和b之间的联合概率密度。f(·)表示相似度函数，并且满足以下三个条件：

a)f(·)是在定义域[0，+∞)内的连续函数；

b)f(·)是单调递减函数，即

c)f(·)的二阶导数是非负的，即

统计相似度度量符合相似度度量的通常定义，且随机向量之间的相似性越高，统计相似度度量就越大。

第二步，构造基于统计相似度度量的鲁棒滤波器。异常值鲁棒滤波器由时间更新和量测更新两部分组成。其中时间更新如下：

式中，

和

表示在k时刻的后验状态估计及其估计误差协方差矩阵。Q_k为标称过程噪声协方差矩阵。

为一步预测状态，

为一步预测误差协方差矩阵。

在量测更新中，通过最大化

和

之间的统计相似度度量与

和

之间的统计相似度度量之和，得到状态的近似后验概率密度函数，其中

和

分别是一步预测误差协方差矩阵

和标称噪声协方差矩阵R_k+1的平方根。

对于式(27)中的最大化问题，直接针对后验概率密度函数q(X_k+1)求最优解几乎是不可能的。为了解决这个问题，将状态后验概率密度函数近似为高斯分布，将状态后验概率密度函数的最大化问题转化为对状态后验均值和协方差矩阵的最大化问题。然后，将式(27)中由代价函数的Jensen不等式求得的下界最大化，建立新的代价函数，从而得到近似解：

式中，tr(·)表示求迹操作。

当f_x(t)＝f_z(t)＝-0.5t时，式(28)中的代价函数的结果与标准卡尔曼滤波器相同，则基于统计相似度度量的异常值鲁棒滤波器等同于标准卡尔曼滤波器。考虑到INS/DVL组合导航系统的状态空间模型中的过程噪声是具有精确协方差的高斯噪声，并且由于异常值的存在，多普勒计程仪的量测噪声是非高斯噪声，相似度函数f_x(t)取f_x(t)＝-0.5t，相似度函数f_z(t)取

ω是自由度参数，s是t的维数。根据最大点的定义可知：

其中，

利用式(31)和(32)可得到：

其中，

其中，

并且将ξ_k+1用

代替。对式(33)求解，就能得到

在最大后验估计框架中，最小二乘代价函数的Hessian矩阵的负逆等价于后验概率密度函数的协方差矩阵。M-估计通过将代价函数的Hessian矩阵的负逆作为后验协方差矩阵来处理后验协方差矩阵。受此启发，最优协方差矩阵应该是Hessian矩阵的负逆

其中，

是二阶项。

其中，

但在计算过程中，由于Hessian矩阵的二阶项

为半正定项，容易使其失去负确定性。为保持

的负确定性，从Hessian矩阵

中减去二阶项

在这种情况下，式(28)中的最大值

可以表示为：

虽然这种减去半正定矩阵的近似方法会对后验协方差矩阵产生误差，但有利于保证滤波的正确定性和稳定性，这对于长时间导航系统是至关重要的。

利用不动点迭代算法计算状态后验概率密度函数的均值和协方差，从而证明基于统计相似度度量的鲁棒滤波器的数值稳定性和滤波稳定性；

具体步骤如下：

第一步，不动点迭代算法。由于函数f_z(t)是非线性的，所以难以通过求解式(33)和(38)来计算

和

的解析解。为了解决这个问题，利用包含修正的Hessian矩阵的牛顿迭代法近似解决该问题。

式中，

i表示牛顿迭代法下的第i次迭代。

根据牛顿迭代法，

和

被近似更新为下式：

将式(31)、(33)、(39)代入式(40)、(41)得到：

利用矩阵逆引理，式(42)、(43)可改写为：

式中，

第二步，滤波稳定性分析。在实际工程应用中，组合导航系统必须能够在水下长时间航行，因此滤波稳定性非常重要。根据新的统计相似度度量理论推导出了异常值鲁棒卡尔曼滤波器。因此，有必要讨论SSMKF的数值稳定性和滤波稳定性。

因为

所以辅助参数ξ_k+1＞0。因此，后验协方差矩阵

量测噪声协方差矩阵

和一步预测误差矩阵

都是正定的。也就是说，本发明提出的滤波器是数值稳定的。

滤波稳定性是指状态估计误差在均方意义上有界。如果

和Q_k+1满足下列不等式：

式中，n为状态向量的维数，m为测量向量的维数。p_min、p_max、r_min和q_max都为正实数。则状态误差在均方意义上有界并且滤波器是稳定的。

因为

所以

是正定矩阵。并且

是有界的。这就是说：

ξ_min≤ξ_k+1≤ξ_max (49)

选取

和Q_k+1的最大特征值分别为p_max和q_max，

和R_k+1/ξ_min的最小特征值分别为r_min和p_min，可得到式(47)和(48)。从中可以看出，从

中减去半正定项

使得异常值鲁棒滤波器的估计误差是有界的是易于证明的，也就是说，保证滤波的稳定性是很容易的。因此，从

中减去

的近似是合理的。

通过实验验证方法是否可行，并与传统的INS/DVL组合系统进行对比，结合图2和图3可知，本发明相比于传统的方法具有更加优秀的校准效果。

Claims (3)

1.一种INS/DVL组合导航系统鲁棒智能协同校准方法，其特征在于，包括以下步骤：

步骤一：建立包括状态方程和量测方程在内的传统INS/DVL系统模型，状态方程具体为：

其中，δp是由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量，

为n坐标系中的地速矢量，相应的误差矢量为

φ表示由俯仰、横滚、航向三组不对称角组成的不对称角矢量，ε^b为陀螺常值漂移矢量，

为加速度计常值漂移矢量，

为陀螺随机漂移矢量，

为加速度计随机漂移矢量，w是假设均值为0的白噪声，F是状态转移矩阵，G是噪声分配矩阵；

量测方程具体为：

其中，Z表示量测向量，

表示多普勒计程仪测得的速度矢量，

表示从b坐标系变换到n坐标系的方向余弦矩阵，V表示量测噪声；

步骤二：采用基于声测距的交替校正算法对海洋无人运动平台进行校正；

步骤三：利用基于统计相似度度量的鲁棒卡尔曼滤波器滤除输出异常值。

2.根据权利要求1所述的.一种INS/DVL组合导航系统鲁棒智能协同校准方法，其特征在于：步骤二所述采用基于声测距的交替校正算法对海洋无人运动平台进行校正具体为：

步骤1：建立评价相对误差大小的评价指标，利用滤波器的创新和创新协方差组成的似然函数构建描述卡尔曼滤波器滤波效果的评价指标，进而确定两个海洋无人运动平台位置误差的相对大小，评价指标为：

式中，

表示残差，

表示残差的协方差矩阵，

为卡尔曼滤波器获得的一步预测估计，

表示一步预测估计误差协方差矩阵，R_k是量测噪声协方差矩阵；

当

时，系统将利用水声测距信息和海洋无人运动平台2的位置信息来修正海洋无人运动平台1的位置，反之，则利用水声测距信息和海洋无人运动平台1的位置信息来修正海洋无人运动平台2的位置；

步骤2：根据评价指标的大小对海洋无人运动平台进行校正：

和

分别表示两个海洋无人运动平台从INS/DVL组合导航系统测得的位置信息，

和

分别表示滤波后得到的海洋无人运动平台1和海洋无人运动平台2的位置误差δp的估计误差协方差矩阵，d_k表示由水声调制解调器测得的两个海洋无人运动平台之间的相对距离，假设海洋无人运动平台2的位置误差小于海洋无人运动平台1，则系统可根据水声测距结果和海洋无人运动平台2的位置信息来修正海洋无人运动平台1的位置，可得如下所示的量测方程：

式中，θ_k表示假设均值为0的高斯噪声，Θ_k为其协方差矩阵；

对上式进行一阶泰勒级数展开，得到：

上式可写成如下形式：

用M_k代替

m_k代替

则得到：

当卡尔曼滤波器的估计结果反馈给惯性导航系统后，海洋无人运动平台1的位置可以看作一个均值为

协方差矩阵为

的随机量，可以得到

的先验概率密度函数：

式中，

表示均值为

协方差矩阵为

似然概率密度函数可近似如下：

可得到

的表达式：

当海洋无人运动平台1的位置误差小于海洋无人运动平台2时，则系统可根据水声测距结果和海洋无人运动平台1的位置信息来修正海洋无人运动平台2的位置，可得到

的表达式：

。

3.根据权利要求1或2所述的一种INS/DVL组合导航系统鲁棒智能协同校准方法，其特征在于：步骤三所述利用基于统计相似度度量的鲁棒卡尔曼滤波器滤除输出异常值具体为：

构造基于统计相似度度量的鲁棒滤波器，异常值鲁棒滤波器由时间更新和量测更新两部分组成，其中时间更新如下：

式中，

和

表示在k时刻的后验状态估计及其估计误差协方差矩阵，Q_k为标称过程噪声协方差矩阵，

为一步预测状态，

为一步预测误差协方差矩阵；

在量测更新中，通过最大化

和

之间的统计相似度度量与

和

之间的统计相似度度量之和，得到状态的近似后验概率密度函数，其中

和

分别是一步预测误差协方差矩阵

和标称噪声协方差矩阵R_k+1的平方根：

将状态后验概率密度函数近似为高斯分布，将状态后验概率密度函数的最大化问题转化为对状态后验均值和协方差矩阵的最大化问题，然后，将由代价函数的Jensen不等式求得的下界最大化，建立新的代价函数，从而得到近似解：

B_k+1＝∫(Z_k+1-H_k+1X)(Z_k+1-H_k+1X)^T

式中，tr(·)表示求迹操作；

相似度函数f_x(t)取f_x(t)＝-0.5t，相似度函数f_z(t)取

ω是自由度参数，s是t的维数，根据最大点的定义可知：

其中，

进而得到：

其中，

其中，

并且将ξ_k+1用

代替，对

求解，得到

最优协方差矩阵是Hessian矩阵的负逆

其中，

是二阶项；

其中，

为保持

的负确定性，从Hessian矩阵

中减去二阶项

在这种情况下，

中的最大值

可以表示为：

Images