基于EKF的姿态解算

首先一个最为基本的公式(我去,怎么编写公式啊?):; 其中;。

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

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

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

观测更新 step4: ; 


协方差更新 step5: 

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

你可能感兴趣的:(fusion,Data)