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