[发明专利]一种地磁场模型辅助的载体姿态和航向计算方法和系统

摘要

本发明公开一种地磁场模型辅助的载体姿态和航向计算方法和系统，该方法包括以下步骤：根据载体的位置信息和地磁场模型，计算理想磁倾角；根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角；根据理想磁倾角和估计磁倾角，判断是否存在环境磁干扰；当存在环境磁干扰时，根据磁传感器量测值的自适应权重，构建磁传感器量测值的等价权矩阵；根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。本发明根据理想磁倾角和估计磁倾角检测环境磁干扰，并在检测到环境磁干扰时，构建磁传感器量测值的等价权矩阵，并实现磁传感器量测值对状态估计值贡献的自适应调整，使计算得到的姿态信息和航向信息具有抗差性。

主权项

1.一种地磁场模型辅助的载体姿态和航向计算方法，其特征在于，包括以下步骤：根据载体的位置信息和地磁场模型，计算理想磁倾角；根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角；根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰；当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重，并根据所述磁传感器量测值的自适应权重，构建所述磁传感器量测值的等价权矩阵；其中，采用以下公式构建所述磁传感器量测值的自适应权重：不存在环境磁干扰时；存在环境磁干扰时；其中，为k时刻所述磁传感器第i个量测值的自适应权重，p_ki为量测值协方差矩阵的第i个主对角线元素，c为常数，V_i,k′为磁传感器量测值的归一化残差；根据加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息，具体包括以下步骤：(1)根据所述加速度计量测值更新状态预测值，得到状态更新值，并使用所述状态更新值更新姿态四元数的估计值；(2)根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新值，得到状态估计值，并使用所述状态估计值更新所述姿态四元数的估计值；(3)根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述载体姿态信息和航向信息；其中步骤(2)又具体包括：根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建第二量测方程；根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值，具体为：采用以下公式更新状态更新值，得到状态估计值：为k时刻的状态估计值，为k时刻的状态更新值，L_m,k为k时刻的所述磁传感器量测值，为的协方差矩阵，为k时刻的协方差矩阵，T为转置，为从导航坐标系到载体坐标系的转化矩阵，K_m,k为k时刻的增益矩阵，I为单位阵,A_m,k为k时刻的量测矩阵，为k时刻的所述磁传感器量测值的等价权矩阵，为所述姿态四元数的估计值。