IMU 积分的误差状态空间方程推导

0. 前言

本文推导的是 IMU 积分的误差状态空间方程，预积分的推导要比积分复杂，但是推到方法和本文是一样的，因此这里只给出积分的推导方式。

另外，本文的推导方式参考了 R2Live Supplementary Material 中给出的推导，这种方法是基于李群李代数使用 全增量 的方法计算误差状态空间方程，非常好理解。

1. 离散时间的IMU运动学方程

$$\{\begin{array}{l}{\mathbf{p}}_{i+1}={\mathbf{p}}_i+{\mathbf{v}}_i\Delta t+\frac{1}{2}\left[{\mathbf{R}}_i\left({\mathbf{a}}_{mi}-{\mathbf{b}}_{a_i}-{\mathbf{n}}_{ai}\right)-\mathbf{g}\right]\Delta t^2\\ {\mathbf{v}}_{i+1}={\mathbf{v}}_i+\left[{\mathbf{R}}_i\left({\mathbf{a}}_{mi}-{\mathbf{b}}_{ai}-{\mathbf{n}}_{ai}\right)-\mathbf{g}\right]\Delta t\\ {\mathbf{R}}_{i+1}={\mathbf{R}}_i\exp \left[\left({\omega }_{mi}-{\mathbf{b}}_{\omega i}-{\mathbf{n}}_{\omega i}\right)\Delta t\right]\\ {\mathbf{b}}_{\omega i+1}={\mathbf{b}}_{\omega i}+{\mathbf{n}}_{b_{\omega }i}\Delta t\\ {\mathbf{b}}_{a_i+1}={\mathbf{b}}_{ai}+{\mathbf{n}}_{b_{a}i}\Delta t\end{array}$$

• 式中忽略了参考坐标系，一般可以选择 $world$ 系 $W$，此时 $g=[0,0,9.8{]}^T$；
• ${\mathbf{n}}_{ai},{\mathbf{n}}_{\omega i}$ 为IMU读数的高斯白噪声；
• ${\mathbf{n}}_{b_{a}i},{\mathbf{n}}_{b_{\omega }i}$ 为IMU零偏的随机游走的高斯分布。

2. 状态变量定义

• 名义状态 $\hat{\mathbf{x}}$：估计出来的状态，真正能算出来的状态；
• 真实状态 $\mathbf{x}$：无法计算出来的状态，永远不知道的状态，只能用公式表示出来；
• 误差状态 $\delta \mathbf{x}$：真实状态 $\mathbf{x}$ 与 名义状态 $\hat{\mathbf{x}}$ 的差值，关系式为：$\mathbf{x}=\hat{\mathbf{x}}+\delta \mathbf{x}$，即 真实状态 = 名义状态 + 误差状态。

比如：

• 角速度 $\omega$ 的名义状态和真实状态：
  $$\{\begin{array}{l}名义值：\widehat{{\omega }_i}={\omega }_{mi}-{\hat{\mathbf{b}}}_{\omega i}\\ 真实值：{\omega }_i=\widehat{{\omega }_i}-\delta {\hat{\mathbf{b}}}_{\omega i}-{\mathbf{n}}_{\omega i}={\omega }_{mi}-{\hat{\mathbf{b}}}_{\omega i}-\delta {\hat{\mathbf{b}}}_{\omega i}-{\mathbf{n}}_{\omega i}\end{array}$$

• 加速度 $\mathbf{a}$ 的名义状态和真实状态：
  $$\{\begin{array}{l}名义值：\widehat{{\mathbf{a}}_i}={\mathbf{a}}_{mi}-{\hat{\mathbf{b}}}_{ai}\\ 真实值：{\mathbf{a}}_i=\widehat{{\mathbf{a}}_i}-\delta {\hat{\mathbf{b}}}_{ai}-{\mathbf{n}}_{ai}={\mathbf{a}}_{mi}-{\hat{\mathbf{b}}}_{ai}-\delta {\hat{\mathbf{b}}}_{ai}-{\mathbf{n}}_{ai}\end{array}$$

3. 补充公式

• 李代数上的增量和李群上的扰动之间的关系：
  $$\begin{array}{r}\exp (\varphi +\Delta \varphi )=\exp (J_l\Delta \varphi )\cdot \exp (\varphi )=\exp (\varphi )\cdot \exp (J_r\Delta \varphi )\end{array}$$

• $SO(3)$的伴随性质：
  $$\begin{array}{r}{R}^{\top }\exp (\varphi )R=\exp \left(R^{\top }\varphi \right)\end{array}$$

• BCH近似公式：
  $$\begin{array}{r}\log \left[\exp \left({\varphi }_1\right)\exp \left({\varphi }_2\right)\right]=\{\begin{array}{l}{\mathrm{J}}_{\mathrm{l}}{\left({\varphi }_2\right)}^{-1}{\varphi }_1+{\varphi }_2,{\varphi }_1\approx 0\\ {\mathrm{J}}_{\mathrm{r}}{\left({\varphi }_1\right)}^{-1}{\varphi }_2+{\varphi }_1,{\varphi }_2\approx 0\end{array}\end{array}$$

4. IMU误差状态空间方程推导

4.1. 旋转误差 $\delta {\hat{\mathbf{r}}}_{i+1}$

$$\begin{array}{rl}\text{ 定义：}\delta {\hat{r}}_{i+1}& =\log \left[{\hat{R}}_{i+1}^{\top }{R}_{itt}\right]\\ \text{名义值：}{\hat{R}}_{i+1}& ={\hat{R}}_i\exp \left({\hat{w}}_i\Delta t\right)\\ \text{真实值：}{R}_{i+1}& ={\hat{R}}_i\exp \left(\delta {\hat{r}}_i\right)\cdot \exp \left({\omega }_i\Delta t\right)\end{array}$$

则旋转误差如下：

4.2. 速度误差 $\delta {\hat{\mathbf{v}}}_{i+1}$

$$\begin{array}{rl}名义值：{\hat{v}}_{i+1}& ={\hat{v}}_i+\left({\hat{R}}_i{\hat{a}}_i-g\right)\Delta t\\ 真实值：v_{i+1}& =\left({\hat{v}}_i+\delta {\hat{v}}_i\right)+\left[{\hat{R}}_i\exp \left(\delta {\hat{r}}_i\right)\left({\hat{a}}_i-\delta {\hat{b}}_{ai}-n_{ai}\right)-g\right]\Delta t\end{array}$$

4.3. 平移误差 $\delta {\mathbf{p}}_{i+1}$

$$\begin{array}{rl}& \text{ 名义值： }{\hat{p}}_{i+1}=\widehat{p_i}+{\hat{v}}_i\Delta t+\frac{1}{2}\left(\widehat{R_i}\widehat{a_i}-g\right)\Delta t^2\\ & \text{ 真实值： }{p}_{i+1}=\left({\hat{p}}_i+\delta {\hat{p}}_i\right)+\left({\hat{v}}_i+\delta {\hat{v}}_i\right)\Delta t+\frac{1}{2}\left[{\hat{R}}_i\cdot \exp \left(\delta {\hat{r}}_i\right)\left({\hat{a}}_i-\delta {\hat{b}}_{ai}-n_{ai}\right)-g\right]\cdot \Delta t^2\\ & \end{array}$$

4.4. 角速度零偏误差 $\delta {\hat{b}}_{w_{i+1}}$

$$\begin{array}{rl}\text{ 名义值：}{\hat{b}}_{w_{i+1}}& ={\hat{b}}_{wi}\\ \text{ 真实值：}{b}_{w_{i+1}}& ={\hat{b}}_{w_i}+\delta b_{wi}+\Delta t\cdot n_{b_{wi}}\\ \text{ 误差：}\delta b_{w_{i+1}}& =\delta b_{wi}+\Delta t\cdot n_{b_{wi}}\end{array}$$

4.5. 加速度零偏误差 $\delta {\hat{b}}_{a_{i+1}}$

$$\begin{array}{rl}\text{ 名义值：}{\hat{b}}_{a_{i+1}}& ={\hat{b}}_{ai}\\ \text{ 真实值：}{b}_{a_{i+1}}& ={\hat{b}}_{a_i}+\delta b_{ai}+\Delta t\cdot n_{b_{ai}}\\ \text{ 误差：}\delta b_{a_{i+1}}& =\delta b_{ai}+\Delta t\cdot n_{b_{ai}}\end{array}$$

4.6. 整理成矩阵状态空间方程的形式

定义IMU运动学的矩阵状态空间方程形式为

$$\delta {\hat{\mathbf{x}}}_{i+1}={\mathbf{F}}_x\cdot \delta {\hat{\mathbf{x}}}_i+{\mathbf{F}}_w\cdot {\mathbf{w}}_i$$

其中状态变量的定义为：

$$\begin{array}{rl}\delta {\hat{\mathbf{x}}}_i& ={\left[\delta {\hat{\mathbf{r}}}_i,\delta {\hat{\mathbf{p}}}_i,\delta {\hat{\mathbf{v}}}_i,\delta {\hat{\mathbf{b}}}_{wi},\delta {\hat{\mathbf{b}}}_{ai}\right]}^{\top }\\ {\mathbf{w}}_i& ={\left[\delta {\mathbf{n}}_{wi},\delta {\mathbf{n}}_{ai},\delta {\mathbf{n}}_{b_{wi}},\delta {\mathbf{n}}_{b_{ai}}\right]}^{\top }\end{array}$$

则系数矩阵为：

$$\begin{array}{rl}{\mathbf{F}}_x& =\left[\begin{array}{ccccc}\exp \left(-\widehat{{\omega }_i\Delta t}\right)& 0& 0& -I\Delta t& 0\\ 0& I& I\Delta t& 0& 0\\ -{\hat{R}}_i{\left({\hat{a}}_i\right)}_{\times }\Delta t& 0& I& 0& -{\hat{R}}_i\Delta t\\ 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0\end{array}\right]\\ & \\ F_{\omega }& =\left[\begin{array}{cccc}-I\Delta t& 0& 0& 0\\ 0& 0& 0& 0\\ 0& -{\hat{R}}_i\Delta t& 0& 0\\ 0& 0& I\Delta t& 0\\ 0& 0& 0& I\Delta t\end{array}\right]\end{array}$$

协方差矩阵的传播公式为：

$${\mathbf{P}}_{i+1}={\mathbf{F}}_x{\mathbf{P}}_i{\mathbf{F}}_x{}^{\top }+{\mathbf{F}}_{\omega }\mathbf{Q}{\mathbf{F}}_{\omega }{}^{\top }$$

其中 $\mathbf{Q}$ 为测量噪声的协方差矩阵。