APM-3.5.2-EKF2笔记（未完待续）

20180704：
一、EKF2.InitialiseFilter()全过程
1. 记录开始时间，预期步长时间(_frameTimeUsec = 2500)，每次融合读取IMU次数(_framesPerPrediction = 4)，确定是否记日志（通过参数，默认不记录）
2. 根据加速度计数量确定启用EKF滤波器个数（num_cores），并创建滤波器
3. 初始化所有滤波器基本环境
   a.传入前端指针
   b.设置IMU索引
   c.设置滤波器索引
   d.传入ahrs指针
   e.初始化相应传感器的缓冲区（分配内存）
4. 所有滤波器基本环境初始化成功后，设置主滤波器索引为0
5. 初始化所有滤波器实例（输入输出变量）
   a.固定翼机型等待GPS 3D定位后开始初始化滤波器
   b.初始化能够重用的一大堆参数
   c.初始化IMU（主循环间隔、读取IMU数据、填充IMU缓冲区）
   d.读取加速度计、磁罗盘等数据
   e.利用加速度计的读值，初始化横滚角和俯仰角
   f.初始化扩展卡尔曼滤波器输出的24个状态（stateStruct）
   g.读取GPS数据，重置速度、位置信息
   h.读取气压计数据，重置高度信息
   i.根据纬度计算地球的自旋矢量
   j.初始化协方差矩阵
   k.重置输出状态
   l.置位滤波器初始化完成标志
6. 清零三个结构体（yaw_reset_data、pos_reset_data、pos_down_reset_data），尚不清楚这三个结构体的用途
7. 检查是否需要记录特定传感器的日志信息
二、EKF2.UpdateFilter()全过程
1. 记录滤波器开始时间（imuSampleTime_us）
2. 运行所有的滤波器(core[i].UpdateFilter(statePredictEnabled[i])，有个负载均衡的算法，目前还不清楚运行机制)
   a.设置startPredictEnabled的值，1：读取IMU数据4次后，允许卡尔曼估计（runUpdates = 1），0：读取IMU数据8次后，允许卡尔曼估计（runUpdates = 1）
   b.获取滤波器开始时间（imuSampleTime_ms）
   c.检查加解锁状态并执行所需的检查和滤波器模式转换
   d.读取IMU数据（3轴角度增量和3轴速度增量）
   e.如果缓冲区中有新的IMU数据，则运行EKF方程（runUpdates = 1）

附录
1. EKF2协方差矩阵（P）初值
    // attitude error
    P[0][0]   = 0.1f;
    P[1][1]   = 0.1f;
    P[2][2]   = 0.1f;
    // velocities
    P[3][3]   = 0.5*0.5;
    P[4][4]   = 0.5*0.5;
    P[5][5]   = 0.7*0.7;
    // positions
    P[6][6]   = 1.0*1.0;
    P[7][7]   = 1.0*1.0;
    P[8][8]   = 3.0*3.0;
    // gyro delta angle biases
    P[9][9] = radians(2.5 * 0.0025)*radians(2.5 * 0.0025);
    P[10][10] = radians(2.5 * 0.0025)*radians(2.5 * 0.0025);
    P[11][11] = radians(2.5 * 0.0025)*radians(2.5 * 0.0025);
    // gyro scale factor biases
    P[12][12] = 0.001*0.001;
    P[13][13] = 0.001*0.001;
    P[14][14] = 0.001*0.001;
    // Z delta velocity bias
    P[15][15] = (0.5 * 0.0025) * (0.5 * 0.0025);
    // earth magnetic field
    P[16][16] = 0.0f;
    P[17][17] = 0.0f;
    P[18][18] = 0.0f;
    // body magnetic field
    P[19][19] = 0.0f;
    P[20][20] = 0.0f;
    P[21][21] = 0.0f;
    // wind velocities
    P[22][22] = 0.0f;
    P[23][23] = 0.0f;

    // optical flow ground height covariance
    Popt = 0.25f;

注：radians(),将角度转换为弧度。