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(),將角度轉換為弧度。
寒假訓練 2筆記
方法一 for i 1 i n為被分解的的數 emmm超時了，明天在更進。1.輸入多個字串時 while gets s 這樣既可 while gets s eof 會報錯！2.回文質數 洛谷和vjudge都做過 ps 乙個數既是回文數又是素數 原先的想法 寫兩個函式，乙個判斷素數，乙個判斷回文，超時...
ROS2 筆記0 安裝
作業系統 ubuntu 18.04 確保系統的local支援utf 8。進行如下的命令設定 sudo locale gen en us en us.utf 8 sudo update locale lc all en us.utf 8 lang en us.utf 8 export lang en ...
Python2筆記（九） 繼承
coding utf 8 class person object def init self,name,age self.name name self.age age class student person def init self,name,age,score super student,se...