开源飞控OpenPilot的扩展卡尔曼滤波EKF学习笔记（一）

OpenPilot是老外的一款开源飞控，不同于大名鼎鼎的APM，OpenPilot在国内基本没人玩，不过这并不妨碍其成为一款优秀的飞控系统。OpenPilot的设计理念是高度集成，它把电台集成到飞控板上，而包含了电台的飞控板还是比APM小。这里我要记录的是其中关于其EKF部分的代码解读。

OpenPilot使用的陀螺仪是mpu6000，包括陀螺仪和加速度计，磁罗盘是hmc5883，气压高度计是ms5611，GPS是LEA6H，EKF算法正是基于这几个传感器的数据。

代码文件位于/OpenPilot/flight/modules/StateEstimation/filterekf.c。

首先看注意maininit(stateFilter *self)函数，这是滤波器的初始化函数，作用是确定滤波器的P，Q，R三个矩阵的具体数值，以及获取home点的经纬度，下面看具体代码。

    struct data *this = (struct data *)self->localdata;
    this->inited       = false;
    this->init_stage   = 0;
    this->work.updated = 0;

前面这几行是定义一些数据结构，赋值一些标志位什么的，属于程序架构部分，和算法关系不大，往后看。

PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA);

这个函数是将后面几个参数填充到第一个参数所指的指针里，this->dtconfig是滤波器关于采样时间dt的数据结构，其中DT