买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：哈尔滨工业大学(威海)

摘要：本发明提供一种基于平方根分解的扩维容积ECKF算法的多AUV协同导航的滤波方法、系统及存储介质，涉及多AUV协同导航技术领域，为解决现有的ECKF算法在滤波过程中的矩阵求逆、矩阵开方等对数值计算时较为敏感,会引入较大计算误差；同时难以有效处理过程噪声的不可加性、量测噪声的未知时变性的问题。包括：S1、建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；S2、采用基于平方根分解的扩维容积ECKF算法对从AUV进行状态预测和量测预测，过程中使用QR正交分解求解状态估计误差协方差矩阵的平方根，以提高滤波精度和数值计算的稳定性；S3、对状态估计值和误差协方差矩阵的平方根进行更新，实现多AUV协同导航的滤波。本发明用于多AUV的协同导航定位。

主权项：1.一种基于平方根分解的扩维容积ECKF算法的多AUV协同导航的滤波方法，其特征在于，包括如下步骤：步骤S1、建立主从式多AUV协同导航系统的离散时间状态方程和量测方程；步骤S2、采用基于平方根分解的扩维容积ECKF算法对从AUV进行状态预测和量测预测，过程中使用QR正交分解求解状态估计误差协方差矩阵的平方根，以提高滤波精度和数值计算的稳定性；步骤S3、对状态估计值和误差协方差矩阵的平方根进行更新，实现多AUV协同导航的滤波；步骤S2中所述状态预测的过程包括：1假设tk+1时刻后验概率密度函数中上一时刻状态估计值和误差协方差矩阵的平方根Sk|k已知，对误差协方差矩阵平方根的初始值直接对矩阵元素开平方：S0|0＝sqrtmP0|0将过程噪声和量测噪声vk加入状态变量，对状态估计值以及Sk|k进行状态扩维： 其中，nw为过程噪声的维度，nv为量测噪声的维度，Qk为tk时刻的过程噪声协方差矩阵，Rk为tk时刻的量测噪声协方差矩阵；通过过程变量和计算容积点Xi,k|k： 其中，为容积点集，i为对应的维度；2根据状态变量分量和过程噪声分量计算传播容积点： 3计算tk+1时刻的状态预测值： 4计算tk+1时刻的加权矩阵 利用加权矩阵计算tk+1时刻的状态误差协方差的预测值的平方根： 其中，qr为对进行QR分解；步骤S2中量测预测的过程包括：1重新构造容积点： 2根据重新构造的容积点Xi,k+1|k和从AUV传感器的输入量uk计算传播容积点：Zi,k+1|k＝hXi,k+1|k,uk3计算tk+1时刻的量测的预测值： 4计算tk+1时刻的加权矩阵Zk+1|k： 根据加权矩阵Zk+1|k计算tk+1时刻的新息协方差矩阵的平方根：Szz,k+1|k＝qr[Zk+1|k]5计算tk+1时刻的加权矩阵χk+1|k： 根据加权矩阵χk+1|k计算tk+1时刻的互协方差矩阵：

全文数据：

权利要求：

百度查询： 哈尔滨工业大学(威海) 一种基于平方根分解的扩维容积ECKF算法的多AUV协同导航的滤波方法、系统及存储介质