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(),將角度轉換為弧度。