>

## 连续时间下的 IMU 运动模型

### 位置、速度、姿态更新

\begin{aligned} \tilde{\boldsymbol{\omega}}^b &= \boldsymbol{\omega}^b + \boldsymbol{b}^g + \boldsymbol{n}^g \\ \tilde{\boldsymbol{a}}^b &= \boldsymbol{a}^b + \boldsymbol{b}^a + \boldsymbol{n}^a \end{aligned}


\begin{aligned} \dot{\boldsymbol{p}}_{wb} &= \boldsymbol{v}^w_t \\ \dot{\boldsymbol{v}}^w_t &= \boldsymbol{a}^w_t \end{aligned}


### 四元数姿态对时间求导

$$\dot{\boldsymbol{q}}_{wb_t} = \boldsymbol{q}\otimes\begin{bmatrix} 0 \\ \frac{1}{2}\boldsymbol{\omega}^{b_t} \end{bmatrix}$$


### 旋转矩阵姿态对时间求导

$$\dot{\boldsymbol{R}}_{wb_t} = \boldsymbol{R}_{wb_t}\boldsymbol{\omega}^{b_t\wedge}$$


### 基于四元数状态更新

\begin{aligned} \boldsymbol{q}_{wb_j} =\boldsymbol{q}_{wb_t}\otimes\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix} 0 \\ \frac{1}{2}\boldsymbol{\omega}^{b_t} \end{bmatrix})}\delta t \\ \boldsymbol{q}_{wb_j} =\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\otimes\begin{bmatrix} 1 \\ \frac{1}{2}\boldsymbol{\omega}^{b_t} \end{bmatrix})}\delta t \end{aligned}


$$\boldsymbol{v}^w_j = \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\boldsymbol{a}(\boldsymbol{q}_{wb_t}^{b_t})^{-1} - \boldsymbol{g}^w)}\delta t$$


$$\boldsymbol{p}_{wb_j} = \boldsymbol{p}_{wb_i} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{q}_{wb_t}\boldsymbol{a}(\boldsymbol{q}_{wb_t}^{b_t})^{-1} - \boldsymbol{g}^w)}\delta t^2$$


### 基于旋转矩阵状态更新

\begin{aligned} \boldsymbol{R}_{wb_j} &= \boldsymbol{R}_{wb_i}\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}(\boldsymbol{\omega}^{b_t})^\wedge)}\delta t \\ \boldsymbol{v}^w_j &= \boldsymbol{v}^w_i + \int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t\\ \boldsymbol{p}_{wb_j} &= \boldsymbol{p}_{wb_t} + \boldsymbol{v}^w_i\Delta t + \int\int_{t\in[i,j]}{(\boldsymbol{R}_{wb_t}\boldsymbol{a} - \boldsymbol{g}^w)}\delta t^2 \end{aligned}


TODO

## IMU 运动模型离散化

### 欧拉法离散积分

\begin{aligned} \boldsymbol{a} &= \boldsymbol{q}_{wb_k}\boldsymbol{a}^{b_k}\boldsymbol{q}_{wb_k}^{-1} - \boldsymbol{g}^w \\ \boldsymbol{\omega} &= \boldsymbol{\omega}^{b_k}\\ \boldsymbol{q}_{wb_{k+1}} &= \boldsymbol{q}_{wb_k}\otimes\begin{bmatrix} 1\\ \frac{1}{2}\boldsymbol{\omega}\Delta t \end{bmatrix}\\ \boldsymbol{v}_{k+1}^w &= \boldsymbol{v}_k^w + \boldsymbol{a}\Delta t\\ \boldsymbol{p}_{wb_{k+1}} &= \boldsymbol{p}_{wb_{k}} + \boldsymbol{v}_k^w\Delta t + \frac{1}{2}\boldsymbol{a}\Delta t^2 \end{aligned}


\begin{aligned} \boldsymbol{a} &= \boldsymbol{R}_{wb_k}\boldsymbol{a}^{b_k} - \boldsymbol{g}^w \\ \boldsymbol{\omega} &= \boldsymbol{\omega}^{b_k}\\ \boldsymbol{R}_{wb_{k+1}} &= \boldsymbol{R}_{wb_k}\boldsymbol{R}_{b_kb_{k+1}} = \boldsymbol{R}_{wb_k}\exp{((\boldsymbol{\omega}\Delta t)^\wedge)}\\ \boldsymbol{v}_{k+1}^w &= \boldsymbol{v}_k^w + \boldsymbol{a}\Delta t\\ \boldsymbol{p}_{wb_{k+1}} &= \boldsymbol{p}_{wb_{k}} + \boldsymbol{v}_k^w\Delta t + \frac{1}{2}\boldsymbol{a}\Delta t^2 \end{aligned}


### 中值法离散积分

\begin{aligned} \boldsymbol{a} &= \frac{1}{2}[\boldsymbol{q}_{wb_k}\boldsymbol{a}^{b_k} \boldsymbol{q}_{wb_k}^{-1} - \boldsymbol{g}^w + \boldsymbol{q}_{wb_{k+1}}(\boldsymbol{a}^{b_{k+1}} - \boldsymbol{b}_{k}^a)\boldsymbol{q}_{wb_{k+1}}^{-1} - \boldsymbol{g}^w]\\ \boldsymbol{\omega} &= \frac{1}{2}[\boldsymbol{\omega}^{b_k} + \boldsymbol{\omega}^{b_{k+1}} ] \end{aligned}


\begin{aligned} \boldsymbol{a} &= \frac{1}{2}(\boldsymbol{R}_{wb_k}\boldsymbol{a}^{b_k} - \boldsymbol{g}^w + \boldsymbol{R}_{wb_{k+1}}\boldsymbol{a}^{b_{k+1}} - \boldsymbol{g}^w) \\ \boldsymbol{\omega} &= \frac{1}{2}[\boldsymbol{\omega}^{b_k} + \boldsymbol{\omega}^{b_{k+1}} ] \end{aligned}


### 姿态更新中的坐标系讨论

• IMU 参考的惯性坐标系，一般以地心惯性系为参考，这个坐标系和 ECEF 系不同不会随着地球自转旋转
• 导航坐标系：一般以当地的东北天坐标系作为导航坐标系
• 机体坐标系：这里可以认为是 IMU 的 Body 系

\begin{aligned} \boldsymbol{\omega}_{in} &= \boldsymbol{\omega}_{ie} + \boldsymbol{\omega}_{en}\\ \boldsymbol{\omega}_{ie} &= \begin{bmatrix} 0 & \omega_{ie}\cos{L} & \omega_{ie}\cos{L} \end{bmatrix}^T\\ \boldsymbol{\omega}_{en} &= \begin{bmatrix} -\frac{v_N}{R_M+h} & \frac{v_E}{R_N + h} & \frac{v_E}{R_N + h}\tan{L} \end{bmatrix}^T \end{aligned}


\begin{aligned} \boldsymbol{R}_{nb} &= \boldsymbol{R}_{ni}\boldsymbol{R}_{ib}\\ &= \boldsymbol{R}_{n_tn_{t-1}}\boldsymbol{R}_{n_{t-1}i}\boldsymbol{R}_{ib_{t-1}}\boldsymbol{R}_{b_tb_{t-1}} \end{aligned}


## 误差分析

\begin{aligned} \dot{z} &= x + y \\ \end{aligned}


\begin{aligned} \dot{\tilde{z}} &= \tilde{x} + \tilde{y}\\ \tilde{z} &= z + \delta z\\ \tilde{x} &= x + \delta x\\ \tilde{y} &= y + \delta y \end{aligned}


\begin{aligned} \dot{z} + \delta\dot{z} = x + \delta x + y + \delta y \end{aligned}


\begin{aligned} x + y + \delta\dot{z} &= x + \delta x + y + \delta y\\ \Rightarrow \delta\dot{z} &= \delta x + \delta y \end{aligned}


### 姿态误差分析（旋转矩阵）

$$\dot{\boldsymbol{R}}_{wb} = \boldsymbol{R}_{wb}((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)$$


$$\dot{\tilde{\boldsymbol{R}}}_{wb} = \tilde{\boldsymbol{R}}_{wb}((\tilde{\boldsymbol{\omega}}_{wb}^{b} - \tilde{\boldsymbol{b}}^{\omega})^\wedge)$$


$$\tilde{\boldsymbol{R}}_{wb} = \boldsymbol{R}_{wb}\exp{(\delta\boldsymbol{\theta})^\wedge}\approx\boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)$$


$$\tilde{\boldsymbol{\omega}}_{wb}^b = \boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}$$


$$\tilde{\boldsymbol{b}}^{\omega} = \boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega}$$


\begin{aligned} \tilde{\boldsymbol{R}}_{wb} &\approx \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)\\ \tilde{\boldsymbol{\omega}}_{wb}^b &= \boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}\\ \tilde{\boldsymbol{b}}^{\omega} &= \boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega} \end{aligned}


$$\dot{\tilde{\boldsymbol{R}}}_{wb} = \tilde{\boldsymbol{R}}_{wb}((\tilde{\boldsymbol{\omega}}_{wb}^{b} - \tilde{\boldsymbol{b}}^{\omega})^\wedge)$$


$$\dot{(\boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge))} = \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega} - \boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge)$$


\begin{aligned} \text{左边} &= \dot{(\boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge))}\\ &= \dot{\boldsymbol{R}}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge) + \boldsymbol{R}_{wb}\dot{(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)}\\ &= \boldsymbol{R}_{wb}((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge) + \boldsymbol{R}_{wb}\dot{\delta\boldsymbol{\theta}}^\wedge\\ \text{右边} &= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega} - \boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge) \end{aligned}


\begin{aligned} \mathrm{左边} &= ((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge) + \dot{\delta\boldsymbol{\theta}}^\wedge\\ \mathrm{右边} &= (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega} - \boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge)\\ \end{aligned}


\begin{aligned} \dot{\delta\boldsymbol{\theta}}^\wedge &= (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega} - \boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge) - ((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)\\ &= (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)((\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge +(\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge) - ((\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega})^\wedge)(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)\\ &= \delta\boldsymbol{\theta}^\wedge(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge - (\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta}^\wedge + (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\\ &= - ((\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta}^\wedge - \delta\boldsymbol{\theta}^\wedge(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge) + (\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\\ &= - ((\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})\times\delta\boldsymbol{\theta})^\wedge + (\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\\ &= (-(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})\times\delta\boldsymbol{\theta} + \boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\\ \Rightarrow \dot{\delta\boldsymbol{\theta}} &= -(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta} + \boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega} \end{aligned}


### 姿态误差分析（四元数）

$$\dot{\boldsymbol{q}}_{wb} = \frac{1}{2}\boldsymbol{q}_{wb}\otimes\begin{bmatrix} 0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega} \end{bmatrix}$$


$$\dot{\tilde{\boldsymbol{q}}}_{wb} = \frac{1}{2}\tilde{\boldsymbol{q}}_{wb}\otimes\begin{bmatrix} 0\\\tilde{\boldsymbol{\omega}}_{wb}^{b} - \tilde{\boldsymbol{b}}^{\omega} \end{bmatrix}$$


\begin{aligned} \tilde{\boldsymbol{q}}_{wb} &= \boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q}\\ \tilde{\boldsymbol{\omega}}_{wb}^b &= \boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}\\ \tilde{\boldsymbol{b}}^{\omega} &= \boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega} \end{aligned}


\begin{aligned} \delta\boldsymbol{q} &= \begin{bmatrix} \cos{(||\delta\boldsymbol{\theta}||/2)} \\ \sin{(||\delta\boldsymbol{\theta}||/2)\frac{\delta\boldsymbol{\theta}}{||\delta\boldsymbol{\theta}||}} \end{bmatrix}\\ &\approx\begin{bmatrix} 1 \\ \delta\boldsymbol{\theta} /2 \end{bmatrix} \qquad ||\delta\boldsymbol{\theta}||\approx0 \end{aligned}


$$\dot{(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})} = \frac{1}{2}(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})\otimes\begin{bmatrix} 0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega}) \end{bmatrix}$$


\begin{aligned} \text{左边} &= \dot{(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})} \\ &= \dot{\boldsymbol{q}}_{wb}\otimes\delta\boldsymbol{q} + \boldsymbol{q}_{wb}\otimes\dot{\delta\boldsymbol{q}}\\ &= \frac{1}{2}\boldsymbol{q}_{wb}\otimes\begin{bmatrix} 0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega} \end{bmatrix} \otimes\delta\boldsymbol{q} + \boldsymbol{q}_{wb}\otimes\dot{\delta\boldsymbol{q}}\\ \text{右边} &= \frac{1}{2}(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})\otimes\begin{bmatrix} 0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega}) \end{bmatrix} \end{aligned}


\begin{aligned} \frac{1}{2}\boldsymbol{q}_{wb}\otimes\begin{bmatrix} 0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega} \end{bmatrix} \otimes\delta\boldsymbol{q} + \boldsymbol{q}_{wb}\otimes\dot{\delta\boldsymbol{q}} &= \frac{1}{2}(\boldsymbol{q}_{wb}\otimes\delta\boldsymbol{q})\otimes\begin{bmatrix} 0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega}) \end{bmatrix}\\ \frac{1}{2}\begin{bmatrix} 0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega} \end{bmatrix} \otimes\delta\boldsymbol{q} + \dot{\delta\boldsymbol{q}} &= \frac{1}{2}\delta\boldsymbol{q}\otimes\begin{bmatrix} 0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega}) \end{bmatrix}\\ \dot{\delta\boldsymbol{q}} &= \frac{1}{2}\delta\boldsymbol{q}\otimes\begin{bmatrix} 0\\(\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega}) \end{bmatrix} - \frac{1}{2}\begin{bmatrix} 0\\\boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega} \end{bmatrix} \otimes\delta\boldsymbol{q} \end{aligned}


\begin{aligned} \boldsymbol{\omega}_1 &= (\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega}) - (\boldsymbol{b}^{\omega} + \delta\boldsymbol{b}^{\omega})\\ \boldsymbol{\omega}_2 &= \boldsymbol{\omega}_{wb}^{b} - \boldsymbol{b}^{\omega} \end{aligned}


\begin{aligned} \dot{\delta\boldsymbol{q}} &= \frac{1}{2}\delta\boldsymbol{q}\otimes\begin{bmatrix} 0\\\boldsymbol{\omega}_1 \end{bmatrix} - \frac{1}{2}\begin{bmatrix} 0\\\boldsymbol{\omega}_2 \end{bmatrix} \otimes\delta\boldsymbol{q}\\ &= \frac{1}{2}\left(\begin{bmatrix}0 \\\boldsymbol{\omega}_1\end{bmatrix}_R\delta\boldsymbol{q} - \begin{bmatrix}0 \\\boldsymbol{\omega}_2\end{bmatrix}_R\delta\boldsymbol{q}\right)\\ &= \frac{1}{2}\left(\begin{bmatrix}0 \\\boldsymbol{\omega}_1\end{bmatrix}_R - \begin{bmatrix}0 \\\boldsymbol{\omega}_2\end{bmatrix}_R\right)\delta\boldsymbol{q}\\ &= \frac{1}{2} \begin{bmatrix} 0 & (\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2)^T\\ (\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2) & -(\boldsymbol{\omega}_1 + \boldsymbol{\omega}_2)^\wedge \end{bmatrix} \delta\boldsymbol{q} \end{aligned}


\begin{aligned} \delta\boldsymbol{q} &\approx\begin{bmatrix}1 \\ \delta\boldsymbol{\theta} /2\end{bmatrix}\\ \dot{\delta\boldsymbol{q}} &\approx \begin{bmatrix}0 \\ \dot{\delta\boldsymbol{\theta}}/2\end{bmatrix} \end{aligned}


\begin{aligned} \begin{bmatrix}0 \\ \dot{\delta\boldsymbol{\theta}}/2\end{bmatrix} &= \frac{1}{2} \begin{bmatrix} 0 & (\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2)^T\\ (\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2) & -(\boldsymbol{\omega}_1 + \boldsymbol{\omega}_2)^\wedge \end{bmatrix} \begin{bmatrix}1 \\ \delta\boldsymbol{\theta} /2\end{bmatrix}\\ \Rightarrow \dot{\delta\boldsymbol{\theta}} &= (\boldsymbol{\omega}_1 - \boldsymbol{\omega}_2) - \frac{1}{2}(\boldsymbol{\omega}_1 + \boldsymbol{\omega}_2)^\wedge\delta\boldsymbol{\theta}\\ &= (\boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega}) - \frac{1}{2}(2\boldsymbol{\omega}_{wb}^b + \boldsymbol{n}^{\omega} - 2\boldsymbol{b}^{\omega} - \delta\boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta}\\ &\approx -(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta} + \boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega} \end{aligned}


### 速度误差分析

$$\dot{\boldsymbol{v}}^w = \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a) -\boldsymbol{g}^w$$


$$\dot{\tilde{\boldsymbol{v}}}^w = \tilde{\boldsymbol{R}}_{wb}(\tilde{\boldsymbol{a}}^b - \tilde{\boldsymbol{b}}^a)- \tilde{\boldsymbol{g}}^w$$


\begin{aligned} \tilde{\boldsymbol{v}}&=\boldsymbol{v}+\delta\boldsymbol{v}\\ \tilde{\boldsymbol{R}}_{wb} &\approx \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)\\ \tilde{\boldsymbol{a}}^b &= \boldsymbol{a}^b + \boldsymbol{n}_a\\ \tilde{\boldsymbol{b}}^a &= \boldsymbol{b}^a + \delta\boldsymbol{b}^a\\ \tilde{\boldsymbol{g}}^w&= \boldsymbol{g}^w + \delta\boldsymbol{g}^w \end{aligned}


\begin{aligned} \dot{\tilde{\boldsymbol{v}}}^w &= \tilde{\boldsymbol{R}}_{wb}(\tilde{\boldsymbol{a}}^b - \tilde{\boldsymbol{b}}^a)- \tilde{\boldsymbol{g}}^w\\ \frac{d(\boldsymbol{v}+\delta\boldsymbol{v})}{dt} &= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w)\\ \dot{\boldsymbol{v}} + \dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w) \end{aligned}


\begin{aligned} \dot{\boldsymbol{v}} + \dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w)\\ \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a) -\boldsymbol{g}^w + \dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w)\\ \dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w) - \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a) +\boldsymbol{g}^w \end{aligned}


\begin{aligned} \dot{\delta\boldsymbol{v}}&= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) -(\boldsymbol{g}^w + \delta\boldsymbol{g}^w) - \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a) +\boldsymbol{g}^w\\ &= \boldsymbol{R}_{wb}(\boldsymbol{I} + \delta\boldsymbol{\theta}^\wedge)(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a))- \delta\boldsymbol{g}^w - \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a)\\ &= \boldsymbol{R}_{wb}(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a)) +\boldsymbol{R}_{wb}\delta\boldsymbol{\theta}^\wedge(\boldsymbol{a}^b + \boldsymbol{n}_a - (\boldsymbol{b}^a + \delta\boldsymbol{b}^a))- \delta\boldsymbol{g}^w - \boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a)\\ &= \boldsymbol{R}_{wb}(\boldsymbol{n}_a - \delta\boldsymbol{b}^a) +\boldsymbol{R}_{wb}\delta\boldsymbol{\theta}^\wedge(\boldsymbol{a}^b - \boldsymbol{b}^a)- \delta\boldsymbol{g}^w\\ &= \boldsymbol{R}_{wb}(\boldsymbol{n}_a - \delta\boldsymbol{b}^a) -\boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a)^\wedge\delta\boldsymbol{\theta}- \delta\boldsymbol{g}^w\\ \end{aligned}


### 位置误差分析

$$\dot{\boldsymbol{p}} = \boldsymbol{v}$$


$$\dot{\tilde{\boldsymbol{p}}} = \tilde{\boldsymbol{v}}$$


$$\tilde{\boldsymbol{p}} = \boldsymbol{p} + \delta\boldsymbol{p}\\ \tilde{\boldsymbol{v}} = \boldsymbol{v} + \delta\boldsymbol{v}$$


$$\dot{\boldsymbol{p}} + \delta\dot{\boldsymbol{p}} = \boldsymbol{v} + \delta\boldsymbol{v}$$


$$\boldsymbol{v} + \delta\dot{\boldsymbol{p}} = \boldsymbol{v} + \delta\boldsymbol{v}$$


$$\delta\dot{\boldsymbol{p}} = \delta\boldsymbol{v}$$


### 零偏误差更新

\begin{aligned} \delta\boldsymbol{b}^a &= 0\\ \delta\boldsymbol{b}^g &= 0\\ \end{aligned}


\begin{aligned} \delta\boldsymbol{b}^a &= \boldsymbol{n}_{b^a}\\ \delta\boldsymbol{b}^w &= \boldsymbol{n}_{b^w}\\ \end{aligned}


### 总结

IMU 状态误差的更新方式为：

\begin{aligned} \delta\dot{\boldsymbol{p}} &= \delta\boldsymbol{v}\\ \dot{\delta\boldsymbol{v}}&=\boldsymbol{R}_{wb}(\boldsymbol{n}_a - \delta\boldsymbol{b}^a) -\boldsymbol{R}_{wb}(\boldsymbol{a}^b - \boldsymbol{b}^a)^\wedge\delta\boldsymbol{\theta}- \delta\boldsymbol{g}^w\\ \delta\dot{\boldsymbol{\theta}}&= -(\boldsymbol{\omega}_{wb}^b - \boldsymbol{b}^{\omega})^\wedge\delta\boldsymbol{\theta} + \boldsymbol{n}^{\omega} - \delta\boldsymbol{b}^{\omega}\\ \delta\dot{\boldsymbol{b}}^a &= \boldsymbol{n}_{b^a} (\text{或者} 0)\\ \delta\dot{\boldsymbol{b}}^g &= \boldsymbol{n}_{b^w} (\text{或者} 0) \end{aligned}


### 误差分析的目的

• 可以用来推导状态和观测方程
• 可以通过导航任务精度的要求，通过误差转移关系反推出所需传感器的精度来辅助器件选型。（另一种选型的方法是通过仿真来选型）
• 在 ESKF 中可以作为预测方程对误差进行扩散

## 参考资料

Built with Hugo
Theme Stack designed by Jimmy