VIO學習(1)——IMU-Model

寫在前面

這次來總結一下IMU的一些東西，主要包含：

• 測量模型
• 運動模型

IMU的測量模型

通常，我們將IMU的測量模型寫作如下模型（注意不是觀測模型）：
$w_m = w_t+b_w+n_w \\ a_m = a_t+b_a+n_a$
亦即：“測量值=真值+零偏+高斯白噪聲”，其中：

1. 零偏值是我們希望實時進行估計的，會在狀態變量中進行估計；
2. 零偏值通常被建模爲隨機遊走過程；

高斯白噪聲

高斯白噪聲其實是我們常見的噪聲模型，一個典型的高斯白噪聲滿足：
$\begin{array}{c}{E[n(t)]=0} \\ {E\left[n\left(t_{1}\right) n\left(t_{2}\right)\right]=\sigma_{g}^{2} \delta\left(t_{1}-t_{2}\right)}\end{array}$
如果一個傳感器的噪聲僅僅包含高斯白噪聲（或者白噪聲）的話，那麼基本上，使用平均濾波是能得到較好的有效值的，因爲白噪聲的期望值是0；

下一步把連續過程離散化，可以比較直觀的得到：$n_d(k) = \sigma_{gd}w(k)$，其中$\sigma_d$是高斯白噪聲的方差，而$w(k)$爲標準的正態分佈。

值得說明的一點是：$\sigma_{gd}$與連續空間的方差並不相同，而是等於$\sigma_g/\sqrt{\Delta t}$，直接去想的話，可以認爲因爲離散空間的積分與時間間隔相關，每次積分都要乘以$\Delta t$，所以最終的方差多了一個$\Delta t$，所以要除下來；這裏還是給出數學上的推導：
$\begin{aligned} n_{d}[k] & \triangleq n\left(t_{0}+\Delta t\right) \simeq \frac{1}{\Delta t} \int_{t_{0}}^{t_{0}+\Delta t} n(\tau) d t \\ E\left(n_{d}[k]^{2}\right) &=E\left(\frac{1}{\Delta t^{2}} \int_{t_{0}}^{t_{0}+\Delta t} \int_{t_{0}}^{t_{0}+\Delta t} n(\tau) n(t) d \tau d t\right) \\ &=E\left(\frac{\sigma_{g}^{2}}{\Delta t^{2}} \int_{t_{0}}^{t_{0}+\Delta t} \int_{t_{0}}^{t_{0}+\Delta t} \delta(t-\tau) d \tau d t\right) \\ &=E\left(\frac{\sigma_{g}^{2}}{\Delta t}\right) \end{aligned}$
一開始我比較不理解第一行的公式，但是後來仔細想想，其實一個時刻的高斯噪聲值可以看做是某個時間段內所有值的平均。需要注意的是，高斯白噪聲由概率密度描述的，在Kalibr的配置文件中，需要輸入加計和陀螺儀的”噪聲密度“值，對應$\sigma_{gd}$。

Bias的隨機遊走

隨機遊走過程本質上是離散的，大家可以想象一些折線圖，每個拐點就是那一時刻的值；其在連續空間被建模爲一個維納過程，也就是導數爲高斯白噪聲的過程：
$\dot{b_g}=n(t)$
根據上面得到的高斯白噪聲模型，隨機遊走的離散模型就比較簡單了：
$\begin{aligned} b_g(k+1) &=b_g(k)+\sigma_{gd}w(t)\Delta{t} \\ &=b_g(k)+\sigma_{g}\sqrt{\Delta t} w(t) \end{aligned}$
在Kalibr的配置文件中，需要輸入加計和陀螺儀的隨機遊走的方差值，也就是上面的$\sigma_g\sqrt{\Delta t}$。

IMU運動模型

說起運動模型，首先就要確立狀態變量，IMU的狀態變量比較多，爲如下形式：
$X=[^WP_I^T, ^WV_I^T, {q_I^W}^T, b_a^T, b_w^T]^T$
除了正常的位姿之外，IMU的模型中添加了速度和零偏的估計值，對應的運動模型爲：
$\left\{\begin{array}{l}{^W\dot{p}=^Wv} \\ {^W\dot{v}=R_{I}^{W}\left(^Ia_{m}-b_{a}-n_{a}\right)} \\ {\dot{q}_I^W=q_I^W \otimes q\left(^Iw_{m}-b_{w}-n_{w}\right)} \\ {\dot{b}_{a}=n_{ad}} \\ {\dot{b}_{w}=n_{wd}}\end{array}\right.$
注意上式的$p, v$都在參考座標系W下，而旋轉使用四元數表示，關於四元數更多的內容，可以查看here

reference

https://fzheng.me/2016/11/20/imu_model_eq/

https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model