[发明专利]一种基于双状态多因子抗差估计的SINS/DVL紧组合系统

权利要求书

1.一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，，包括：

步骤1：获取载体当前时刻SINS和DVL的采集数据；

步骤2：根据步骤1的采集数据建立状态方程和量测方程；

步骤3：对状态量和量测量进行卡方检测；

步骤4：计算模型误差和量测误差的自适应因子；

步骤5：根据步骤4的多因子调节卡尔曼滤波的增益矩阵；

步骤6：根据步骤2的系统方程和步骤5的增益矩阵，利用卡尔曼进行最优估计，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。

2.根据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：步骤1的SINS和DVL采集数据包括：IMU采集的陀螺数据：其中，表示载体坐标系下X轴的角速度，表示载体坐标系下Y轴角速度，表示载体坐标系下Z轴角速度；IMU采集的加速度数据：其中，表示载体坐标系下X轴的加速度，表示载体坐标系下Y轴的及速度，表示载体坐标系下Z轴的加速度；DVL采集的四通道的多普勒频移其中，表示DVL通道1的多普勒频移，表示DVL通道2的多普勒频移，表示DVL通道3的多普勒频移，表示DVL通道4的多普勒频移。

3.根据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：步骤2建立系统状态方程，具体步骤如下：

其中，F表示系统状态转移矩阵，X表示状态向量，G表示系统噪声矩阵，W表示系统噪声矢量，其中，状态向量表示为：

其中，[φ_x φ_y φ_z]^T表示载体俯仰角、横滚角、航向角误差；表示载体东向、北向、天向速度误差；[δλ δL δh]^T表示载体地理经度、纬度、高度误差；表示SINS的加速度零偏；[ε_x ε_y ε_z]^T表示SINS的陀螺零偏；[ρ₁ ρ₂ ρ₃ ρ₄]^T表示DVL四通道多普勒频移常值偏移；k_D表示DVL刻度因子误差；b_ps表示深度计零偏；系统状态转移矩阵F、系统噪声矩阵G、系统噪声矢量W表示为：

其中，

其中，表示b系到n系的姿态转移矩阵，表示加速度计噪声，表示陀螺噪声，ω_ie表示地球自转角速率，V_E表示东向速度，V_N表示北向速度，V_U表示天向速度，R_e表示地球长半轴半径，e表示地球偏心率，ω_ie表示地球自转角速率，g_e表示赤道重力，β表示地球重力扁率，β₂＝3.08×10^-6S^-2，β₁＝(1/8)(2βf+f²)，f表示地球扁率。