IMU 101

2022-12-10

/ Project

# STM32 # Python # C

IMU 101

What is IMU?

IMU ＝ inertia measurement unit
3-axis accelerometer＋3-axis gyroscopes

source: https://www.ceva-dsp.com/ourblog/what-is-an-imu-sensor/

為什麼需要濾波？

加速度計會產生高頻振蕩的噪音訊號，又陀螺儀是將角速度對時間積分產生角度，故易產生誤差。

source: https://stackoverflow.com/questions/1586658/combine-gyroscope-and-accelerometer-data

互補濾波

Complementary filter

加速度計計算瞬時傾角誤差比較大
陀螺儀積分結果易受長時間影響

定時對加速度採樣的角度取平均值 + 短時間內採用陀螺儀得到的角度

加速度計要濾掉高頻訊號，陀螺儀要濾掉低頻訊號

https://stackoverflow.com/questions/1586658/combine-gyroscope-and-accelerometer-data

$angle’ = α (angle + gyro \times d t) + (1 - α) \times (X_{a})$

$gyro$ 為陀螺儀值(角速度)， $d t$ 為計算週期
$X_{A}$ 為加速度數據換算後的角度值
$α = 0.98$

卡爾曼濾波

Kalman filter

線性
純時域
高斯雜訊

基本模型-真實狀態

$\begin{matrix} x_{k} = F x_{k - 1} + B u_{k} + w_{k} \\ w_{k} \sim N (0, Q_{k}) \end{matrix}$

$x_{k}$ : $k$ 時刻狀態
$F$ : 狀態變換矩陣
$u_{k}$ : 控制向量
$B$ : 控制輸入模型
$w_{k}$ : 過程雜訊
$Q_{k}$ : $w_{k}$ 的共變異數矩陣

基本模型-觀測

$\begin{matrix} z_{k} = H x_{k} + v_{k} \\ v_{k} \sim N (0, R_{k}) \end{matrix}$

$z_{k}$ : $k$ 時刻測量到的狀態
$H$ : 觀測模型
$v_{k}$ : 觀測誤差
$R_{k}$ : $v_{k}$ 的共變異數矩陣

預測

$\begin{matrix} {\hat{x}}_{k}^{-} = F {\hat{x}}_{k - 1} + B {\dot{θ}}_{k} \end{matrix}$

$F = [\begin{matrix} 1 & - Δ t \\ 0 & 1 \end{matrix}] {\hat{x}}_{k} = {[\begin{matrix} θ \\ {\dot{θ}}_{b} \end{matrix}]}_{k} B = [\begin{matrix} Δ t \\ 0 \end{matrix}]$
$P_{k}$ : $x_{k}$ 的誤差共變異數矩陣
- $P_{k} = E [(x_{k} - E [x_{k}]) (x_{k} - E [x_{k}])^{T}]$
- $c o v (A X + B, A X + B) = v a r (A X + B) = A v a r (X) A^{T}$

修正

$\begin{matrix} K_{k} = P_{k}^{-} H^{T} (H P_{k}^{-} H^{T} + R)^{- 1} \\ {\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - H {\hat{x}}_{k}^{-}) \\ P_{k} = (I - K_{k} H) P_{k}^{-} \end{matrix}$

$K_{k}$ : 卡爾曼增益 (最小化 $P_{k}$ )
$H = [\begin{matrix} 1 & 0 \end{matrix}]$