基于水声定位系统的水下载体运动状态估计方法及系统

本发明公开了一种基于水声定位系统的水下载体运动状态估计方法及系统，所述方法包括：获取水下载体的初始运动状态；所述初始运动状态包括初始时刻水下载体的速度、位置和水声定位系统的基阵与水下载体之间的观测距离；以水下载体的速度和位置为状态量、以观测距离为观测量，构建自适应容积卡尔曼滤波算法的状态转移方程和观测方程；实时获取水下载体的观测距离，通过自适应容积卡尔曼滤波算法对水下载体进行运动状态估计。本发明改进了自适应容积卡尔曼滤波算法，实现了过程噪声方差和观测噪声方差等未知噪声的估计，削弱噪声统计特性不确定性对卡尔曼滤波器的冲击，有效剔除水声定位野值，能够提升水下载体运动状态估计精度。