上一个章节说了无人机姿态计算，无人机姿态估计1：四元数和姿态角到底什么关系？但是如果只是纯陀螺仪积分计算，这样就会导致姿态角漂移。

目前国内很多的开源飞控都是用Mahony滤波器来修正姿态角，其优点是运算量小，易于理解。但Mahony滤波器是线性的，通过PID跟随器来让陀螺仪积分角度跟随加速度计计算的角度。

在这里，我介绍一种基于扩展卡尔曼滤波器的姿态修正方法，我开源的飞控软件也利用此方式来估计姿态角。

这种卡尔曼滤波器我在嵌入式平台（平台芯片是STM32F103，主频72MHz）测试过，精度比Mahony高不少，并且计算量适中，进行一次解算大约5.5ms，一般芯片也能适应，特别是在IMU存在大量噪声的时候效果也非常好。

基于扩展卡尔曼滤波器的姿态修正

先回忆一下扩展卡尔曼滤波器的五个方程组：

扩展卡尔曼滤波器的5个方程组

建立状态量x，总共7维，分别是4个四元数，3个角速度漂移：

状态量

建立量测量z，总共4维，分别是3个比力，1个磁力计角度：

量测量

状态转移矩阵F是f对x的雅各比矩阵，f在这里是四元数的微分方程，x是状态量。

过程噪声矩阵Q是陀螺仪的三个轴的协方差矩阵，由于陀螺仪三个轴无相关性，所以过程噪声矩阵是个3阶对角矩阵。

噪声变换矩阵L是7*3阶矩阵，是f对w的雅各比矩阵，w是角速度。

量测变换矩阵H是z对x的雅各比矩阵。

在陀螺仪有噪声的情况下静止测试：

在芯片上运行的结果

角速度wy的噪声幅度在±7度每秒左右，姿态角在±0.3之内变化。

在陀螺仪有噪声的情况下运动测试：

芯片上运行结果

可以看出，姿态角反应快速，解算正常。

项目已开源，代码托管在

本代码在 Fs-Flight/SRC/NAVI/ahrs_advance.c中实现

下面的链接可以直接访问项目。