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(),将角度转换为弧度。