[发明专利]一种惯性导航系统的预积分算法

权利要求书

1.一种惯性导航系统的预积分算法，其特征在于：包括以下步骤：

步骤1：初始化定义；

步骤2：计算预积分结果；

步骤3：计算等效旋转矢量；

步骤4：计算连续两个时刻之间机体系的旋转关系；

步骤5：惯性状态解算。

2.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤1所述的初始化定义，具体为初始化运动载体的位置信息速度信息和姿态信息其中：式中ΔP_x、ΔP_y、ΔP_z是X、Y、Z三个方向的位置变化量，ΔV_x、ΔV_y、ΔV_z是X、Y、Z三个方向的速度变化量，是t_i时刻到t_i+1时刻的姿态变化量，t_i是载体运动初始时刻，I是单位矩阵。

3.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤2所述的计算预积分结果，具体为：当有新的量测数据在t_j+1时刻加入系统中时，计算更新位置变化量更新速度变化量更新姿态变化量计算预积分Δx_i→j+1，其中t_j+1＝t_j+Δt，t_j是运动载体预积分计算时刻，Δt是新的量测数据到来的时间间隔。

4.根据权利要求3所述的计算一种惯性导航系统的预积分算法，其特征在于：所述的计算更新位置变化量计算公式为：

计算更新速度变化量计算公式为：

式中f_j是新增惯性量测；

计算更新姿态变化量计算公式为：式中为t_j时刻到t_j+1时刻的姿态信息；

计算预积分Δx_i→j+1，计算公式为：

5.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤3所述的计算等效旋转矢量具体为：在速度更新周期T＝t_j+1-t_j较短的情况下，等效旋转矢量Φ计算如下：

其中，式中，是导航系相对于机体系的角速度，是惯性系相对于机体系的角速度，是t_i时刻运动载体的姿态矩阵，是导航系相对于惯性系的角速度。

6.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤4所述的计算连续两个时刻之间机体系的旋转关系，计算公式如下：

7.根据权利要求1所述的一种惯性导航系统的预积分算法，其特征在于：步骤5所述的惯性状态解算，包括导航系统的位置速度和姿态的解算，计算公式分别为：

为t_i时刻到t_j时刻的姿态旋转矩阵，为t_i时刻导航系统的位置，为t_i时刻机体系的姿态矩阵，为t_i时刻的速度，为t_i时刻所在位置的重力加速度，为t_i时刻运动载体的角速度；

本次预积分过程结束后，在下一次预积分开始时，将参考坐标系选为此时的机体坐标系，并进行下列初始化：令进入下一次预积分过程。

8.根据权利要求7所述的一种惯性导航系统的预积分算法，其特征在于：t_i时刻的速度计算公式为：

t_i时刻机体系的姿态矩阵计算公式为：