OpenVINS与MSCKF_VIO RK4积分对比

VIO系统在使用IMU测量值进行状态预测时，需要将连续时间的微分方程离散化为差分方程，离散化的本质是积分，根据数值积分近似程度不同，常用的有欧拉法、中点法和四阶龙格库塔法等，OpenVINS和MSCKF_VIO虽然都使用RK4积分，但具体代码实现却有所区别。

一、MSCKF_VIO RK4积分

---

姿态使用JPL四元数表示，MSCKF_VIO姿态不使用RK4积分，而是直接使用Zeroth Order Quaternion Integrator积分器，输入是$t_n$时刻的角速度测量值，得到$t_n+\Delta t$和$t_n+\frac{\Delta t}{2}$时刻的姿态，
$$\begin{array}{rl}\mathbf{\omega }& ={\mathbf{\omega }}_{m_{t_n}}-{\mathbf{b}}_g\\ {}_w^b{q}_{t_n+\Delta t}& =\left[\begin{array}{c}\frac{\mathbf{\omega }}{|\mathbf{\omega }|}\sin \left(\frac{|\mathbf{\omega }|}{2}\Delta t\right)\\ \cos \left(\frac{|\mathbf{\omega }|}{2}\Delta t\right)\end{array}\right]\otimes {}_w^b{q}_{t_n}=\left(\cos \left(\frac{|\mathbf{\omega }|}{2}\Delta t\right)\cdot {\mathbf{I}}_{4\times 4}+\frac{1}{|\mathbf{\omega }|}\sin \left(\frac{|\mathbf{\omega }|}{2}\Delta t\right)\cdot \mathbf{\Omega }(\mathbf{\omega })\right){}_w^b{q}_{t_n}\\ {}_w^b{q}_{t_n+\frac{\Delta t}{2}}& =\left[\begin{array}{c}\frac{\mathbf{\omega }}{|\mathbf{\omega }|}\sin \left(\frac{|\mathbf{\omega }|}{4}\Delta t\right)\\ \cos \left(\frac{|\mathbf{\omega }|}{4}\Delta t\right)\end{array}\right]\otimes {}_w^b{q}_{t_n}=\left(\cos \left(\frac{|\mathbf{\omega }|}{4}\Delta t\right)\cdot {\mathbf{I}}_{4\times 4}+\frac{1}{|\mathbf{\omega }|}\sin \left(\frac{|\mathbf{\omega }|}{4}\Delta t\right)\cdot \mathbf{\Omega }(\mathbf{\omega })\right){}_w^b{q}_{t_n}\end{array}$$

输入$t_n$时刻的加速度测量值，对位置和速度进行RK4积分
v 0 = v t n k 1 = [ p ˙ 1 v ˙ 1 ] = [ v 0 R { w b q t n } ⊤ ( a m t n − b a ) + g ] k 2 = [ p ˙ 2 v ˙ 2 ] = [ v 0 + Δ t 2 v ˙ 1 R { w b q t n + Δ t 2 } ⊤ ( a m t n − b a ) + g ] k 3 = [ p ˙ 3 v ˙ 3 ] = [ v 0 + Δ t 2 v ˙ 2 R { w b q t n + Δ t 2 } ⊤ ( a m t n − b a ) + g ] k 4 = [ p ˙ 4 v ˙ 4 ] = [ v 0 + Δ t v ˙ 3 R { w b q t n + Δ t } ⊤ ( a m t n − b a ) + g ] \begin{aligned} \mathbf{v}_{0} &= \mathbf{v}_{t_{n}} \\ k_{1} &= \begin{bmatrix} \dot{\mathbf{p}}_{1} \\ \dot{\mathbf{v}}_{1} \end{bmatrix} = \begin{bmatrix} \mathbf{v}_{0} \\ \mathbf{R} \{ {}^b_w q_{{t_n}} \}^\top (\mathbf{a}_{m_{t_n}} - \mathbf{b}_a) + \mathbf{g} \end{bmatrix} \\ k_{2} &= \begin{bmatrix} \dot{\mathbf{p}}_{2} \\ \dot{\mathbf{v}}_{2} \end{bmatrix} = \begin{bmatrix} {\mathbf{v}}_{0} + \frac{\Delta t}{2} \dot{\mathbf{v}}_{1} \\ \mathbf{R} \{ {}^b_w q_{{{t_n} + \frac{\Delta t}{2}}} \}^\top (\mathbf{a}_{m_{t_n}} - \mathbf{b}_a) + \mathbf{g} \end{bmatrix} \\ k_{3} &= \begin{bmatrix} \dot{\mathbf{p}}_{3} \\ \dot{\mathbf{v}}_{3} \end{bmatrix} = \begin{bmatrix} {\mathbf{v}}_{0} + \frac{\Delta t}{2} \dot{\mathbf{v}}_{2} \\ \mathbf{R} \{ {}^b_w q_{{{t_n} + \frac{\Delta t}{2}}} \}^\top (\mathbf{a}_{m_{t_n}} - \mathbf{b}_a) + \mathbf{g} \end{bmatrix} \\ k_{4} &= \begin{bmatrix} \dot{\mathbf{p}}_{4} \\ \dot{\mathbf{v}}_{4} \end{bmatrix} = \begin{bmatrix} {\mathbf{v}}_{0} + \Delta t \dot{\mathbf{v}}_{3} \\ \mathbf{R} \{ {}^b_w q_{{{t_n} + \Delta t}} \}^\top (\mathbf{a}_{m_{t_n}} - \mathbf{b}_a) + \mathbf{g} \end{bmatrix} \end{aligned} v0​k1​k2​k3​k4​​=vtn​​=[p˙​1​v˙1​​]=[v0​R{wb​qtn​​}⊤(amtn​​​−ba​)+g​]=[p˙​2​v˙2​​]=[v0​+2Δt​v˙1​R{wb​qtn​+2Δt​​}⊤(amtn​​​−ba​)+g​]=[p˙​3​v˙3​​]=[v0​+2Δt​v˙2​R{wb​qtn​+2Δt​​}⊤(amtn​​​−ba​)+g​]=[p˙​4​v˙4​​]=[v0​+Δtv˙3​R{wb​qtn​+Δt​}⊤(amtn​​​−ba​)+g​]​

$$\left[\begin{array}{c}{\mathbf{q}}_{t_n+\Delta t}\\ {\mathbf{p}}_{t_n+\Delta t}\\ {\mathbf{v}}_{t_n+\Delta t}\end{array}\right]=\left[\begin{array}{c}{}_w^b{\mathbf{q}}_{t_n+\Delta t}\\ {\mathbf{p}}_{t_n}+\frac{\Delta t}{6}\left({\dot{\mathbf{p}}}_1+2{\dot{\mathbf{p}}}_2+2{\dot{\mathbf{p}}}_3+{\dot{\mathbf{p}}}_4\right)\\ {\mathbf{v}}_{t_n}+\frac{\Delta t}{6}\left({\dot{\mathbf{v}}}_1+2{\dot{\mathbf{v}}}_2+2{\dot{\mathbf{v}}}_3+{\dot{\mathbf{v}}}_4\right)\end{array}\right]$$

二、OpenVINS RK4积分

---

输入$t_n$和$t_{n+1}$时刻的角速度和加速度测量值，计算角加速度和加加速度（jerk）
$$\begin{gathered} \mathbf{\alpha }=\frac{({\mathbf{\omega }}_{m_{t_{n+1}}}-{\mathbf{b}}_g)-({\mathbf{\omega }}_{m_{t_n}}-{\mathbf{b}}_g)}{\Delta t} \\ \mathbf{j}=\frac{({\mathbf{a}}_{m_{t_{n+1}}}-{\mathbf{b}}_a)-({\mathbf{a}}_{m_{t_n}}-{\mathbf{b}}_a)}{\Delta t} \end{gathered}$$

姿态使用JPL四元数表示，姿态积分时有一个trick，积分并不是从$t_n$时刻的姿态开始的，而是从0姿态开始
v 0 = v t n q 0 = w b q t n Δ q 0 = [ 0 0 0 1 ] k 1 = [ Δ q ˙ 1 p ˙ 1 v ˙ 1 ] = [ 1 2 Ω ( ω m t n − b g ) Δ q 0 v 0 R { Δ q 0 ⊗ q 0 } ⊤ ( a m t n − b a ) − g ] Δ q 1 = norm ( Δ q 0 + Δ t 2 Δ q ˙ 1 ) k 2 = [ Δ q ˙ 2 p ˙ 2 v ˙ 2 ] = [ 1 2 Ω ( ω m t n − b g + Δ t 2 α ) Δ q 1 v 0 + Δ t 2 v ˙ 1 R { Δ q 1 ⊗ q 0 } ⊤ ( a m t n − b a + Δ t 2 j ) − g ] Δ q 2 = norm ( Δ q 0 + Δ t 2 Δ q ˙ 2 ) k 3 = [ Δ q ˙ 3 p ˙ 3 v ˙ 3 ] = [ 1 2 Ω ( ω m t n − b g + Δ t 2 α ) Δ q 2 v 0 + Δ t 2 v ˙ 2 R { Δ q 2 ⊗ q 0 } ⊤ ( a m t n − b a + Δ t 2 j ) − g ] Δ q 3 = norm ( Δ q 0 + Δ t Δ q ˙ 3 ) k 4 = [ Δ q ˙ 4 p ˙ 4 v ˙ 4 ] = [ 1 2 Ω ( ω m t n − b g + Δ t α ) Δ q 3 v 0 + Δ t v ˙ 3 R { Δ q 3 ⊗ q 0 } ⊤ ( a m t n − b a + Δ t j ) − g ] \begin{aligned} \mathbf{v}_{0} &= \mathbf{v}_{t_{n}} \\ \mathbf{q}_0 &= {}^b_w \mathbf{q}_{t_n} \\ \Delta \mathbf{q}_0 &= \begin{bmatrix} 0 & 0 & 0 & 1 \end{bmatrix} \\ k_{1} &= \begin{bmatrix} \dot{\Delta \mathbf{q}}_{1} \\ \dot{\mathbf{p}}_{1} \\ \dot{\mathbf{v}}_{1} \end{bmatrix} = \begin{bmatrix} \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}_{m_{t_{n}}} - \mathbf{b}_g) \Delta \mathbf{q}_0 \\ \mathbf{v}_0 \\ \mathbf R \{\Delta \mathbf{q}_0 \otimes \mathbf{q}_0 \}^\top (\mathbf{a}_{m_{t_{n}}} - \mathbf{b}_a) - \mathbf{g} \end{bmatrix} \\ \Delta \mathbf{q}_1 &= \text{norm} \left( \Delta \mathbf{q}_0 + \frac{\Delta t}{2} \dot{\Delta \mathbf{q}}_{1} \right) \\ k_{2} &= \begin{bmatrix} \dot{\Delta \mathbf{q}}_{2} \\ \dot{\mathbf{p}}_{2} \\ \dot{\mathbf{v}}_{2} \end{bmatrix} = \begin{bmatrix} \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}_{m_{t_{n}}} - \mathbf{b}_g + \frac{\Delta t}{2} \boldsymbol{\alpha}) \Delta \mathbf{q}_1 \\ \mathbf{v}_0 + \frac{\Delta t}{2} \dot{\mathbf{v}}_{1} \\ \mathbf R \{ \Delta \mathbf{q}_1 \otimes \mathbf{q}_0 \}^\top (\mathbf{a}_{m_{t_{n}}} - \mathbf{b}_a + \frac{\Delta t}{2} \mathbf{j}) - \mathbf{g} \end{bmatrix} \\ \Delta \mathbf{q}_2 &= \text{norm} \left( \Delta \mathbf{q}_0 + \frac{\Delta t}{2} \dot{\Delta \mathbf{q}}_{2} \right) \\ k_{3} &= \begin{bmatrix} \dot{\Delta \mathbf{q}}_{3} \\ \dot{\mathbf{p}}_{3} \\ \dot{\mathbf{v}}_{3} \end{bmatrix} = \begin{bmatrix} \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}_{m_{t_{n}}} - \mathbf{b}_g + \frac{\Delta t}{2} \boldsymbol{\alpha}) \Delta \mathbf{q}_2 \\ \mathbf{v}_0 + \frac{\Delta t}{2} \dot{\mathbf{v}}_{2} \\ \mathbf R \{ \Delta \mathbf{q}_2 \otimes \mathbf{q}_0 \}^\top (\mathbf{a}_{m_{t_{n}}} - \mathbf{b}_a + \frac{\Delta t}{2} \mathbf{j}) - \mathbf{g} \end{bmatrix} \\ \Delta \mathbf{q}_3 &= \text{norm} \left( \Delta \mathbf{q}_0 + \Delta t \dot{\Delta \mathbf{q}}_{3} \right) \\ k_{4} &= \begin{bmatrix} \dot{\Delta \mathbf{q}}_{4} \\ \dot{\mathbf{p}}_{4} \\ \dot{\mathbf{v}}_{4} \end{bmatrix} = \begin{bmatrix} \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}_{m_{t_{n}}} - \mathbf{b}_g + \Delta t \boldsymbol{\alpha}) \Delta \mathbf{q}_3 \\ \mathbf{v}_0 + \Delta t \dot{\mathbf{v}}_{3} \\ \mathbf R \{ \Delta \mathbf{q}_3 \otimes \mathbf{q}_0 \}^\top (\mathbf{a}_{m_{t_{n}}} - \mathbf{b}_a + \Delta t \mathbf{j}) - \mathbf{g} \end{bmatrix} \end{aligned} v0​q0​Δq0​k1​Δq1​k2​Δq2​k3​Δq3​k4​​=vtn​​=wb​qtn​​=[0​0​0​1​]= ​Δq˙​1​p˙​1​v˙1​​ ​= ​21​Ω(ωmtn​​​−bg​)Δq0​v0​R{Δq0​⊗q0​}⊤(amtn​​​−ba​)−g​ ​=norm(Δq0​+2Δt​Δq˙​1​)= ​Δq˙​2​p˙​2​v˙2​​ ​= ​21​Ω(ωmtn​​​−bg​+2Δt​α)Δq1​v0​+2Δt​v˙1​R{Δq1​⊗q0​}⊤(amtn​​​−ba​+2Δt​j)−g​ ​=norm(Δq0​+2Δt​Δq˙​2​)= ​Δq˙​3​p˙​3​v˙3​​ ​= ​21​Ω(ωmtn​​​−bg​+2Δt​α)Δq2​v0​+2Δt​v˙2​R{Δq2​⊗q0​}⊤(amtn​​​−ba​+2Δt​j)−g​ ​=norm(Δq0​+ΔtΔq˙​3​)= ​Δq˙​4​p˙​4​v˙4​​ ​= ​21​Ω(ωmtn​​​−bg​+Δtα)Δq3​v0​+Δtv˙3​R{Δq3​⊗q0​}⊤(amtn​​​−ba​+Δtj)−g​ ​​

$$\left[\begin{array}{c}{\mathbf{q}}_{t_n+\Delta t}\\ {\mathbf{p}}_{t_n+\Delta t}\\ {\mathbf{v}}_{t_n+\Delta t}\end{array}\right]=\left[\begin{array}{c}\text{norm}\left(\Delta {\mathbf{q}}_0+\frac{\Delta t}{6}\left({\dot{\Delta \mathbf{q}}}_1+2{\dot{\Delta \mathbf{q}}}_2+2{\dot{\Delta \mathbf{q}}}_3+{\dot{\Delta \mathbf{q}}}_4\right)\right)\otimes {}_w^b{\mathbf{q}}_{t_n}\\ {\mathbf{p}}_{t_n}+\frac{\Delta t}{6}\left({\dot{\mathbf{p}}}_1+2{\dot{\mathbf{p}}}_2+2{\dot{\mathbf{p}}}_3+{\dot{\mathbf{p}}}_4\right)\\ {\mathbf{v}}_{t_n}+\frac{\Delta t}{6}\left({\dot{\mathbf{v}}}_1+2{\dot{\mathbf{v}}}_2+2{\dot{\mathbf{v}}}_3+{\dot{\mathbf{v}}}_4\right)\end{array}\right]$$

---

@leatherwang

二零二二年九月十二日

---