[发明专利]一种具有自适应的协同导航滤波方法

摘要

本发明涉及协同导航算法领域，具体涉及一种针对未知量测噪声下的具有自适应的协同导航滤波方法。本发明包括：输入交互过程；模型滤波；模型概率更新；交互输出。相比于传统的协同导航滤波方法，在噪声未知的环境下条件下，本方能有效减小因为噪声预设偏差过大导致的精度下降问题。本方法具有很好的模块化特性，所以我们不仅可以增加模型数量，而且还可以自由的选取具有各种特性的滤波器，因此其具有很好的后期修正性。

主权项

1.一种具有自适应的协同导航滤波方法，其特征在于，包括如下步骤：(1)输入交互过程：通过输入交互过程、模型滤波、模型概率更新以及输出交互过程四个过程完成状态估计；在输入交互过程中，首次通过预验模型概率获取混合状态，下次循环则通过计算得到的模型概率替换；协同导航从AUV交互混合状态估计及协方差为：式中为k‑1时刻第j各滤波器的状态估计；P_j(k‑1)为对应的协方差矩阵；P_0i(k‑1)为状态估计所对应的状态误差协方差矩阵；首次滤波中，用初始模型预测概率代替模型条件转移概率进行运算，下个滤波周期中通过利用上一个周期各个滤波器的状态估计和模型条件转移概率进行运算；(2)模型滤波基于EKF的协同导航滤波器1工作：系统状态一步预测：其中表示从AUV在时刻k+1的一步估计状态，f为从AUV运动学方程，为从AUV交互混合估计状态1，u_k量测输入；一步预测协方差矩阵为：其中F_k为状态转移矩阵，P_k为上一时刻的的协方差矩阵，G_k为过程噪声分布矩阵，Q_k表示噪声协方差矩阵；量测量预测为：其中h表示量测方程，分别为领航者AUV1，AUV2的状态；残差信息和其协方差：其中Z_k+1为传感器接收到的量测值，H_k+1为量测矩阵的雅可比矩阵，为滤波器1中的量测噪声协方差矩阵；滤波增益：状态估计及误差协方差矩阵：P_1,k+1＝(I‑K_k+1H_k+1)P_k+1,k，其中I为单位矩阵。基于EKF的协同导航滤波器2中不同于滤波器1的是量测噪声协方差矩阵选取了另外一组其他步骤相同，并得到滤波器2下残差信息v_2,k+1和其协方差S_2,k+1，状态估计及其误差协方差矩阵P_2,k+1；(3)模型概率更新通过获取各个滤波器的残差信息并且结合贝叶斯假设检验方法，得到各模型的似然函数：m＝3表示协同导航中量测向量的维数；v_i(k)为协同导航模型在k时刻时通过卡尔曼滤波获得的残差向量；S_i(k)为对应残差协方差矩阵；结合上一时刻的模型概率以及模型先验信息，得到k时刻的模型概率为(4)交互输出由前面各个基于不同模型滤波器的状态估计值及其模型概率加权融合得到估计结果，结合各模型的从AUV联合状态估计和协方差矩阵P(k)计算如下：至此完成一次输出，进入下一时刻的状态估计。