[发明专利]改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法

权利要求书

1.改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，包括如下步骤：

(1)对需要用到的坐标系进行定义：

(2)建立水下潜航阶段SINS/DVL子系统的状态方程：以SINS的3轴姿态误差，3轴速度误差，位置误差，3轴陀螺仪随机漂移、3轴加速度计零偏和DVL速度偏移误差、刻度系数误差建立21维状态向量，根据系统误差方程建立该子系统的状态方程；

(3)建立水下潜航阶段SINS/DVL子系统的量测方程：根据SINS解算的3维速度量和DVL测量的速度量之差作为量测量，并结合步骤(2)选取的误差状态向量建立该子系统的量测方程；

(4)建立水面位置修正阶段SINS/GPS子系统的状态方程：不考虑天向运动速度及误差，以SINS的3轴姿态误差、速度误差、位置误差、3轴陀螺仪随机漂移、加速度计零偏建立12维状态向量，根据误差方程建立该子系统的状态方程；

(5)建立水面位置修正阶段SINS/GPS子系统的量测方程：以SINS解算出来的位置、速度与GPS输出的位置、速度之差作为量测量，并结合步骤(4)选取的误差状态向量建立该子系统的量测方程；

(6)综合步骤(2)和步骤(3)建立水下潜航阶段的非线性滤波方程，在水下潜航一段时间以后，AUV浮出水面，结合步骤(4)和步骤(5)建立水面位置修正阶段非线性滤波方程，进行改进的无迹卡尔曼滤波解算，完成时间更新、量测更新和滤波更新，完成定时位置信息校正。

2.根据权利要求1所述的改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法，其特征在于，所述步骤(1)建立的坐标系具体包括：

i——惯性坐标系：不随地球旋转，原点位于地球中心，z_i轴指向北极，x_i轴指向春分点，y_i轴与x_i、z_i构成右手坐标系；

e——地球坐标系：与地球固联，原点位于地心，x_e轴穿越本初子午线与赤道交点，z_e轴指向北极，y_e轴x_e、z_e构成右手坐标系；

b——载体坐标系：原点位于运载体中心，z_b轴垂直运载体向上，x_b指向运载体前方，y_b与x_b、z_b构成右手坐标系；

p——实际计算得出的平台坐标系；

n——与东-北-天地理坐标系重合的导航坐标系。