# IMU 101 --- ### What is IMU? ---- - IMU ＝ inertia measurement unit - 3-axis accelerometer＋3-axis gyroscopes <img src="https://i.imgur.com/uRxtdgW.png" width="500"> <font style="font-size: 10px">source: https://www.ceva-dsp.com/ourblog/what-is-an-imu-sensor/ </font> ---- --- ## 為什麼需要濾波？ ---- 加速度計會產生高頻振蕩的噪音訊號，又陀螺儀是將角速度對時間積分產生角度，故**易產生誤差**。  <font style="font-size: 10px;">https://stackoverflow.com/questions/1586658/combine-gyroscope-and-accelerometer-data</font> --- ## 互補濾波 ### Complementary filter ---- - 加速度計計算瞬時傾角誤差比較大 - 陀螺儀積分結果易受長時間影響 ⇓ **定時**對加速度採樣的角度取平均值 + **短時間**內採用陀螺儀得到的角度 ---- 加速度計要濾掉高頻訊號，陀螺儀要濾掉低頻訊號  <font style="font-size: 10px;">https://stackoverflow.com/questions/1586658/combine-gyroscope-and-accelerometer-data</font> ---- $$ angle' = \alpha(angle + gyro * dt) + (1-\alpha)*(X_a) $$ * $angle'$為得到實際角度，$angle$為前一時刻角度 * $gyro$為陀螺儀值(角速度)，$dt$為計算週期 * $X_A$為加速度數據換算後的角度值 * $\alpha = 0.98$ --- ## 卡爾曼濾波 ### Kalman filter ---- - 線性 - 純時域 - 高斯雜訊 ---- ### 基本模型-真實狀態 $$ x_k = Fx_{k-1}+Bu_k+w_k \\ w_k \sim N(0,\ Q_k) $$ - $x_k$ : $k$ 時刻狀態 - $F$ : 狀態變換矩陣 - $u_k$ : 控制向量 - $B$ : 控制輸入模型 - $w_k$ : 過程雜訊 - $Q_k$ : $w_k$ 的共變異數矩陣 ---- ### 基本模型-觀測 $$ z_k = Hx_k + v_k \\ v_k \sim N(0, R_k) $$ - $z_k$ : $k$ 時刻測量到的狀態 - $H$ : 觀測模型 - $v_k$ : 觀測誤差 - $R_k$ : $v_k$ 的共變異數矩陣 ---- ### 預測 $$ \hat{x}^{-}_{k} = F\hat{x}_{k-1}+B\dot{\theta}_{k} \\ P^-_k = FP_{k-1}F^T+Q $$ - $F= \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix} \ \hat{x}_k = \begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_k \ B= \begin{bmatrix} \Delta t \\ 0 \end{bmatrix}$ - $P_k$ : $x_k$ 的誤差共變異數矩陣 - $P_k = E[(x_k - E[x_k])(x_k - E[x_k])^T]$ - $cov(AX+B,\ AX+B) = var(AX+B)\\ = A\ var(X)\ A^T$ ---- ### 修正 $$ K_k = P_k^-H^T(HP^-_kH^T+R)^{-1} \\ \hat{x}_k = \hat{x}^-_k + K_k(z_k-H\hat{x}_k^-) \\ P_k = (I-K_kH)P^-_k $$ - $K_k$ : 卡爾曼增益 (最小化$P_k$) - $H = \begin{bmatrix} 1 & 0 \end{bmatrix}$ ---- <div style="background-color: #FFFFFF; padding-top: 50px; padding-bottom: 50px;"> ```flow st=>start: Start predict1=>operation: Predict state predict2=>operation: Predict error covariance correct3=>operation: Update error covariance correct2=>operation: Update estimate correct1=>operation: Compute Kalman gain st->predict1->predict2(right)->correct1(bottom)->correct2(bottom)->correct3(right)->predict1 ```  </div> --- ## 實作 ---- ### 裝置&流程 <img src="https://i.imgur.com/edJJ7RM.png" width="650"> ---- <img src="https://i.imgur.com/px2mxKe.jpg" height="650"> ---- ### 測試結果 Part 1  ---- ### 測試結果 Part 2 | Pitch | Row | | -------- | -------- | |  |  | --- ## Reference <font style="font-size: 13px;"><li style="font-size: 13px;">[Welch, G. and Bishop, G. (2006). *An Introduction to the Kalman Filter*, Department of Computer Science University of North Carolina at Chapel Hill](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) - [Lauszus. (2012). *A practical approach to Kalman filter and how to implement it*, TKJ Electronics](http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/) </font> --- ## Q&A