一种地形辅助惯导紧组合的水下潜器导航定位方法

摘要

本发明公开了一种地形辅助惯导紧组合的水下潜器导航定位方法，主要步骤包括：初始化组合导航系统；利用非线性离散状态方程进行状态量一步预测；利用非线性离散量测方程计算粒子滤波器的重要性权值；归一化重要性权值并估计潜器的导航参数；进行重要性重采样；完成非线性粒子滤波算法。本发明方法能有效、充分地利用各子系统的信息，相比地形辅助惯导松组合方式，定位的精度有明显提高，保证了水下潜器的长航时导航定位精度。

主权项

一种地形辅助惯导紧组合的水下潜器导航定位方法，其特征在于，该方法包括以下步骤：步骤1)初始化地形辅助惯导组合导航定位系统，为系统的导航参数更新提供初始值：通过全球导航卫星系统获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；初始时刻k＝0，初始化M个粒子，根据先验概率分布进行粒子初始化；步骤2)令k:＝k+1，利用以潜器位置、姿态角和陀螺漂移为状态量的非线性离散状态方程进行一步预测，得到k时刻的一步预测状态量x_k；步骤3)利用非线性离散量测方程处理水深数据并提取水平距离，计算波束投影点水深误差、潜器的东向距离误差和北向距离误差，然后计算重要性权值；步骤4)归一化重要性权值，得到k时刻状态量的最小均方估计为将其作为组合后的潜器导航参数输出；步骤5)进行重要性重采样：得到新的粒子集合步骤6)判断潜器航行时间是否结束，若是则结束本方法，否则返回步骤2)。