[发明专利]用于惯性-地磁组合的快速扩展卡尔曼方法

摘要

本发明提供一种用于惯性‑地磁组合的快速扩展卡尔曼方法，该算法首先利用三轴加速度计和三轴地磁传感器在每个采样时刻输出的三轴向上的测量值分别构建两个三维矢量，这两个三维矢量分别作为重力加速度矢量和地磁场矢量在载体坐标系下的观测值。随后用上述两个三维矢量构建两个新的空间正交三维矢量。新产生的两个正交矢量最后用于简化卡尔曼增益矩阵。简化后的卡尔曼增益矩阵的性能仍然和原卡尔曼增益矩阵相同，即使得扩展卡尔曼算法保持原有的精度，但是由于避免了逆运算，简化的增益矩阵相当程度地提高了扩展卡尔曼算法的运算速度。

主权项

1.一种用于惯性‑地磁组合的快速扩展卡尔曼算法，其特征在于：所述惯性‑地磁组合包括三轴加速度计、三轴陀螺仪以及三轴地磁传感器，还包括以下步骤：步骤1：构建重力加速度矢量g_r和地磁场矢量m_r在当地地理坐标系下的表示，即：g_r＝[0,0,‑1]^T，m_r＝[cos(90°‑θ_gm),0,‑sin(90°‑θ_gm)]^T   (1)其中，θ_gm为g_r和m_r的空间夹角，随后利用g_r和m_r构建两个相互正交的空间矢量，即：r₁′＝(g_r+m_r)/[2cos(θ_gm/2)]，r₂′＝(g_r‑m_r)/[2sin(θ_gm/2)]   (2)然后，将上述r₁′和r′₂进行空间旋转，使得r₁′和r′₂的测量轴与陀螺仪的测量轴一致，即：其中，q_rot＝cos(θ_gm/4)+sin(θ_gm/4)·(b′₁×b′₂)/|b′₁×b′₂|，后续步骤会将r₁′和r′₂作为姿态解算时的参考矢量；步骤2：在每个采样时刻k，由三轴加速度计和三轴地磁传感器的输出分别构建r₁和r₂在载体坐标系下的观测值，即g_b和m_b；随后利用g_b和m_b构建两个正交矢量，即：b′₁＝(g_b+m_b)/[2cos(θ_gm/2)]，b′₂＝(g_b‑m_b)/[2sin(θ_gm/2)]   (4)然后，对b′₁和b′₂进行空间旋转，以使得旋转后的两个正交矢量的测量轴与陀螺仪一致，即：扩展卡尔曼算法的过程方程和观测方程通常分别具有如下形式，过程方程为：观测方程为：其中，f_s为采样频率；为由k时刻的陀螺仪三轴向输出ω_x、ω_y和ω_z构成的三维反对称阵；Φ_k命名为状态转换矩阵；q_k和q_k+1分别为k时刻和k+1时刻由四元数表示的载体姿态；w_k为高斯白噪声，其协方差其中I_4×4为四维单位矩阵，σ_w为陀螺仪每个轴向上的输出噪声的标准差，进一步地，如果定义姿态的一步预测估计为q_k+1/k＝Φ_kq_k＝[q₀ q₁i q₂j q₃k]，那么：旋转矩阵：和为互不相关的观测噪声，其协方差分别为，和其中σ_a和σ_m分别为b₁和b₂在每个轴向上的输出噪声的标准差；扩展卡尔曼算法的姿态递推迭代方程为：式中，卡尔曼增益矩阵其中P_k+1/k为利用k时刻的姿态及k+1时刻的陀螺仪输出对k+1时刻的姿态进行一步预测的预测协方差，为线性化后的观测矩阵，为观测方程的观测误差协方差；步骤3：利用r₁和r₂,将F_k+1化简为：步骤4：令姿态验后估计误差协方差P_k/k＝α²I_4×4，随后利用P_k/k将P_k+1/k简化为：步骤5：令其中max()函数表示取括号内的两者的最大值，然后将矩阵简化为：其中,将c/4Δ_k合理地略去，在略去c/4Δ_k之后，矩阵的逆矩阵简化为：其中，步骤6：将矩阵简化为：步骤7：利用步骤5和步骤6的结果将卡尔曼增益简化为：并最终将扩展卡尔曼算法的姿态递推迭代方程简化为：其中，返回步骤2，直至用户终止运算。