[发明专利]一种基于地磁场自适应修正的航姿系统姿态解算方法

摘要

本发明提出一种基于地磁场自适应修正的航姿系统姿态解算方法，通过分别计算状态转移矩阵、量测矩阵、系统噪声协方差矩阵、量测噪声协方差矩阵和自适应矩阵，实现采用自适应扩展卡尔曼滤波算法进行航姿系统姿态解算；本发明有效解决了载体姿态的累积误差较大的问题，同时提高了姿态解算精度。

主权项

1.一种基于地磁场自适应修正的航姿系统姿态解算方法，其特征在于：采用自适应扩展卡尔曼滤波算法进行航姿系统姿态解算，其中状态方程为：X_k＝Φ_k/k‑1αX_k‑1+W_k‑1量测方程为：Z_k＝H_kX_k+V_k其中X_k为t_k时刻的状态向量，Z_k为t_k时刻的量测向量，Φ_k/k‑1为t_k‑1时刻至t_k时刻的状态转移矩阵，H_k为量测矩阵，V_k为量测噪声，W_k‑1为系统噪声，系统噪声协方差矩阵为Q_k‑1，量测噪声协方差矩阵为R_k；α为自适应矩阵；采用以下步骤计算状态转移矩阵、量测矩阵、系统噪声协方差矩阵、量测噪声协方差矩阵和自适应矩阵：状态转移矩阵采用公式计算得到，其中I为单位矩阵，Δθ根据公式确定，ΔΘ根据公式得到，其中为航姿系统中传感器输出的载体坐标系B下的载体角速度信息；量测矩阵采用公式计算得到，其中a为在载体坐标系B中测量的重力加速度矢量，g为在全局坐标系G中测量的重力加速度矢量，并采用零标量的四元数方式表示矢量g与a；系统噪声协方差矩阵采用公式计算得到，其中Δt为相邻时刻的时间间隔，Σ_gyro为陀螺仪噪声协方差矩阵q(t_k‑1)为t_k‑1时刻全局坐标系G到载体坐标系B的旋转四元数，对于某一四元数q，Ξ(q)为Σ_gyro为航姿系统中陀螺仪噪声协方差矩阵；量测噪声协方差矩阵采用公式计算得到，其中Σ_acc为航姿系统中加速度计噪声协方差矩阵，q(t_k)为t_k时刻全局坐标系G到载体坐标系B的旋转四元数；自适应矩阵采用公式计算得到，其中为根据公式计算得到的四元数；Γ＝l_x²+l_y²，矢量l＝(l_x l_y l_z)^T表示由载体坐标系下测量到的地磁场矢量m＝[m_x m_y m_z]^T进行坐标变换后得到的地磁场矢量，其中为计算得到的姿态四元数。