使用EKF算法处理IMU数据

研一已经开学了，在大四学习四旋翼的时候，深刻意识到自己的数学功底有多差，所以接下来不再接着搞四旋翼了，要专心学习了，不过南京的小伙伴想约飞可以带上我哦，不飞手痒。因为不忍心把自己的博客断了，所以想直接把最重要的部分分享出来，希望对搞四旋翼研究的人有所帮助。

在实现四旋翼自稳功能的时候，最重要的就是两点，一个是PID，另一个就是IMU数据处理。我本身不是自控专业，自己对PID的理解也是从胡寿松的《自动控制原理》这本书上看来的，有需要的可以参考。然后这里我想讲一下IMU数据处理。

关于IMU数据处理网络上已经有很多开源的代码，但是学习四旋翼绝对不仅仅是为了让飞机能飞起来，而是要搞懂其中的数学原理，为进一步的开发做准备。我这里想讲的EKF算法，大家可以参考清华大学《信号检测与估计》这本书。然后具体EKF算法是怎样应用到IMU数据处理上来的，可以参考这篇论文《A Double-Stage Kalman Filter for Orientation Tracking with an Integrated Processor in 9-D IMU》，这篇论文我简单翻译了一下，但是我想愿意读这篇博客的人应该都愿意花时间去啃这篇论文吧。在看这篇论文之前，还要看完北航可靠飞行控制组（http://rfly.buaa.edu.cn/resources.html）前5章的PPT。

下面开始步入正题，贴代码：

 

clear all;close all;
load('pix_log.mat');
ATT_T=0.1;  %每100ms执行一次ATT数据的log
IMU_T=0.04; %每40ms执行一次IMU数据的log
hTime =