基于因子图的协同定位与误差估计算法

摘要

针对多自主水下航行器协同定位系统中从艇的数据融合问题,首先建立了协同定位系统的数学模型,然后分析了速度误差及航向误差对从艇定位误差的影响,同时设计了协同定位及误差估计的因子图模型.接着,提出了基于高斯噪声的协同定位及误差估计算法,利用均值和方差在因子图各节点间传递完成对从艇位置、速度误差和航向误差的估计.为了验证算法的有效性,通过仿真实验和实船试验的离线数据对协同定位及误差估计算法进行验证.结果表明,所提算法可以有效降低从艇的定位误差,在从艇自主定位时尤其明显,大幅提高了从艇的导航定位能力.