OpenVINS与MSCKF_VIO RK4积分对比

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{aligned} ω & = ω_{m_{t_{n}}} - b_{g} \\ _{w}^{b} q_{t_{n} + Δ t} & = [\begin{matrix} \frac{ω}{| ω |} \sin (\frac{| ω |}{2} Δ t) \\ \cos (\frac{| ω |}{2} Δ t) \end{matrix}] \otimes_{w}^{b} q_{t_{n}} = (\cos (\frac{| ω |}{2} Δ t) \cdot I_{4 \times 4} + \frac{1}{| ω |} \sin (\frac{| ω |}{2} Δ t) \cdot Ω (ω))_{w}^{b} q_{t_{n}} \\ _{w}^{b} q_{t_{n} + \frac{Δ t}{2}} & = [\begin{matrix} \frac{ω}{| ω |} \sin (\frac{| ω |}{4} Δ t) \\ \cos (\frac{| ω |}{4} Δ t) \end{matrix}] \otimes_{w}^{b} q_{t_{n}} = (\cos (\frac{| ω |}{4} Δ t) \cdot I_{4 \times 4} + \frac{1}{| ω |} \sin (\frac{| ω |}{4} Δ t) \cdot Ω (ω))_{w}^{b} q_{t_{n}} \end{aligned}$ $ω_{w}^{b} q_{t_{n} + Δ t}_{w}^{b} q_{t_{n} + \frac{Δ t}{2}} = ω_{m_{t_{n}}} - b_{g} = \frac{ω}{∣ ω ∣} sin (\frac{∣ ω ∣}{2} Δ t) cos (\frac{∣ ω ∣}{2} Δ t) \otimes_{w}^{b} q_{t_{n}} = (cos (\frac{∣ ω ∣}{2} Δ t) \cdot I_{4 \times 4} + \frac{1}{∣ ω ∣} sin (\frac{∣ ω ∣}{2} Δ t) \cdot Ω (ω))_{w}^{b} q_{t_{n}} = \frac{ω}{∣ ω ∣} sin (\frac{∣ ω ∣}{4} Δ t) cos (\frac{∣ ω ∣}{4} Δ t) \otimes_{w}^{b} q_{t_{n}} = (cos (\frac{∣ ω ∣}{4} Δ t) \cdot I_{4 \times 4} + \frac{1}{∣ ω ∣} sin (\frac{∣ ω ∣}{4} Δ t) \cdot Ω (ω))_{w}^{b} q_{t_{n}}$

输入 $t_n$ 时刻的加速度测量值，对位置和速度进行RK4积分

$\begin{aligned} v_{0} & = v_{t_{n}} \\ k_{1} & = [\begin{matrix} {\dot{p}}_{1} \\ {\dot{v}}_{1} \end{matrix}] = [\begin{matrix} v_{0} \\ R {_{w}^{b} q_{t_{n}}}^{⊤} (a_{m_{t_{n}}} - b_{a}) + g \end{matrix}] \\ k_{2} & = [\begin{matrix} {\dot{p}}_{2} \\ {\dot{v}}_{2} \end{matrix}] = [\begin{matrix} v_{0} + \frac{Δ t}{2} {\dot{v}}_{1} \\ R {_{w}^{b} q_{t_{n} + \frac{Δ t}{2}}}^{⊤} (a_{m_{t_{n}}} - b_{a}) + g \end{matrix}] \\ k_{3} & = [\begin{matrix} {\dot{p}}_{3} \\ {\dot{v}}_{3} \end{matrix}] = [\begin{matrix} v_{0} + \frac{Δ t}{2} {\dot{v}}_{2} \\ R {_{w}^{b} q_{t_{n} + \frac{Δ t}{2}}}^{⊤} (a_{m_{t_{n}}} - b_{a}) + g \end{matrix}] \\ k_{4} & = [\begin{matrix} {\dot{p}}_{4} \\ {\dot{v}}_{4} \end{matrix}] = [\begin{matrix} v_{0} + Δ t {\dot{v}}_{3} \\ R {_{w}^{b} q_{t_{n} + Δ t}}^{⊤} (a_{m_{t_{n}}} - b_{a}) + g \end{matrix}] \end{aligned}$ $v_{0} k_{1} k_{2} k_{3} k_{4} = v_{t_{n}} = [\dot{p}_{1} \dot{v}_{1}] = [v_{0} R {_{w}^{b} q_{t_{n}}}^{⊤} (a_{m_{t_{n}}} - b_{a}) + g] = [\dot{p}_{2} \dot{v}_{2}] = [v_{0} + \frac{Δ t}{2} \dot{v}_{1} R {_{w}^{b} q_{t_{n} + \frac{Δ t}{2}}}^{⊤} (a_{m_{t_{n}}} - b_{a}) + g] = [\dot{p}_{3} \dot{v}_{3}] = [v_{0} + \frac{Δ t}{2} \dot{v}_{2} R {_{w}^{b} q_{t_{n} + \frac{Δ t}{2}}}^{⊤} (a_{m_{t_{n}}} - b_{a}) + g] = [\dot{p}_{4} \dot{v}_{4}] = [v_{0} + Δ t \dot{v}_{3} R {_{w}^{b} q_{t_{n} + Δ t}}^{⊤} (a_{m_{t_{n}}} - b_{a}) + g]$

$[\begin{matrix} q_{t_{n} + Δ t} \\ p_{t_{n} + Δ t} \\ v_{t_{n} + Δ t} \end{matrix}]$ = $[\begin{matrix} _{w}^{b} q_{t_{n} + Δ t} \\ p_{t_{n}} + \frac{Δ t}{6} ({\dot{p}}_{1} + 2 {\dot{p}}_{2} + 2 {\dot{p}}_{3} + {\dot{p}}_{4}) \\ v_{t_{n}} + \frac{Δ t}{6} ({\dot{v}}_{1} + 2 {\dot{v}}_{2} + 2 {\dot{v}}_{3} + {\dot{v}}_{4}) \end{matrix}]$ $q_{t_{n} + Δ t} p_{t_{n} + Δ t} v_{t_{n} + Δ t} =_{w}^{b} q_{t_{n} + Δ t} p_{t_{n}} + \frac{Δ t}{6} (\dot{p}_{1} + 2 \dot{p}_{2} + 2 \dot{p}_{3} + \dot{p}_{4}) v_{t_{n}} + \frac{Δ t}{6} (\dot{v}_{1} + 2 \dot{v}_{2} + 2 \dot{v}_{3} + \dot{v}_{4})$

二、OpenVINS RK4积分

输入 $t_n$ 和 $t_{n+1}$ 时刻的角速度和加速度测量值，计算角加速度和加加速度（jerk）
$\boldsymbol{\alpha} = \frac{(\boldsymbol{\omega}_{m_{t_{n+1}}} - \mathbf{b}_g) - (\boldsymbol{\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} \\$

姿态使用JPL四元数表示，姿态积分时有一个trick，积分并不是从 $t_n$ 时刻的姿态开始的，而是从0姿态开始

$\begin{aligned} v_{0} & = v_{t_{n}} \\ q_{0} & =_{w}^{b} q_{t_{n}} \\ Δ q_{0} & = [\begin{matrix} 0 & 0 & 0 & 1 \end{matrix}] \\ k_{1} & = [\begin{matrix} {\dot{Δ q}}_{1} \\ {\dot{p}}_{1} \\ {\dot{v}}_{1} \end{matrix}] = [\begin{matrix} \frac{1}{2} Ω (ω_{m_{t_{n}}} - b_{g}) Δ q_{0} \\ v_{0} \\ R {Δ q_{0} \otimes q_{0}}^{⊤} (a_{m_{t_{n}}} - b_{a}) - g \end{matrix}] \\ Δ q_{1} & = norm (Δ q_{0} + \frac{Δ t}{2} {\dot{Δ q}}_{1}) \\ k_{2} & = [\begin{matrix} {\dot{Δ q}}_{2} \\ {\dot{p}}_{2} \\ {\dot{v}}_{2} \end{matrix}] = [\begin{matrix} \frac{1}{2} Ω (ω_{m_{t_{n}}} - b_{g} + \frac{Δ t}{2} α) Δ q_{1} \\ v_{0} + \frac{Δ t}{2} {\dot{v}}_{1} \\ R {Δ q_{1} \otimes q_{0}}^{⊤} (a_{m_{t_{n}}} - b_{a} + \frac{Δ t}{2} j) - g \end{matrix}] \\ Δ q_{2} & = norm (Δ q_{0} + \frac{Δ t}{2} {\dot{Δ q}}_{2}) \\ k_{3} & = [\begin{matrix} {\dot{Δ q}}_{3} \\ {\dot{p}}_{3} \\ {\dot{v}}_{3} \end{matrix}] = [\begin{matrix} \frac{1}{2} Ω (ω_{m_{t_{n}}} - b_{g} + \frac{Δ t}{2} α) Δ q_{2} \\ v_{0} + \frac{Δ t}{2} {\dot{v}}_{2} \\ R {Δ q_{2} \otimes q_{0}}^{⊤} (a_{m_{t_{n}}} - b_{a} + \frac{Δ t}{2} j) - g \end{matrix}] \\ Δ q_{3} & = norm (Δ q_{0} + Δ t {\dot{Δ q}}_{3}) \\ k_{4} & = [\begin{matrix} {\dot{Δ q}}_{4} \\ {\dot{p}}_{4} \\ {\dot{v}}_{4} \end{matrix}] = [\begin{matrix} \frac{1}{2} Ω (ω_{m_{t_{n}}} - b_{g} + Δ t α) Δ q_{3} \\ v_{0} + Δ t {\dot{v}}_{3} \\ R {Δ q_{3} \otimes q_{0}}^{⊤} (a_{m_{t_{n}}} - b_{a} + Δ t j) - g \end{matrix}] \end{aligned}$ $v_{0} q_{0} Δ q_{0} k_{1} Δ q_{1} k_{2} Δ q_{2} k_{3} Δ q_{3} k_{4} = v_{t_{n}} =_{w}^{b} q_{t_{n}} = [0001] = \dot{Δ q}_{1} \dot{p}_{1} \dot{v}_{1} = \frac{1}{2} Ω (ω_{m_{t_{n}}} - b_{g}) Δ q_{0} v_{0} R {Δ q_{0} \otimes q_{0}}^{⊤} (a_{m_{t_{n}}} - b_{a}) - g = norm (Δ q_{0} + \frac{Δ t}{2} \dot{Δ q}_{1}) = \dot{Δ q}_{2} \dot{p}_{2} \dot{v}_{2} = \frac{1}{2} Ω (ω_{m_{t_{n}}} - b_{g} + \frac{Δ t}{2} α) Δ q_{1} v_{0} + \frac{Δ t}{2} \dot{v}_{1} R {Δ q_{1} \otimes q_{0}}^{⊤} (a_{m_{t_{n}}} - b_{a} + \frac{Δ t}{2} j) - g = norm (Δ q_{0} + \frac{Δ t}{2} \dot{Δ q}_{2}) = \dot{Δ q}_{3} \dot{p}_{3} \dot{v}_{3} = \frac{1}{2} Ω (ω_{m_{t_{n}}} - b_{g} + \frac{Δ t}{2} α) Δ q_{2} v_{0} + \frac{Δ t}{2} \dot{v}_{2} R {Δ q_{2} \otimes q_{0}}^{⊤} (a_{m_{t_{n}}} - b_{a} + \frac{Δ t}{2} j) - g = norm (Δ q_{0} + Δ t \dot{Δ q}_{3}) = \dot{Δ q}_{4} \dot{p}_{4} \dot{v}_{4} = \frac{1}{2} Ω (ω_{m_{t_{n}}} - b_{g} + Δ t α) Δ q_{3} v_{0} + Δ t \dot{v}_{3} R {Δ q_{3} \otimes q_{0}}^{⊤} (a_{m_{t_{n}}} - b_{a} + Δ t j) - g$

$[\begin{matrix} q_{t_{n} + Δ t} \\ p_{t_{n} + Δ t} \\ v_{t_{n} + Δ t} \end{matrix}]$ = $[\begin{matrix} norm (Δ q_{0} + \frac{Δ t}{6} ({\dot{Δ q}}_{1} + 2 {\dot{Δ q}}_{2} + 2 {\dot{Δ q}}_{3} + {\dot{Δ q}}_{4})) \otimes_{w}^{b} q_{t_{n}} \\ p_{t_{n}} + \frac{Δ t}{6} ({\dot{p}}_{1} + 2 {\dot{p}}_{2} + 2 {\dot{p}}_{3} + {\dot{p}}_{4}) \\ v_{t_{n}} + \frac{Δ t}{6} ({\dot{v}}_{1} + 2 {\dot{v}}_{2} + 2 {\dot{v}}_{3} + {\dot{v}}_{4}) \end{matrix}]$ $q_{t_{n} + Δ t} p_{t_{n} + Δ t} v_{t_{n} + Δ t} = norm (Δ q_{0} + \frac{Δ t}{6} (\dot{Δ q}_{1} + 2 \dot{Δ q}_{2} + 2 \dot{Δ q}_{3} + \dot{Δ q}_{4})) \otimes_{w}^{b} q_{t_{n}} p_{t_{n}} + \frac{Δ t}{6} (\dot{p}_{1} + 2 \dot{p}_{2} + 2 \dot{p}_{3} + \dot{p}_{4}) v_{t_{n}} + \frac{Δ t}{6} (\dot{v}_{1} + 2 \dot{v}_{2} + 2 \dot{v}_{3} + \dot{v}_{4})$

@leatherwang

二零二二年九月十二日
相关阅读:
Docker in Docker原理与实战
 标准库unsafe：带你突破golang中的类型限制
 牛牛的等差数列（思维 + 线段树）
微信公众号后台管理
 IODE海洋数据门户平台简述
 Spring_IOC
tcpdump进行IP抓包
 EIP-3664合约研究笔记04--Metacore平台功能分析
 【uniapp】uniapp开发app在线预览pdf文件
 vivo z1换电池笔记
原文地址：https://blog.csdn.net/hzwwpgmwy/article/details/126816415

一、MSCKF_VIO RK4积分

二、OpenVINS RK4积分