算法總結
- 低通濾波
- 羅格里德角表示
- 四元數
- 方向餘弦矩陣
- 向量外積
- PI控制器
- 一階龍格庫塔法
- 歸一化
- 歐拉角
低通濾波
高頻噪聲的信號過濾得到準確的數據，加速度計高頻噪聲較大。
羅格里德
用該表示方法，最後轉化成sin cos角的形式，得到四元數的表示方法
四元數
- 單純使用方向餘弦矩陣轉換存在函數Y與自變量相同的問題
- 簡化數學運算，防止處理器變慢
方向餘弦矩陣
- 座標系轉換：可由二維的轉換左乘表示，x軸固定，y軸固定，z軸固定
- 任何三維座標的變換都可以旋轉三次得到。
向量外積
- 外積的表達式 |a||b|sin<a,b>，當a和b的角度小於40°時，sin<a,b>的值和a、b的夾角基本相等，可以近似。
PI控制器
- KP比例，KI積分
- KP越大，值越大，由加速度計得到的值累加到陀螺儀數據上，所以越相信加速度計。
一階龍格庫塔法
- 時變微分方程的計算機方便計算的方法
歸一化
- 去除數值，保留方向
- 方便計算
- 外積可以直接作爲角度
歐拉角
由方向餘弦矩陣反解得到歐拉角