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

权利要求书

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，直至用户终止运算。

2.如权利要求1所述的用于惯性-地磁组合的快速扩展卡尔曼方法，其特征在于：对P_k/k＝α²I_4×4具体解释为：

由于初始时刻时载体是静止的，利用单采样点姿态解算算法，解算出载体的初始姿态，此时，扩展卡尔曼算法的初始姿态估计误差设置为：

忽略高阶项，得到：

其中，

利用式(16)-式(18)，P_k/k可以由P_0/0迭代计算出：

上式中，变量的说明已经在解释式(6)-式(19)时给出。