基于EKF的姿态解算

首先一个最为基本的公式（我去，怎么编写公式啊？）：; 其中;。

由此，我们可以得出状态方程：step1: ,。

计算协方差矩阵 step2: ; Q 为过程噪声。

计算卡尔曼增益 step3: ;R为观测噪声。

观测更新 step4： ; 

协方差更新 step5： 

根据以上步骤即可得出实时的四元素矩阵，然后再根据四元素转化为欧拉角。