本发明涉及协同导航算法领域，具体涉及一种针对未知量测噪声下的具有自适应的协同导航滤波方法。
背景技术：
自主水下航行器(Autonomous Underwater Vehicles,AUV)在民用还是军事方面都有广泛的应用。随着对海洋的探索越来越深入，使得AUV的使命也变得越来越复杂化和多样化，如果仅仅依靠单个AUV去实现这些功能，不仅在成本上会提高很多，而且可靠性也会降低，而由多台水下AUV组成的协作系统具有空间分布、功能分布、冗余等特点。然而不论单AUV还是多AUV系统以及其执行何种任务，导航系统的优劣直接影响系统的性能好坏。协同导航定位技术作为一种新的导航定位方法具有只对有限数目的AUV增加设备费从而来提高编队的导航精度等优点。由于水下环境和基于声学定位的特殊性，协同导航滤波过程中所需要量测噪声协方差阵具有时变性，传统算法中是通过提前设置预值来进行定位解算的，并未考虑到其它可能的变化，限制了AUV的适用范围。本方法提出通过提前预设多组不同的量测噪声协方差矩阵来提高协同导航滤波的自适应性，扩大AUV的适用范围。
技术实现要素：
本发明的目的在于提出一种在未知量测噪声环境下时对协同编队进行定位能够抑制噪声未知下导致的从AUV定位精度下降问题的具有自适应的协同导航滤波方法。
本发明的目的是这样实现的
第一步：输入交互过程
如图1所示，交互式多模型算法是一个循环算法，通过输入交互过程、模型滤波、模型概率更新以及输出交互过程四个过程完成状态估计。
在输入交互过程中，首次通过预验模型概率获取混合状态，下次循环则通过计算得到的模型概率替换。
协同导航从AUV交互混合状态估计及协方差为：
式中为k-1时刻第j各滤波器的状态估计；Pj(k-1)为对应的协方差矩阵；P0i(k-1)为状态估计所对应的状态误差协方差矩阵。
首次滤波中，用初始模型预测概率代替模型条件转移概率进行运算，下个滤波周期中通过利用上一个周期各个滤波器的状态估计和模型条件转移概率进行运算。
第二步：模型滤波
基于EKF的协同导航滤波器1工作如下：
系统状态一步预测：其中表示从AUV在时刻k+1的一步估计状态，f为从AUV运动学方程，为从AUV交互混合估计状态1，uk量测输入。
一步预测协方差矩阵为：
其中Fk为状态转移矩阵，Pk为上一时刻的的协方差矩阵，Gk为过程噪声分布矩阵，Qk表示噪声协方差矩阵。
量测量预测为：
其中h表示量测方程，分别为领航者AUV1，AUV2的状态。
残差信息和其协方差：
其中Zk+1为传感器接收到的量测值，Hk+1为量测矩阵的雅可比矩阵，为滤波器1中的量测噪声协方差矩阵。
滤波增益：
状态估计及误差协方差矩阵：
P1,k+1＝(I-Kk+1Hk+1)Pk+1,k，其中I为单位矩阵。
基于EKF的协同导航滤波器2中主要不同于滤波器1的是量测噪声协方差矩阵选取了另外一组其他步骤相同，并得到滤波器2下残差信息v2,k+1和其协方差S2,k+1，状态估计及其误差协方差矩阵P2,k+1。
第三步：模型概率更新
做为本算法的重要部分，通过获取各个滤波器的残差信息并且结合贝叶斯假设检验方法，得到各模型的似然函数，求法如下：
m＝3表示协同导航中量测向量的维数；vi(k)为协同导航模型在k时刻时通过卡尔曼滤波获得的残差向量；Si(k)为对应残差协方差矩阵。
结合上一时刻的模型概率以及模型先验信息，得到k时刻的模型概率为
第四步：交互输出
循环算法中最后一步，由前面各个基于不同模型滤波器的状态估计值及其模型概率加权融合得到，结合各模型的从AUV联合状态估计和协方差矩阵P(k)计算如下：
至此完成一次输出，进入下一时刻的状态估计。
本发明方法具有以下优点：
相比于传统的协同导航滤波方法，在噪声未知的环境下条件下，本方能有效减小因为噪声预设偏差过大导致的精度下降问题。本方法具有很好的模块化特性，所以我们不仅可以增加模型数量，而且还可以自由的选取具有各种特性的滤波器，因此其具有很好的后期修正性。
附图说明
图1为本发明示意图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明提供一种关于协同导航自适应滤波方法。该方法通过事先构建多组关于不同量测噪声的状态模型，并通过交互式多模型算法将各组先验模型整合起来，达到自适应滤波的目的。该方法的特点在于在未知环境下通过应用交互式多模型算法能够有效抑制实际噪声与预测噪声不匹配时造成的导航精度过低的问题，同时该方法具有模块化特点，能够进一步针对具体噪声特点进行滤波算法的优化，适合应用于协同导航中。
实施例：
具体操作如下所述：
下面结合图1及实施例对本发明的内容做进一步详细的阐述。
第一步：输入交互过程
如图1所示，交互式多模型算法是一个循环算法，通过输入交互过程、模型滤波、模型概率更新以及输出交互过程四个过程完成状态估计。
在输入交互过程中，首次通过预验模型概率获取混合状态，下次循环则通过计算得到的模型概率替换。
协同导航从AUV交互混合状态估计及协方差为：
式中为k-1时刻第j各滤波器的状态估计；Pj(k-1)为对应的协方差矩阵；P0i(k-1)为状态估计所对应的状态误差协方差矩阵。
首次滤波中，用初始模型预测概率代替模型条件转移概率进行运算，下个滤波周期中通过利用上一个周期各个滤波器的状态估计和模型条件转移概率进行运算。
第二步：模型滤波
基于EKF的协同导航滤波器1工作如下：
系统状态一步预测：其中表示从AUV在时刻k+1的一步估计状态，f为从AUV运动学方程，为从AUV交互混合估计状态1，uk量测输入。
一步预测协方差矩阵为：
其中Fk为状态转移矩阵，Pk为上一时刻的的协方差矩阵，Gk为过程噪声分布矩阵，Qk表示噪声协方差矩阵。
量测量预测为：
其中h表示量测方程，分别为领航者AUV1，AUV2的状态。
残差信息和其协方差：
其中Zk+1为传感器接收到的量测值，Hk+1为量测矩阵的雅可比矩阵，为滤波器1中的量测噪声协方差矩阵。
滤波增益：
状态估计及误差协方差矩阵：
P1,k+1＝(I-Kk+1Hk+1)Pk+1,k，其中I为单位矩阵。
基于EKF的协同导航滤波器2中主要不同于滤波器1的是量测噪声协方差矩阵选取了另外一组其他步骤相同，并得到滤波器2下残差信息v2,k+1和其协方差S2,k+1，状态估计及其误差协方差矩阵P2,k+1
第三步：模型概率更新
做为本算法的重要部分，通过获取各个滤波器的残差信息并且结合贝叶斯假设检验方法，得到各模型的似然函数，求法如下：
m＝3表示协同导航中量测向量的维数；vi(k)为协同导航模型在k时刻时通过卡尔曼滤波获得的残差向量；Si(k)为对应残差协方差矩阵。
结合上一时刻的模型概率以及模型先验信息，得到k时刻的模型概率为
第四步：交互输出
循环算法中最后一步，由前面各个基于不同模型滤波器的状态估计值及其模型概率加权融合得到，结合各模型的从AUV联合状态估计和协方差矩阵P(k)计算如下：
至此完成一次输出，进入下一时刻的状态估计。
以上结合附图对本发明的具体实施方式做了详细说明，但这些说明不能被理解为限制了本发明的范围，本发明的保护范围由随附的权利要求书限定，任何在本发明权利要求基础上的改动都属于本发明的保护范围。