Logo

Notes on state derivation of IMU model in OKVIS

Posted at 2017-05

Background

In OKVIS paper, states are:
$$
\mathbf{x}{\mathrm{R}}:=\left[{W} \mathbf{r}{S}^{\mathrm{T}}, \mathbf{q}{W S}^{\mathrm{T}}, s \mathbf{v}^{\mathrm{T}}, \mathbf{b}{\mathrm{g}}^{\mathrm{T}}, \mathbf{b}{\mathrm{a}}^{\mathrm{T}}\right]^{\mathrm{T}} \in \mathbb{R}^{3} \times S^{3} \times \mathbb{R}^{9}
$$

The minimal robot error state error:
$$
\delta \boldsymbol{\chi}{\mathrm{R}}=\left[\delta \mathbf{p}^{\mathrm{T}}, \delta \boldsymbol{\alpha}^{\mathrm{T}}, \delta \mathbf{v}^{\mathrm{T}}, \delta \mathbf{b}{\mathrm{g}}^{\mathrm{T}}, \delta \mathbf{b}_{\mathrm{a}}^{\mathrm{T}}\right]^{\mathrm{T}} \in \mathbb{R}^{15}
$$

We devive the linearized model of the error states:

$$
\delta \dot{\boldsymbol{\chi}}{\mathrm{R}} \approx \mathbf{F}{c}\left(\overline{\mathbf{x}}{\mathrm{R}}\right) \delta \boldsymbol{\chi}{\mathrm{R}}+\mathbf{G}\left(\overline{\mathbf{x}}_{\mathrm{R}}\right) \mathbf{w}
$$

Derivation

Translation

$$\begin{aligned} \delta_{W} \dot{\mathbf{r}}{S} &=\delta \mathbf{C}{WS}{S\mathbf{v}}
\ &=\dot{\mathbf{C}}{WS} { S\mathbf{v} + \mathbf{C}{WS}} {S\mathbf{\dot{v}}}
\ &=-[\delta \boldsymbol{a}]{\times} \mathbf{C}{WS} {S\mathbf{v}}+\mathbf{C}{W S} \delta{S} \mathbf{v} 
\ &=-\left[\mathbf{C}{WS} {S\mathbf{v}}\right]{\times} \delta \boldsymbol{\alpha}+\mathbf{C}{W S} \delta_{S} \mathbf{v} 
\ &=\left[\mathbf{C}{W S}{S\mathbf{v}}\right]{\times} \delta \boldsymbol{a}+\mathbf{C}{W S} \delta_{S} \mathbf{v} \end{aligned}$$

Rotation

$$\because \mathbf{C}{W S}=\left[\mathbf{I}-[\delta \boldsymbol{a}]{\times}\right] \overline{\mathbf{C}}{W S} \quad \therefore[\delta \boldsymbol{a}]{\times}=\mathbf{I}-\mathbf{C}{W S} \overline{\mathbf{C}}{W S}^{T}$$

$$\therefore[\delta \ddot{a}]{\times}=-\dot{\mathbf{C}}{W S} \overline{\mathbf{C}}{W S}^{T}-\mathbf{C}{W S} \dot{\overline{\mathbf{C}}}_{W S}^{T}$$

$$
\begin{aligned} \because \dot{\mathbf{C}}{W S} &=\mathbf{C}{W S} \mathbf{\Omega}, \quad \mathbf{\Omega}=[\mathbf{\omega}]{\times} \ \dot{\overline{\mathbf{C}}}{W S} &=\overline{\mathbf{C}}{W S} \overline{\mathbf{\Omega}}, \quad \overline{\mathbf{\Omega}}=[\overline{\mathbf{\omega}}]{\mathbf{X}} \end{aligned}
$$

$$\begin{aligned} \therefore[\delta \dot{\mathbf{a}}]{\mathrm{X}} &=-\mathbf{C}{W S} \mathbf{\Omega} \overline{\mathbf{C}}{W S}^{T}-\mathbf{C}{W S}\left(\overline{\mathbf{C}}{W S} \overline{\mathbf{\Omega}}\right)^{T}
\\ &=-\mathbf{C}{W S} \mathbf{\Omega} \overline{\mathbf{C}}{W S}^{T}+\mathbf{C}{W S} \overline{\mathbf{\Omega}}^{T} \overline{\mathbf{C}}{W S}^{T}
\\ &=-\left[\mathbf{I}-[\delta \boldsymbol{a}]{\times}\right] \overline{\mathbf{C}}{W S} \mathbf{\Omega} \overline{\mathbf{C}}{W S}^{T}+\left[\mathbf{I}-[\delta \boldsymbol{a}]{\times}\right] \overline{\mathbf{C}}{W S} \overline{\mathbf{\Omega}}^{T} \overline{\mathbf{C}}{W S}^{T} \\
&=-\overline{\mathbf{C}}{W S} \mathbf{\Omega} \overline{\mathbf{C}}{W S}^{T}+[\delta \boldsymbol{a}]{\times} \overline{\mathbf{C}}{W S} \mathbf{\Omega} \overline{\mathbf{C}}{W S}^{T}+\overline{\mathbf{C}}{W S} \overline{\mathbf{\Omega}}^{T} \overline{\mathbf{C}}{W S}^{T}-[\delta \boldsymbol{a}]{\times} \overline{\mathbf{C}}{W S}^{T} \overline{\mathbf{C}}{W S}^{T} \\
&=-\overline{\mathbf{C}}{W S}(\mathbf{\Omega}-\overline{\boldsymbol{\Omega}}) \overline{\mathbf{C}}{W S}^{T}+[\delta \boldsymbol{a}]{\times} \overline{\mathbf{C}}{W S}(\mathbf{\Omega}-\overline{\boldsymbol{\Omega}}) \overline{\mathbf{C}}{W S}^{T} \end{aligned}$$

Let $[\delta \mathbf{\omega}]{\times}=\mathbf{\Omega}-\overline{\mathbf{\Omega}}$, ignore $[\delta \dot{\boldsymbol{a}}]{\times} \approx-\overline{\mathbf{C}}{W S}[\delta \boldsymbol{\omega}]{\times} \overline{\mathbf{C}}_{W S}^{T}$

$${\because[\mathbf{C r}]{\times}=\mathbf{C}[\mathbf{r}]{\times} \mathbf{C}^{T}}$$

$${\therefore[\delta \dot{\boldsymbol{a}}]{\times}=-\left[\overline{\mathbf{C}}{W S} \delta \mathbf{\omega}\right]{\times} \Rightarrow \delta \dot{\boldsymbol{\alpha}}=-\overline{\mathbf{C}}{W S} \delta \boldsymbol{\omega}}$$

$${\because \delta \mathbf{\omega}=\delta\left(\tilde{\mathbf{\omega}}+\mathbf{w}{\mathrm{g}}-\mathbf{b}{\mathrm{g}}\right)=-\delta \mathbf{b}_{\mathrm{g}}}$$

$${\therefore \delta \dot{\boldsymbol{\alpha}}=-\overline{\mathbf{C}}{W S}\left(-\delta \mathbf{b}{\mathrm{g}}\right)=\overline{\mathbf{C}}{W S} \delta \mathbf{b}{\mathrm{g}}}$$

Velocity

$$
\begin{array}{l}{\delta_{S} \dot{\mathbf{v}}=\delta\left({S} \mathbf{a}+\mathbf{w}{a}-\mathbf{b}{a}+\mathbf{C}{S W W} \mathbf{g}-\left({S} \mathbf{\omega}\right) \times{S} \mathbf{v}\right)} \\ {\quad=-\delta \mathbf{b}{a}+\mathbf{C}{S W}[\delta \boldsymbol{a}]{\times W} \mathbf{g}-\left[\delta{S} \mathbf{\omega}\right]{\times S} \mathbf{v}-\left[{S} \mathbf{\omega}\right]{\times} \delta{S} \mathbf{v}} \\ {\quad=-\mathbf{C}{S W}\left[{W} \mathbf{g}\right]{\times} \delta \boldsymbol{\alpha}-\left[-\delta \mathbf{b}{g}\right]{\times S} \mathbf{v}-\left[{S} \boldsymbol{\omega}\right]{\times} \delta{S} \mathbf{v}-\delta \mathbf{b}{a}} \\ {\quad=-\mathbf{C}{S W}\left[{W} \mathbf{g}\right]{\times} \delta \boldsymbol{\alpha}-\left[{S} \mathbf{\omega}\right]{\times} \delta_{S} \mathbf{v}-\left[{S} \mathbf{v}\right]{\times} \delta \mathbf{b}{g}-\delta \mathbf{b}{a}}\end{array}
$$

Gyro Bias

$$\delta \dot{\mathbf{b}}_{g}=0$$

Accelerometer Bias

$$\delta \dot{\mathbf{b}}{\mathrm{a}}=-\frac{1}{\tau} \delta \mathbf{b}{\mathrm{a}}$$

Finally, we get:
$$
\delta \dot{\boldsymbol{\chi}}{\mathrm{R}} \approx \mathbf{F}{c}\left(\overline{\mathbf{x}}{\mathrm{R}}\right) \delta \boldsymbol{\chi}{\mathrm{R}}+\mathbf{G}\left(\overline{\mathbf{x}}{\mathrm{R}}\right) \mathbf{w}
$$
where
$$
\mathbf{F}{c}=\left[\begin{array}{ccccc}{\mathbf{0}{3 \times 3}} & {\left[\mathbf{C}{W S S} \overline{\mathbf{v}}\right]^{\times}} & {\overline{\mathbf{C}}{W S}} & {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} \ {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} & {\overline{\mathbf{C}}{W S}} & {\mathbf{0}{3 \times 3}} \ {\mathbf{0}{3 \times 3}} & {-\overline{\mathbf{C}}{W S}\left[{W \mathbf{B}}\right]^{\times}} & {-\left[{S} \overline{\boldsymbol{\omega}}\right]^{\times}} & {-[s \overline{\mathbf{v}}]^{\times}} & {-\mathbf{I}{3}} \ {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} \ {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} & {\mathbf{0}{3 \times 3}} & {-\frac{1}{\tau} \mathbf{I}{3}}\end{array}\right],
$$
and
$$
\mathbf{G}=\left[\begin{array}{cccc}{\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} \ {\mathbf{I}{3}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} \ {\mathbf{0}} & {\mathbf{I}{3}} & {\mathbf{0}} & {\mathbf{0}} \ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I}{3}} & {\mathbf{0}} \ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I}_{3}}\end{array}\right]
$$

Reference

  1. Leutenegger, S., Lynen, S., Bosse, M., Siegwart, R., & Furgale, P. (2015). Keyframe-based visual–inertial odometry using nonlinear optimization. The International Journal of Robotics Research, 34(3), 314-334.
  2. Savage, Paul G. “Strapdown inertial navigation integration algorithm design part 1: Attitude algorithms.” Journal of guidance, control, and dynamics 21.1 (1998): 19-28.
  3. Savage, Paul G. “Strapdown inertial navigation integration algorithm design part 2: Velocity and position algorithms.” Journal of Guidance, Control, and Dynamics 21.2 (1998): 208-221.
  4. Shin, Eun-Hwan, and Naser El-Sheimy. “An unscented Kalman filter for in-motion alignment of low-cost IMUs.” Position Location and Navigation Symposium, 2004. PLANS 2004. IEEE, 2004.
  5. Sola, Joan. “Quaternion kinematics for the error-state KF.” Laboratoire d’Analyse et d’Architecture des Systemes-Centre national de la recherche scientifique (LAAS-CNRS), Toulouse, France, Tech. Rep (2012).