IMU 是移動機器人、移動智能設備上常見的傳感器。常見的 IMU 為六軸傳感器，配備輸出三軸加速度的加速度計和輸出三軸角速度的陀螺儀。九軸 IMU 還會配備輸出三軸姿態角的磁力計。我們這里只討論六軸 IMU。
IMU 的狀態量通常表示為：
\[{\bf X}_{IMU} = [ ^I_G \bar{q}^T \quad {\bf b}_g^T \quad ^G{\bf v}_I^T \quad {\bf b}_a^T \quad ^G{\bf p}_I^T] \tag{0.0}\]
這里我們使用和 MSCKF [1] 一樣的 notation。用 {I} 表示 IMU 坐標系，{G} 表示參考坐標系。IMU 的姿態由旋轉量 $^I_G \bar{q}$ 和平移量 $^G{\bf p}_I$ 表示。更具體來說，前者為將任意向量從 {G} 坐標映射到 {I} 坐標的旋轉量，用單位四元數表示；后者為 IMU 在 {G} 下的三維位置。$^G{\bf v}_I$ 表示 IMU 在 {G} 下的平移速度。另外兩個量 ${\bf b}_g$ 和 ${\bf b}_a$ 表示陀螺儀（角速度計）和加速度計的 bias。可以注意一下這里除了 bias 之外的狀態量的時間維度：平移量表達到速度（p 和 v，對時間的一階導），因為 IMU 只提供到加速度（對時間的二階導）的測量；旋轉量只表達姿態量（對時間的零階導），因為 IMU 提供到角速度（對時間的一階導）。狀態量的估計可以由 IMU 測量積分得到。
對於 IMU 狀態估計問題，需要提供運動模型、觀測（噪聲）模型、估計誤差模型：
\[\dot{\bf x} = f({\bf x}) \tag{0.1}\] \[{\bf z} = g({\bf x}) + {\bf n} \tag{0.2}\] \[\delta {\bf x} = e(\hat{\bf x},{\bf x}) \tag{0.3}\]
這是一個通用模型，我們用 $\bf x$ 表示真實狀態量（待估計，不可知），用 ${\bf z}$ 表示觀測量，${\bf n}$ 表示觀測噪聲，$\hat{\bf x}$ 表示當前的狀態估計量。這篇小文主要講 IMU （即 ${\bf x} := {\bf X}_{IMU}$ 時）這三個模型的推導。
1. IMU 運動模型
1-1. 前置(1)： 旋轉量求導
這部分講剛體動力學相關的前置知識，熟悉的讀者可以跳過。
眾所周知，一個剛體在同一個慣性坐標系下進行平移運動，其平移量對時間的一階導和二階導即速度和加速度：
\[\dot{\bf p}={\bf v}, \dot{\bf v} = {\bf a}\]
對於旋轉量以及非慣性系參考坐標系，情況稍微復雜些。
首先，如下圖（左）所示，考慮一個從原點出發的向量 $\bf r$ 繞單位軸 $\bf u$ 旋轉，角速度大小為 $\dot{\theta}$。
角速度矢量可以表示為 ${\boldsymbol \omega}=\dot{\theta}\bf u$。易得向量 $\bf r$ 末端點 P 的速度矢量，即 $\bf r$的時間一階導為
\[\frac{d{\bf r}}{dt} = {\boldsymbol \omega} \times {\bf r}\]
現在考慮上圖（右），坐標系 {B} 繞單位軸 $\bf u$ 旋轉，如上所述，其三個軸的時間一階導同樣為
\[\frac{d{\bf i}_B}{dt} = {\boldsymbol \omega} \times {\bf i}_B, \frac{d{\bf j}_B}{dt} = {\boldsymbol \omega} \times {\bf j}_B, \frac{d{\bf k}_B}{dt} = {\boldsymbol \omega} \times {\bf k}_B\]
我們知道，$ [ {\bf i}_B \quad {\bf j}_B \quad {\bf k}_B ]$ 實際上就是坐標系 {B} 相對於參考坐標系的旋轉矩陣 $\bf R$。所以 $\bf R$ 的時間一階導為
\[\dot{\bf R} = [ {\boldsymbol \omega} \times {\bf i}_B \quad {\boldsymbol \omega} \times {\bf j}_B \quad {\boldsymbol \omega} \times {\bf k}_B ] = {\boldsymbol \omega} \times {\bf R} \tag{1.0}\]
我們知道上面的叉乘運算可以轉化為負對稱矩陣的乘法：
\[\dot{\bf R} = {\boldsymbol \omega}^{\land} {\bf R} \tag{1.1}\]
其中負對稱矩陣為
\[\quad {\boldsymbol \omega}^{\land}= \begin{bmatrix}0 & -\omega_3 & \omega_2\\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0\end{bmatrix}\]
注意這里的角速度 ${\boldsymbol \omega}$ 是在參考坐標系下表達的。角速度也經常表達在體坐標系 {B} 下，記為 ${}^B{\boldsymbol \omega} = {\bf R}^T{\boldsymbol \omega}$，即 ${\boldsymbol \omega} = {\bf R}{}^B{\boldsymbol \omega}$，於是 $(1.1)$ 可以寫作
\[\dot{\bf R} = ({\bf R}{}^B{\boldsymbol \omega})^{\land} {\bf R} \tag{1.2}\]
這里我們要利用負對稱矩陣的一個很好的性質：對任意旋轉矩陣 $\bf R$ 和三維向量 $\bf v$，都有 $({\bf R v})^{\land} = {\bf Rv^{\land}R}^T$（參看《(Rv)^ = Rv^R’ 的簡單證明》），於是 $(1.2)$ 可以寫成
\[\dot{\bf R} = {\bf R}({}^B{\boldsymbol \omega})^{\land} \tag{1.3}\]
比較一下 $(1.1)$ 和 $(1.3)$，可以發現一個很有趣的事實，角速度如果表達在參考坐標系下，負對稱矩陣寫在左邊；如果表達在體坐標系下，負對稱矩陣寫在右邊。這點微小的區別，讀者在閱讀文獻時可以特別留意。
1-2. 前置(2)： 四元數
這部分講四元數如何表示旋轉的前置知識，熟悉的讀者可以跳過。
用旋轉矩陣來表示旋轉很直觀，但過於冗余，因為旋轉只有三個自由度，而旋轉矩陣有九個量。表征旋轉還可以用歐拉角，但有萬向鎖問題，而且計算也不方便。旋轉向量（即李代數 so(3)）和四元數是更常用的表征方法，在慣性導航中四元數似乎更普遍些。這里采用四元數。
一個四元數由一個實部和三個虛部構成，書寫順序各家不同，這里和 MSCKF [1] 一樣，虛部在前實部在后：
\[{\bf q} = q_1 i + q_2 j + q_3 k + q_4 = [{\boldsymbol v}^T \quad q_4]^T\]
虛部 ${\boldsymbol v}=[q_1 \quad q_2 \quad q_3]^T$。虛部三個基 $i,j,k$ 滿足 $i^2=j^2=k^2=ijk=-1$。 四元數仍是一種冗余表達法，為了更緊湊，通常使用使用單元四元數 $\bar{\bf q}$，通過將四元數的模直為 1 得到。
四元數和旋轉向量有很直接的轉換關系。繞單位軸 $\bf u$ 轉了 $\theta$ 角度，用四元數表達為
\[{\bf q} = [{\bf u}\sin\frac{\theta}{2} \quad \cos\frac{\theta}{2}] \tag{1.4}\]
四元數乘法 $\otimes$ 為類似於多項式乘法的逐項相乘：
\[\begin{aligned} {\bf q}\otimes {\bf p} = & (q_1 i + q_2 j + q_3 k + q_4)(p_1 i+p_2j+p_3k+p_4) \\ = & (q_1p_4+q_2p_3-q_3p_2+q_4p_1)i+(-q_1p_3+q_2p_4+q_3p_1+q_4p_2)j+\\ &(q_1p_2-q_2p_1+q_3p_4+q_4p_3)k + (-q_1p_1-q_2p_2-q_3p_3+q_4p_4) \end{aligned}\]
這個計算結果可以表達為多種形式：
\[\begin{aligned} {\bf q}\otimes {\bf p} & = \begin{bmatrix} q_4{\bf I}_3+{\boldsymbol v}_q^{\land} & {\boldsymbol v}_q \\ -{\boldsymbol v}_q^T & q_4 \end{bmatrix} \begin{bmatrix} {\boldsymbol v}_p \\ p_4 \end{bmatrix} \\ & = \begin{bmatrix} p_4{\bf I}_3-{\boldsymbol v}_p^{\land} & {\boldsymbol v}_p \\ -{\boldsymbol v}_p^T & p_4 \end{bmatrix} \begin{bmatrix} {\boldsymbol v}_q \\ q_4 \end{bmatrix} \end{aligned}\]
四元數乘法和其對應的兩個旋轉矩陣相乘物理意義是一樣的，即 ${\bf R}({\bf q\otimes p})={\bf R}({\bf q}){\bf R}({\bf p})$。四元數對應的旋轉矩陣為：
\[{\bf R}({\bf q}) = (2q_4^2-1){\bf I}_3 +2q_4{\boldsymbol v}^{\land} + 2{\boldsymbol{vv}}^T \tag{1.5}\]
四元數的逆為 ${\bf q}^{-1} = [-{\boldsymbol v}^T \quad q_4]^T$。易得 ${\bf q}\otimes{\bf q}^{-1}=[0\quad 0\quad 0\quad 1]^T:={\bf q}_I$，故 ${\bf q}_I$ 表示旋轉量為零。
四元數對時間一階導為
\[\begin{aligned} \dot{\bf q} &= \frac{1}{2} \begin{bmatrix} {\boldsymbol \omega}^{\land} & {\boldsymbol\omega} \\ -{\boldsymbol \omega}^T & 0 \end{bmatrix} {\bf q} := \frac{1}{2}{\boldsymbol \Omega}({\boldsymbol \omega}){\bf q} \tag{1.6} \\ &= \frac{1}{2} \begin{bmatrix} q_4{\bf I}_3-{\boldsymbol v}^{\land} \\ -{\boldsymbol v}^T \end{bmatrix} {\boldsymbol \omega} \end{aligned}\]
讀者可能注意到了 $(1.6)$ 和 $(1.1)$ 形式上的相似。這里 $\boldsymbol \omega$ 的意義也是一樣的。$(1.6)$ 的推導可以參考 [2]，這里不贅述。
1-3. IMU 運動模型
有了前置知識的鋪墊之后，我們可以給出 IMU 的運動模型：
\[\begin{aligned} {}^I_G \dot{\bar{q}} &= \frac{1}{2}{\boldsymbol \Omega}({\boldsymbol \omega}){}^I_G \bar{q}\\ \dot{\bf b}_g &= {\bf n}_{wg}\\ {}^G\dot{\bf v}_I &= {}^G{\bf a} \tag{1.7}\\ \dot{\bf b}_a &= {\bf n}_{wa} \\ {}^G\dot{\bf p}_I &= {}^G{\bf v}_I \end{aligned}\]
${}^I_G \dot{\bar{q}}$ 由 $(1.6)$ 直接得到。注意這里角速度 $\boldsymbol \omega$ 是在體坐標系 {I} 下表達的，與 $(1.1)$ 處相反。原因是 ${}^I_G \bar{q}$ 表示的旋轉方向與 $(1.1)$ 處的 $\bf R$ 是相反的。其他的四項，速度和加速度都很簡單，bias 兩項在下面觀測模型部分講。
2. IMU 觀測和噪聲模型
2-1 前置(1)： 科氏加速度
這部分在 1-1 的基礎上，討論參考坐標系不是慣性系的情況，熟悉科氏加速度的讀者可以跳過。我們仍利用 1-1 中的圖，但這次把繞慣性系 {A} 中固定單位軸 $\bf u$ 旋轉的 {B} 作為參考坐標系。考慮下圖， 點 P 相對於 {B} 運動，記 $^B{\bf r}$ 分別為 P 在 {B} 下的坐標，$\bf r$ 為 P 的絕對坐標（即 {A} 下坐標）， $\bf R$ 仍為 {B} 相對於 {A} 的旋轉矩陣，易知 $ {\bf r}={\bf R}^B{\bf r}$。
求一階時間導，並利用公式 $(1.1)$：
\[{\bf v} = \dot{\bf r} = \dot{\bf R} {}^B{\bf r} + {\bf R}^B\dot{\bf r} = {\boldsymbol \omega}^{\land}{\bf R}{}^B{\bf r}+ {\bf R}^B\dot{\bf r}\]
記 P 在 {B} 下速度為 $^B\bf v$，於是
\[\begin{aligned} {\bf v} & = {\boldsymbol \omega}^{\land}{\bf r} + {\bf R}^B{\bf v} \\ & = {\boldsymbol \omega}\times{\bf r}+ {\bf v}_r \tag{2.0} \end{aligned}\]
請注意，這里用 ${\bf v}_r$ 來表達「相對速度」的概念，准確定義為 P 相對於 {B} 的速度，在慣性系 {A} 下的表達。請分清 ${\bf v}_r$、$\bf v$ 以及 $^B\bf v$ 三者之間的區別和聯系。
再對 $(2.0)$ 求時間導：
\[\begin{aligned} {\bf a} = \dot{\bf v} & = \dot{\boldsymbol \omega}\times {\bf r} + {\boldsymbol \omega} \times \dot{\bf r}+ \dot{\bf R} {}^B{\bf v} + {\bf R}{}^B\dot{\bf v} \\ & = {\boldsymbol \alpha}\times {\bf r}+{\boldsymbol \omega}\times（{\boldsymbol \omega}\times{\bf r}+ {\bf v}_r）+ {\boldsymbol \omega}\times{\bf R}{}^B{\bf v}+{\bf R}{}^B{\bf a} \\ & = {\boldsymbol \alpha}\times {\bf r}+{\boldsymbol \omega}\times({\boldsymbol \omega}\times{\bf r})+2{\boldsymbol \omega}\times{\bf v}_r+{\bf a}_r \tag{2.1} \end{aligned}\]
我們來逐項分析上面這個式子。第一項中 ${\boldsymbol \alpha}$ 為 {B} 的角加速度，所以第一項的物理意義是 {B} 旋轉所造成的 P 的切向加速度。第二項是 {B} 旋轉所造成的向心加速度。第四項為 P 相對於 {B} 的加速度，但在慣性系 {A} 下表達——類似於 ${\bf v}_r$，定義相對加速度 ${\bf a}_r$。第三項比較特殊，為 {B} 的旋轉運動與 P 相對 {B} 的平移運動耦合產生的加速度，稱為「科氏加速度」。可以看到，除了第四項外，另外三項都和 {B} 的旋轉有關。
2-2 前置(2)： 慣性導航相關坐標系定義
這部分講慣性導航中經常出現的幾個坐標系的定義 [5]。
Earth-Centered-Earth-Fixed (ECEF) Frame：地心地固坐標系 ECEF。以地心為坐標原點，向北為 z 軸，x-y 平面為赤道平面，x 軸指向經緯度 (0,0) 點。ECI 固連在地球上，跟隨地球自轉，非慣性坐標系。MSCKF 一代 [1] 使用 ECEF 為參考坐標系 {G}。
Earth-Centered-Inertial (ECI) Frame：地心慣性坐標系 ECI。以地心為坐標原點，向北為 z 軸，x-y 平面為赤道平面，x 軸指向春分點（vernal equinox point，即每年春分時日心-地心連線與赤道的交點）。ECI 不跟隨地球自轉，在慣性導航中視為慣性坐標系。MSCKF 二代 [3] 使用 ECI 為參考坐標系 {G}。
Body Frame：體坐標系。原點在導航體的質心，固連在導航體上，用來表示導航體的姿態。在本文前置推導部分為 {B}，在 MSCKF 中為 {I}。
2-3 前置(3)： 高斯白噪聲與隨機游走
這部分講高斯白噪聲和隨機游走(random walk)模型，及其離散化。這部分在kalibr 庫中的 IMU noise model [4] 有簡單的介紹，這里在其基礎上添加了離散化的推導，因為離散化中部分內容還是有些令人疑惑的。離散化的推導部分參考自 [5]。
先講高斯白噪聲。一個連續時間的高斯白噪聲 $n(t)$，滿足以下兩個條件
\[E[n(t)]=0 \\ E[n(t_1)n(t_2)] = \sigma_g^2 \delta(t_1-t_2)\]
其中 $\delta()$ 表示狄拉克函數。可以看出，不同時刻的高斯白噪聲相互獨立。$\sigma_g^2$ 為方差，值越大，表示噪聲程度越大。
將高斯白噪聲離散化，可得到：
\[n_d[k]=\sigma_{gd} w[k]\]
其中
\[w[k] \sim \mathcal{N}(0,1) \\ \sigma_{gd}=\frac{\sigma_g}{\sqrt{\Delta t}}\]
其中 $\Delta t$ 為采樣時間。為什么離散化后分母會多出 $\sqrt{\Delta t}$ 這一項呢？我們假定在一個采樣周期內 $n(t)$ 為常數，於是
\[n_d[k] \triangleq n(t_0+\Delta t)\simeq\frac{1}{\Delta t}\int_{t_0}^{t_0+\Delta t}n(\tau)dt\] \[\begin{aligned} E(n_d[k]^2) &= E(\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 dt) \\ &= E( \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 dt)\\ &= E(\frac{\sigma_g^2}{\Delta t}) \end{aligned}\]
所以有 $\sigma_{gd}^2=\frac{\sigma_g^2}{\Delta t}$，即 $\sigma_{gd}=\frac{\sigma_g}{\sqrt{\Delta t}}$。
接下來討論隨機游走模型。准確地講，隨機游走其實是一個離散模型，其連續模型稱為維納過程（Wiener Process）。維納模型是高斯白噪聲的積分：
\[\dot{b}_g(t)=n(t)=\sigma_{bg}w(t)\]
其中 $w$ 為單位高斯白噪聲。將其離散化后得到隨機游走模型：
\[b_d[k] = b_d[k-1]+\sigma_{bgd}w[k]\]
其中
\[w[k] \sim \mathcal{N}(0,1) \\ \sigma_{gd}=\sigma_{bg}\sqrt{\Delta t}\]
這里多出來的 $\sqrt{\Delta t}$ 又是哪來的呢？仍假定一個采樣周期內高斯白噪聲為常數，有：
\[b_d[k] \triangleq b(t_0) + \int_{t_0}^{t_0+\Delta t}n(t)dt\] \[\begin{aligned} E((b_d[k]-b_d[k-1])^2) &=E(\int_{t_0}^{t_0+\Delta t}\int_{t_0}^{t_0+\Delta t}n(t)n(\tau)d \tau dt)\\ &= E({\sigma_{bg}^2}\int_{t_0}^{t_0+\Delta t}\int_{t_0}^{t_0+\Delta t}\delta(t-\tau)d \tau dt)\\ &= E(\sigma_{bg}^2\Delta t) \end{aligned}\]
所以有 $\sigma_{bgd}^2=\sigma_{bg}^2\Delta t$，即 $\sigma_{bgd}=\sigma_{bg}\sqrt{\Delta t}$。
於是我們得到隨機游走模型的完整表達。實際上，觀察離散模型的表達式，可以發現它生動闡釋了「隨機游走」的含義：每一時刻都是上一個采樣時刻加上一個高斯白噪聲得到的，猶如一個游走的粒子，踏出的下一步永遠是隨機的。在我們前面給出的 IMU 的運動模型中，bias 就設定為服從隨機游走模型。
2-4. IMU 觀測模型
根據上述前置知識，現在我們可以給出 IMU 的觀測模型。需要注意的是，觀測在不同參考坐標系下形式不同。
以 ECEF 為參考坐標系：這是 MSCKF 一代 [1] 的做法。因為 ECEF 不是慣性系，需要考慮地球自轉，於是加速度模型中將會引入科氏加速度。記 ${\boldsymbol \omega}_G$ 為地球自轉角速度， ${}^G{\bf g}$ 為重力加速度， ${\boldsymbol \omega}_m,{\bf a}_m$ 為陀螺儀和加速度計的觀測量，觀測模型由以下公式給出：
\[{\boldsymbol \omega}_m = {\boldsymbol \omega}+{\bf R}({}^I_G \bar{q}){\boldsymbol \omega}_G+{\bf b}_g+{\bf n}_g \tag{2.2}\] \[{\bf a}_m = {\bf R}({}^I_G \bar{q})({}^G{\bf a} -{}^G{\bf g} +2{\boldsymbol \omega}_G^{\land}{}^G{\bf v}_I+({\boldsymbol \omega}_G^{\land})^2{}^G{\bf p}_I)+{\bf b}_a+{\bf n}_a \tag{2.3}\]
觀測量都是在體坐標系 {I} 下表達的，所以在參考坐標系 {G} 下表達的量都需要左乘一個旋轉矩陣轉化到體坐標系。每個觀測量的不確定量都用一個隨機游走的 bias 和一個高斯白噪聲之和來表達。陀螺儀的觀測模型是比較易懂的。加速度計的觀測模型，我們先將其改寫為形如 $(2.1)$ 的形式：
\[{\bf R}^T({}^I_G \bar{q})({\bf a}_m-{\bf b}_a-{\bf n}_a)=({\boldsymbol \omega}_G^{\land})^2{}^G{\bf p}_I+2{\boldsymbol \omega}_G^{\land}{}^G{\bf v}_I+{}^G{\bf a}-{}^G{\bf g}\]
但這還不夠，因為各個量只是在 ECEF 坐標系 {G} 下的表達，而 $(2.1)$ 中的量都是表達在慣性坐標系下的。記 ${\bf R}_G$ 為將 {G} 下坐標映射到慣性坐標系下坐標的旋轉矩陣。由於 ECEF 繞固定的 z 軸勻速轉動，易得 ${\bf R}_G{\boldsymbol \omega}_G = {\boldsymbol \omega}_G $。於是上式兩邊左乘 ${\bf R}_G$，可得
\[{\bf R}_G{\bf R}^T({}^I_G \bar{q})({\bf a}_m-{\bf b}_a-{\bf n}_a)={\boldsymbol \omega}_G\times({\boldsymbol \omega}_G\times{\bf R}_G{}^G{\bf p}_I) + 2{\boldsymbol \omega}_G\times({\bf R}_G{}^G{\bf v}_I)+{\bf R}_G({}^G{\bf a}-{}^G{\bf g})\]
這里我們還利用了 ${\bf R}(\bf a\times b)={\bf Ra}\times{\bf Rb}$ 的性質。上式對應到 $(2.1)$ 中各項，左邊為絕對加速度 $\bf a$；因為地球自轉是勻速的，故切向加速度項 ${\boldsymbol \alpha} \times\bf r$ 為零。其余各項，依次為向心加速度項 ${\boldsymbol \omega}\times({\boldsymbol \omega}\times{\bf r})$，科氏加速度項 $2{\boldsymbol \omega}\times{\bf v}_r$，以及相對加速度項 ${\bf a}_r$。
以 ECI 為參考坐標系：這是 MSCKF 二代 [3] 的做法。由於 ECI 為慣性系，不需要考慮地球自轉，於是觀測模型簡單很多：
\[{\boldsymbol \omega}_m = {\boldsymbol \omega}+{\bf b}_g+{\bf n}_g \tag{2.4}\] \[{\bf a}_m = {\bf R}({}^I_G \bar{q})({}^G{\bf a} -{}^G{\bf g} )+{\bf b}_a+{\bf n}_a \tag{2.5}\]
因為比較簡單，就不多做解釋了。從文獻上看，現在移動機器人領域 ECI 用得更多些。
至此，我們推導完了 IMU 的觀測模型。
3. IMU 狀態估計誤差模型
3-1. 前置：四元數誤差小量
旋轉量是非線性的，不宜像線性量那樣使用 $\tilde{ \bf x}= x-\hat{x}$ 來定義誤差量。這里我們使用四元數誤差小量來定義誤差量。根據 $(1.4)$，四元數可以用旋轉向量經簡單的轉換得到。假定繞單位軸 $\bf u$ 旋轉了一個角度小量 $\delta \theta$，用四元數表達為：
\[\begin{aligned} \delta {\bf q} &= \begin{bmatrix} {\bf u}\sin{\frac{\delta \theta}{2}} \\ \cos{\frac{\delta \theta}{2}} \end{bmatrix}\\ &\simeq \begin{bmatrix} {\bf u}\frac{\delta \theta}{2} \\ 1 \end{bmatrix} \triangleq \begin{bmatrix} \frac{\boldsymbol{\delta \theta}}{2} \\ 1 \end{bmatrix} \end{aligned}\]
於是，可以用 $\delta {\bf q} $ 來表示旋轉的真實值和估計值之間的誤差，具體關系為
\[{\bf q} = \delta {\bf q} \otimes\hat{\bf q}\]
直接使用 $\boldsymbol{\delta \theta}$，可以實現參數最小化，適用於優化問題中的目標函數。
3-2. IMU 狀態估計誤差模型
我們直接給出和 MSCKF 一樣的 IMU 狀態估計誤差模型：
\[\tilde{\bf X}_{IMU} = [ \boldsymbol{\delta \theta}_I^T \quad \tilde{\bf b}_g^T \quad ^G\tilde{\bf v}_I^T \quad \tilde{\bf b}_a^T \quad ^G\tilde{\bf p}_I^T] \tag{3.0}\]
其中旋轉量按照四元數誤差小量給出，其余直接由真實值和估計值相減得到。
4. 小結
本文從基礎出發推導了 IMU 的運動模型$(1.7)$、觀測和噪聲模型$(2.2)-(2.5)$、估計誤差模型$(3.0)$，適用於用 IMU 來做狀態估計的場合。至於以上這些模型如何再經過線性化、離散化等處理進入具體狀態估計問題的框架中，這里不做贅述，留待讀者閱讀和探索。