VINS-Fusion 是港科大开源的VIO算法，VINS-Fusion 是基于视觉惯性的滑动窗口的紧耦合SLAM系统。2017年港科大开源了VINS-Mono, 支持单目和IMU的slam系统，此次开源系统支持如下传感器：
单目 + IMU
双目
双目 + IMU
双目 + IMU + GPS
详细请参考https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
Calibration
Target
- Calibrate the pose relationship between left and right camera
- Calibrate the pose relationship left camera between and IMU
Generate tag
kalibr_create_target_pdf --type apriltag --nx 4 --ny 6 --tsize 0.05 --tspace 0.2
Record rosbag
rosrun topic_tools throttle messages /camera/fisheye1/image_rect_raw 4.0 /camera/fisheye1/image_raw
rosrun topic_tools throttle messages /camera/fisheye2/image_rect_raw 4.0 /camera/fisheye2/image_raw
IMU intrinsic
kalibr requires imu data to be calibrated by intrinsic parameters by default.The intrinsic parameters calibration tool uses imu_utils, and analize allan using imu_tk.
rosrun imu_utils imu_an _imu_topic:=/motion_camera/imu/data_raw _imu_name:=t265 _data_save_path:=./ _max_time_min:=30 _max_cluster:=100
rosbag play imu-static.bag
Multiple camera calibration
kalibr_calibrate_cameras --approx-sync 0.01 --show-extraction --target ./april_4x6.yaml --models omni-radtan omni-radtan --bag ./t265_april_0.5_0.2-static.bag --topics /motion_camera/left/image_raw /motion_camera/right/image_raw
Camera IMU calibration
kalibr_calibrate_imu_camera --show-extraction --bag ./t265-april_0.5_0.2-dynamic.bag --cam ./camchain-.t265-april_0.5_0.2-static.yaml --imu ./imu.yaml --target ./april_4x6.yaml
VINS 框架
坐标系统于符号定义
VINS系统会有三个坐标系统，分别为世界坐标系，Body/IMU坐标系和相机坐标系。其中：
\((.)^{w}\)定义为世界坐标系
\((.)^{b}\)定义为Body/IMU坐标系
\((.)^{c}\)定义为相机坐标系
\(q_{b}^{w} \ p_{b}^{w}\)定义为IMU坐标系到世界坐标系的旋转和平移变换
\(b_{k}\)定义为捕获到第k个图像时刻的IMU坐标系
\(c_{k}\)定义为捕获到第k个图像时刻的相机坐标系
\(g^{w} = [0, 0, g]^{T}\)定义为在世界坐标系下的重力向量
\(\hat{(.)}\)定义为具有噪声的观测量或估计量
初始化
陀螺仪Bias
通过视觉和陀螺仪积分计算得到的两个旋转误差做最小二乘得到陀螺仪bias。
\[
\min _{\delta b_{W}} \sum_{k \in B}\left\|q_{b_{k+1}}^{c_{0}^{-1}} \otimes q_{b_{k}}^{c_{0}} \otimes \gamma_{b_{k+1}}^{b_{k}}\right\|^{2}
\]
其中：
\[
\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{b_{w}}^{\gamma} \delta b_{w}}\end{array}\right]
\]
两项测量几乎相等得到：
\[
\begin{aligned}
&\hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{b_{w}}^{\gamma} \delta b_{w}}\end{array}\right]=q_{b_{k}}^{c_{0}^{-1}} \otimes q_{b_{k+1}}^{c_{0}} \\
&\left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{b_{w}}^{\gamma} \delta b_{w}}\end{array}\right]=\hat{\gamma}_{b_{k+1}}^{b_{k}^{-1}} \otimes q_{b_{k}}^{c_{0}^{-1}} \otimes q_{b_{k+1}}^{c_{0}}
\end{aligned}
\]
取虚部得到:
\[
J_{b_{w}}^{\gamma} \delta b_{w} = 2\left(\hat{\gamma}_{b_{k+1}}^{b_{k}^{-1}} \otimes q_{b_{k}}^{c_{0}^{-1}} \otimes q_{b_{k+1}}^{c_{0}}\right).vec
\]
即可求取出\(\delta b_{w}\).
代码实现上采用LDLT分解, 对于一个最小二乘问题\(Ax = b\)等价于求解正规方程\(A^TAx = A^Tb\).
对于多次观测的情况下：
\[
\begin{aligned}
&H = \left[ \begin{array}{l}{H_{1}} \\ {H_{2}} \\ \cdots \\ {H_{n}}\end{array}\right]
b = \left[ \begin{array}{l}{b_{1}} \\ {b_{2}} \\ \cdots \\ {b_{n}}\end{array}\right] \\
&\Rightarrow \\
&H^THx = H^Tb \\
&\Rightarrow \\
&\left[ \begin{array}{l}{H_{1}} \\ {H_{2}} \\ \cdots \\ {H_{n}}\end{array}\right]^T
\left[ \begin{array}{l}{H_{1}} \\ {H_{2}} \\ \cdots \\ {H_{n}}\end{array}\right]
= \left[ \begin{array}{l}{H_{1}} \\ {H_{2}} \\ \cdots \\ {H_{n}}\end{array}\right]^T
\left[ \begin{array}{l}{b_{1}} \\ {b_{2}} \\ \cdots \\ {b_{n}}\end{array}\right] \\
&\Rightarrow \\
&\sum_{i =0}^{n} H_i^T H_i = \sum_{i = 0}^{n} H_i^Tb_i
\end{aligned}
\]
紧耦合滑动窗口后端优化
对于滑动窗口内，系统需要求解的状态向量如下所示：
\[
\begin{aligned} \mathcal{X} &=\left[\mathrm{x}_{0}, \mathrm{x}_{1}, \ldots \mathrm{x}_{n}, \mathrm{x}_{c}^{b}, \lambda_{0}, \lambda_{1}, \ldots \lambda_{m}\right] \\ \mathrm{x}_{k} &=\left[\mathbf{p}_{b_{k}}^{w}, \mathbf{v}_{b_{k}}^{w}, \mathbf{q}_{b_{k}}^{w}, \mathbf{b}_{a}, \mathbf{b}_{g}\right], k \in[0, n] \\ \mathbf{x}_{c}^{b} &=\left[\mathbf{p}_{c}^{b}, \mathbf{q}_{c}^{b}\right] \end{aligned}
\]
其中包含 n+1 个所有相机的状态（包括位置、朝向、速度、加速度计 bias 和陀螺仪 bias）、Camera 和IMU之间的外参、m+1个Map Point的逆深度。
最小二乘目标函数
\[
\min _{\mathcal{X}} \left\{ \left\|\mathbf{r}_{p}-\mathbf{H}_{p} \mathcal{X}\right\|^{2} + \sum_{k \in \mathcal{B}}\left\|\mathbf{r}_{\mathcal{B}}\left(\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \mathcal{X}\right)\right\|_{\mathbf{P}_{b_{k+1}}^{b}}^{2} + \sum_{(l, j) \in \mathcal{C}} \rho \left(\left\|\mathbf{r}_{\mathcal{C}}\left(\hat{\mathbf{z}}_{l}^{c_{j}}, \mathcal{X}\right)\right\|_{\mathbf{P}_{l}^{c_{j}}}^{2}\right) \right\}
\]
\[
\rho(s)=\left\{\begin{array}{ll}{s} & {s \leq 1} \\ {2 \sqrt{s}-1} & {s>1}\end{array}\right.
\]
可以将目标函数拆分成三个部分，都用马氏距离表示，分别为：
视觉重投影残差
误差方程
\[
\begin{aligned}
&\mathbf{r}_{\mathcal{C}}\left(\hat{\mathbf{z}}_{l}^{c_{j}}, \mathcal{X}\right)=\left[\mathbf{b}_{1} \mathbf{b}_{2}\right]^{T} \cdot\left(\frac{\mathcal{P}_{l}^{c_{j}}}{\left\|\mathcal{P}_{l}^{c_{j}}\right\|} - \hat{\overline{\mathcal{P}}}_{l}^{c_{j}}\right) \\
&\hat{\overline{\mathcal{P}}}_{l}^{c_{j}} =\pi_{c}^{-1} \left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{j}}} \\ {\hat{v}_{l}^{c_{j}}}\end{array}\right] \right) \\
&\mathcal{P}_{l}^{c_{j}} = \mathbf{R}_{b}^{c}\left(\mathbf{R}_{w}^{b_{j}}\left(\mathbf{R}_{b_{i}}^{w} \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right) + P_{b_i}^w - P_{b_j}^w\right) - P_{c}^{b} \right) \\
\end{aligned}
\]
相比于传统的小孔成像模型归一化平面的重投影误差，这里定义了单位球上的重投影误差。通过将当前图像观测到的landmark变换到当前单位圆上（预测值）与当前图像特征点（观测值）相减，定义出误差方程。
优化变量
\[
\left[p_{b_{i}}^{w}, q_{b_{i}}^{w}\right]^T,\left[p_{b_{j}}^{w}, q_{b_{j}}^{w}\right]^T, \left[p_{c}^{b},q_{c}^{b}\right]^T, \lambda_{l}
\]
Jacobian
对应的jacobian分别如下：
\[
\begin{aligned}
&J[0]^{3 \times 6} = \left[\frac{\partial r_{C}}{\partial p_{b_{i}}^{w}}, \frac{\partial r_{C}}{\partial q_{b_{i}}^{w}}\right] =\left[ \begin{array}{cc}{q_{b}^{c} q_{w}^{b_{j}}} & {-q_{b}^{c} q_{w}^{b_{j}} q_{b_{i}}^{w}\left[q_{c}^{b} \frac{\overline{P}_{l}^{c_{l}}}{\lambda_{l}}+p_{c}^{b}\right]_{ \times}}\end{array}\right] \\
&J[1]^{3 \times 6} = \left[\frac{\partial r_{C}}{\partial p_{b_{j}}^{w}}, \frac{\partial r_{C}}{\partial q_{b_{j}}^{w}}\right] = \left[ \begin{array}{ccc}{-q_{b}^{c} q_{w}^{b_{j}}} & {q_{b}^{c} q_{w}^{b_{j}}} {\left[q_{b_{i}}^{w}\left(q_{c}^{b} \frac{\overline{P}_{l}^{c_{i}}}{\lambda_{l}}+p_{c}^{b}\right)+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right]_{ \times}}\end{array}\right] \\
&J[2]^{3 \times 6} = \left[\frac{\partial r_{C}}{\partial p_{c}^{b}}, \frac{\partial r_{C}}{\partial q_{c}^{b}}\right] = \left[ \begin{array}{ccc} q_{b}^{c}\left(q_{w}^{b_{j}} q_{b i}^{w}-I_{3 * 3}\right) & -q_{b}^{c} q_{w}^{b_{j}} q_{b_{i}}^{w} q_{c}^{b}\left[\frac{\overline{P}_{l}^{c_{i}}}{\lambda_{l}}\right]_{ \times}+\left[q_{b}^{c}\left(q_{w}^{b_{j}}\left(q_{b_{i}}^{w} p_{c}^{b}+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right)-p_{c}^{b}\right)\right]_{\times} \end{array}\right] \\
&J[3]^{3 \times 1} = \left[\frac{\partial r_{C}}{\partial \lambda_{l}}\right] = -q_{b}^{c} q_{w}^{b_{j}} q_{b_{i}}^{w} q_{c}^{b} \frac{\overline{P}_{l}^{c_{i}}}{\lambda_{l}^{2}} \\
\end{aligned}
\]
下面对\(J[0]^{3 \times 6} = \left[\frac{\partial r_{C}}{\partial p_{b_{i}}^{w}}, \frac{\partial r_{C}}{\partial q_{b_{i}}^{w}}\right]\)进行推导：
关于位置部分，由复合函数求导法则：
\[
\frac{\partial r_{C}}{\partial p_{b_{i}}^{w}} = q_{b}^{c} q_{w}^{b_{j}}
\]
关于旋转部分:
参考Quaternion kinematics for the error-state Kalman filter Perturbations, uncertainties, noise部分公式189, 190, 191, 192 :
\[
\begin{aligned}
&\tilde{\mathbf{R}}_{\mathcal{L}}=\mathbf{R}_{\mathcal{L}} \cdot \operatorname{Exp}\left(\Delta \boldsymbol{\phi}_{\mathcal{L}}\right) \\
&\Delta \mathbf{R}_{\mathcal{L}} \approx \mathbf{I}+\left[\Delta \boldsymbol{\phi}_{\mathcal{L}}\right]_{ \times} \\
\end{aligned}
\]
和Cross Product：
\[
\begin{aligned}
&\mathbf{a} \times \mathbf{b}=-(\mathbf{b} \times \mathbf{a}) \\
&\mathbf{a} \times \mathbf{b}=[\mathbf{a}]_{ \times} \mathbf{b}=\left[ \begin{array}{ccc}{0} & {-a_{3}} & {a_{2}} \\ {a_{3}} & {0} & {-a_{1}} \\ {-a_{2}} & {a_{1}} & {0}\end{array}\right] \left[ \begin{array}{l}{b_{1}} \\ {b_{2}} \\ {b_{3}}\end{array}\right] \\
&\Rightarrow \\
&[a]_\times b = a \times b = -(b \times a) = - [b]_\times a
\end{aligned}
\]
由复合函数求导法则对误差函数分解，设：
\[
\begin{aligned}
f(q_{b_i}^{w}) &= {R}_{b_{i}}^{w} \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right) \\
\end{aligned}
\]
求导，得：
\[
\begin{aligned}
\frac{\partial f\left(q_{b_i}^{w}\right)}{\partial q_{b_{i}}^{w}} &= \lim _{\delta \theta_{b_{i}}^{w} \rightarrow 0} \frac{ {R}_{b_{i}}^{w} exp(\left[\delta \theta_{b_i}^{w}\right]_{\times}) \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right) - {R}_{b_{i}}^{w} \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right) }{\delta \theta_{b_{i}}^{w}} \\
&\approx \lim _{\delta \theta_{b_{i}}^{w} \rightarrow 0} \frac{ {R}_{b_{i}}^{w} (I^{3 \times 3} + \left[ \delta \theta_{b_i}^{w} \right]_\times) \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right) - {R}_{b_{i}}^{w} \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right) }{\delta \theta_{b_{i}}^{w}} \\
&= \lim _{\delta \theta_{b_{i}}^{w} \rightarrow 0} \frac{ {R}_{b_{i}}^{w} \left[ \delta \theta_{b_i}^{w} \right]_\times \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right) }{\delta \theta_{b_{i}}^{w}} \\
&= \lim _{\delta \theta_{b_{i}}^{w} \rightarrow 0} \frac{ -{R}_{b_{i}}^{w} \delta \theta_{b_i}^{w} \left[ \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right)\right]_\times }{\delta \theta_{b_{i}}^{w}} \\
&= -{R}_{b_{i}}^{w} \left[ \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right)\right]_\times \\
\end{aligned}
\]
则：
\[
\frac{\partial r_{C}}{\partial q_{b_{i}}^{w}} = -q_{b}^{c} q_{w}^{b_{j}} {R}_{b_{i}}^{w} \left[ \left( \mathbf{R}_{c}^{b} \frac{1}{\lambda_{l}} \pi_{c}^{-1}\left(\left[ \begin{array}{c}{\hat{u}_{l}^{c_{i}}} \\ {\hat{v}_{l}^{c_{i}}}\end{array}\right]\right) + P_{c}^{b} \right)\right]_\times
\]
协方差
对于视觉的协方差，为重投影误差，假设为1.5像素，则信息矩阵为协方差矩阵的逆：
\[
\Sigma_{c}^{-1}=\left(\frac{1.5}{f} I_{2 \times 2}\right)^{-1}=\frac{f}{1.5} I_{2 \times 2}
\]
IMU 残差
IMU是惯性测量单元，在VINS中，IMU具有两个设备，分别为陀螺仪和加速度计，其中陀螺仪测量IMU坐标系下的旋转角速度，加速度计测量IMU坐标系下的线性加速度（包括重力加速度）。
\[
\begin{aligned}
&\hat{\mathbf{a}}_{t}=\mathbf{a}_{t}+\mathbf{b}_{a_{t}}+\mathbf{R}_{w}^{t} \mathbf{g}^{w}+\mathbf{n}_{a} \\
&\hat{\boldsymbol{\omega}}_{t}=\boldsymbol{\omega}_{t}+\mathbf{b}_{w_{t}}+\mathbf{n}_{w} \\
\ \\
&\dot{\mathbf{b}}_{a_{t}}=\mathbf{n}_{b_{a}}, \quad \dot{\mathbf{b}}_{w_{t}}=\mathbf{n}_{b_{w}}
\end{aligned}
\]
其中\(b_{a} \ b_{w}\)为零偏，服从随机游走，由于元器件内部机械、温度造成，\(n_{a} \ n_{w}\)为白噪声，由于AD转换器件测量噪声造成。
IMU 动力学方程
一个刚体在同一个惯性坐标系下进行平移运动，其平移量对时间的一阶导和二阶导即速度和加速度：
\[
\begin{array}{c}{
\dot{p}^{w}=v^{w} \\
{\dot{v}^{w}=a^{w}} \\
\dot{q}=\frac{1}{2} \mathbf{q} \otimes \left[ \begin{array}{c}{0} \\ {\boldsymbol{\omega}_{\mathcal{L}}}\end{array}\right]
}\end{array}
\]
预积分
下一次的状态可以通过当前的状态迭代求解，如下：
\[
\begin{aligned}
&\mathbf{p}_{b_{k+1}}^{w}=\mathbf{p}_{b_{k}}^{w}+\mathbf{v}_{b_{k}}^{w} \Delta t_{k}+\iint_{t \in\left[t_{k}, t_{k+1}\right\}}\left(\mathbf{R}_{t}^{w}\left(\hat{\mathbf{a}}_{t}-\mathbf{b}_{a_{i}}-\mathbf{n}_{a}\right)-\mathbf{g}^{w}\right) d t^{2} \\
&\mathbf{v}_{b_{k+1}}^{w}=\mathbf{v}_{b_{k}}^{w}+\int_{t \in\left[t_{k}, t_{k+1}\right]}\left(\mathbf{R}_{t}^{w}\left(\hat{\mathbf{a}}_{t}-\mathbf{b}_{a_{t}}-\mathbf{n}_{a}\right)-\mathbf{g}^{w}\right) d t \\
&\mathbf{q}_{b_{k+1}}^{w}=\mathbf{q}_{b_{k}}^{w} \otimes \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} \Omega\left(\hat{\omega}_{t}-\mathbf{b}_{w_{t}}-\mathbf{n}_{w}\right) \mathbf{q}_{t}^{b_{k}} d t \\
&\Omega(\omega)=\left[ \begin{array}{cc}{-\lfloor\omega\rfloor_{ \times}} & {\omega} \\ {-\omega^{T}} & {0}\end{array}\right],\lfloor\omega\rfloor_{ \times}=\left[ \begin{array}{ccc}{0} & {-\omega_{z}} & {\omega_{y}} \\ {\omega_{z}} & {0} & {-\omega_{x}} \\ {-\omega_{y}} & {\omega_{x}} & {0}\end{array}\right] \\
\end{aligned}
\]
对于优化算法需要迭代求解变量\(R_{t}^{w}\)就会反复计算上式的积分过程，因此把左右都乘一个旋转\(R_{w}^{b_k}\)将积分转到\(b_{k}\)坐标系下。
\[
\begin{aligned}
&\mathbf{R}_{w}^{b_{k}} \mathbf{p}_{b_{k+1}}^{w}=\mathbf{R}_{w}^{b_{k}}\left(\mathbf{p}_{b_{k}}^{w}+\mathbf{v}_{b_{k}}^{w} \Delta t_{k}-\frac{1}{2} \mathbf{g}^{w} \Delta t_{k}^{2}\right)+\boldsymbol{\alpha}_{b_{k+1}}^{b_{k}} \\
&\mathbf{R}_{w}^{b_{k}} \mathbf{v}_{b_{k+1}}^{w}=\mathbf{R}_{w}^{b_{k}}\left(\mathbf{v}_{b_{k}}^{w}-\mathbf{g}^{w} \Delta t_{k}\right)+\boldsymbol{\beta}_{b_{k+1}}^{b_{k}} \\
&\mathbf{q}_{w}^{b_{k}} \otimes \mathbf{q}_{b_{k+1}}^{w}=\gamma_{b_{k+1}}^{b_{k}} \\
\end{aligned}
\]
其中：
\[
\begin{aligned}
&\alpha_{b_{k+1}}^{b_{k}}=\iint_{t \in\left[t_{k}, t_{k+1}\right]} \mathbf{R}_{t}^{b_{k}}\left(\hat{\mathbf{a}}_{t}-\mathbf{b}_{a_{t}}-\mathbf{n}_{a}\right) d t^{2} \\
&\boldsymbol{\beta}_{b_{k+1}}^{b_{k}}=\int_{t \in\left[t_{k}, t_{k+1}\right]} \mathbf{R}_{t}^{b_{k}}\left(\hat{\mathbf{a}}_{t}-\mathbf{b}_{a_{t}}-\mathbf{n}_{a}\right) d t \\
&\gamma_{b_{k+1}}^{b_{k}}=\int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} \Omega\left(\hat{\omega}_{t}-\mathbf{b}_{w_{t}}-\mathbf{n}_{w}\right) \gamma_{t}^{b_{k}} d t \\
\end{aligned}
\]
zero-order hold 欧拉积分（取第i时刻值的斜率乘以时间差加上i时刻的初值，就得到i+1时刻的值）离散形式为：
\[
\begin{aligned}
&\hat{\boldsymbol{\alpha}}_{i+1}^{b_{k}}=\hat{\boldsymbol{\alpha}}_{i}^{b_{k}}+\hat{\boldsymbol{\beta}}_{i}^{b_{k}} \delta t+\frac{1}{2} \mathbf{R}\left(\hat{\boldsymbol{\gamma}}_{i}^{b_{k}}\right)\left(\hat{\mathbf{a}}_{i}-\mathbf{b}_{a_{i}}\right) \delta t^{2} \\
&\hat{\boldsymbol{\beta}}_{i+1}^{b_{k}}=\hat{\boldsymbol{\beta}}_{i}^{b_{k}}+\mathbf{R}\left(\hat{\boldsymbol{\gamma}}_{i}^{b_{k}}\right)\left(\hat{\mathbf{a}}_{i}-\mathbf{b}_{a_{i}}\right) \delta t \\
&\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes \left[ \begin{array}{c}{1} \\ {\frac{1}{2}\left(\hat{\omega}_{i}-\mathbf{b}_{w_{i}}\right) \delta t}\end{array}\right]
\end{aligned}
\]
first-order hold 中值积分（斜率取得是i和i+1中点的时刻斜率）离散形式为：
\[
\begin{aligned}
&\hat{\alpha}_{i+1}^{b_{k}}=\hat{\alpha}_{i}^{b_{k}}+\hat{\beta}_{i}^{b_{k}} \delta t+\frac{1}{2} \overline{\hat{a}}_{i} \delta t^{2} \\
&\hat{\beta}_{i+1}^{b_{k}}=\hat{\beta}_{i}^{b_{k}}+\overline{\hat{a}}_{i} \delta t \\
&\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes \hat{\gamma}_{i+1}^{i}=\hat{\gamma}_{i}^{b_{k}} \otimes \left[ \begin{array}{c}{1} \\ {\frac{1}{2}\overline{\hat{\omega}}_{i} \delta t}\end{array}\right] \\
&\overline{\hat{a}}_{i}=\frac{1}{2}\left[q_{i}\left(\hat{a}_{i}-b_{a_{i}}\right)+q_{i+1}\left(\hat{a}_{i+1}-b_{a_{i}}\right)\right] \\
&\overline{\hat{\omega}}_{i}=\frac{1}{2}\left(\hat{\omega}_{i}+\hat{\omega}_{i+1}\right)-b_{\omega_{i}}
\end{aligned}
\]
即为预积分项。
其中预积分项与陀螺仪和加速度计的偏置相关，可以写成一阶泰勒展开近似：
\[
\begin{aligned}
&\boldsymbol{\alpha}_{b_{k+1}}^{b_{k}} \approx \hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}}+\mathbf{J}_{b_{a}}^{\alpha} \delta \mathbf{b}_{a_{k}}+\mathbf{J}_{b_{w}}^{\alpha} \delta \mathbf{b}_{w_{k}} \\
&\boldsymbol{\beta}_{b_{k+1}}^{b_{k}} \approx \hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}}+\mathbf{J}_{b_{a}}^{\beta} \delta \mathbf{b}_{a_{k}}+\mathbf{J}_{b_{w}}^{\beta} \delta \mathbf{b}_{w_{k}} \\
&\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes \left[ \begin{array}{c}{1} \\ {\frac{1}{2} \mathbf{J}_{b_{w}}^{\gamma} \delta \mathbf{b}_{w_{k}}}\end{array}\right] \\
\end{aligned}
\]
误差状态方程
对于IMU误差状态向量:
\[
\delta X =
[\delta P \quad \delta v \quad \delta \theta
\quad \delta b_a \quad \delta b_g]^T
\in \mathbb{R}^{15 \times 1}
\]
由Quaternion kinematics for the error-state Kalman filter ：
对于中值积分, 误差状态方程为：
\[
\dot{\delta X_k} =
\begin{cases}
\dot{\delta \theta_{k}} =&
-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times} \delta \theta_{k}-\delta b_{g_{k}}+\frac{n_{w0}+n_{w1}}{2} \\
\dot{\delta\beta_{k}} =&
-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta \\
&-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}((I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta_{k} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t) \\
&-\frac{1}{2}q_{k}\delta b_{a_{k}}-\frac{1}{2}q_{k+1}\delta b_{a_{k}}-\frac{1}{2}q_{k}n_{a0}-\frac{1}{2}q_{k}n_{a1} \\
\dot{\delta\alpha_{k}} =&
-\frac{1}{4}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta\delta t \\
&-\frac{1}{4}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}((I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t)\delta t \\
&-\frac{1}{4}q_{k}\delta b_{a_{k}}\delta t-\frac{1}{4}q_{k+1}\delta b_{a_{k}}\delta t-\frac{1}{4}q_{k}n_{a0}\delta t-\frac{1}{4}q_{k}n_{a1}\delta t \\
\dot{\delta b_{a_k}} =& n_{b_a} \\
\dot{\delta b_{g_k}} =& n_{b_g}
\end{cases}
\]
写成矩阵形式：
\[
\dot{\delta X_k} = F \delta X_k + Gn
\]
因此：
\[
\begin{aligned}
\delta X_{k+1}
&= \delta X_k + \dot{\delta X_k} \delta t \\
&= \delta X_k + (F \delta X_k + Gn) \delta t \\
&= (I + F \delta t) \delta X_k + (G \delta t) n
\end{aligned}
\]
展开：
\[
\begin{aligned}
\begin{bmatrix}
\delta \alpha_{k+1}\\
\delta \theta_{k+1}\\
\delta \beta_{k+1} \\
\delta b_{a{}{k+1}} \\
\delta b_{g{}{k+1}}
\end{bmatrix}&=\begin{bmatrix}
I & f_{01} &\delta t & -\frac{1}{4}(q_{k}+q_{k+1})\delta t^{2} & f_{04}\\
0 & I-[\frac{w_{k+1}+w_{k}}{2}-b_{wk}]_{\times } \delta t & 0 & 0 & -\delta t \\
0 & f_{21}&I & -\frac{1}{2}(q_{k}+q_{k+1})\delta t & f_{24}\\
0 & 0& 0&I &0 \\
0& 0 & 0 & 0 & I
\end{bmatrix}
\begin{bmatrix}
\delta \alpha_{k}\\
\delta \theta_{k}\\
\delta \beta_{k} \\
\delta b_{a{}{k}} \\
\delta b_{g{}{k}}
\end{bmatrix} \\
&+
\begin{bmatrix}
\frac{1}{4}q_{k}\delta t^{2}& v_{01}& \frac{1}{4}q_{k+1}\delta t^{2} & v_{03} & 0 & 0\\
0& \frac{1}{2}\delta t & 0 & \frac{1}{2}\delta t &0 & 0\\
\frac{1}{2}q_{k}\delta t& v_{21}& \frac{1}{2}q_{k+1}\delta t & v_{23} & 0 & 0 \\
0 & 0 & 0 & 0 &\delta t &0 \\
0& 0 &0 & 0 &0 & \delta t
\end{bmatrix}
\begin{bmatrix}
n_{a0}\\
n_{w0}\\
n_{a1}\\
n_{w1}\\
n_{ba}\\
n_{bg}
\end{bmatrix}
\end{aligned}
\]
其中：
\[
\begin{aligned}
f_{01}&=-\frac{1}{4}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta t^{2}-\frac{1}{4}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}(I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t)\delta t^{2} \\
f_{21}&=-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta t-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}(I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t)\delta t \\
f_{04}&=\frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})(-\delta t) \\
f_{24}&=\frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t)(-\delta t) \\
v_{01}&=\frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \\
v_{03}&=\frac{1}{4}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \\
v_{21}&=\frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t \\
v_{23}&=\frac{1}{2}(-q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta t^{2})\frac{1}{2}\delta t
\end{aligned}
\]
令:
\[
\begin{aligned}
F' &= I + F \delta t & \in \mathbb{R}^{15 \times 15} \\
V &= G \delta t & \in \mathbb{R}^{15 \times 18}
\end{aligned}
\]
则简写为:
\[
\delta X_{k+1} = F' \delta X_k + V n
\]
最后得到IMU预积分测量关于IMU Bias的雅克比矩阵\(J_{k+1}\)、IMU预积分测量的协方差矩阵\(P_{k+1}\)和 噪声的协方差矩阵\(Q\)，初始状态下的雅克比矩阵和协方差矩阵为单位阵和零矩阵。
\[
\begin{aligned}
J_{b_k} &= I \\
P_{b_{k}}^{b_k} &= 0 \\
J_{t+\delta t} &= F' J_t = (I + F_t \delta t) J_t, \quad t \in [k, k+1] \\
P_{t+\delta t}^{b_k} &= F' P_t^{b_k} F'^T + V Q V^T \\
&= (I + F_t \delta t) P_{t}^{b_k} (I + F_t \delta t) + (G_t \delta t) Q (G_t \delta t) \\
Q &= \text{diag}(
\sigma_{a_0}^2 \quad \sigma_{\omega_0}^2 \quad
\sigma_{a_1}^2 \quad \sigma_{\omega_1}^2 \quad
\sigma_{b_a}^2 \quad \sigma_{b_g}^2) \in \mathbb{R}^{18 \times 18}
\end{aligned}
\]
此过程可以和kalman filter 对比：
误差方程
根据IMU积分公式，预测值与测量值相减即可得到误差方程。
\[
\mathbf{r}_{\mathcal{B}}\left(\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \mathcal{X}\right)
=\left[ \begin{array}{c}{\delta \boldsymbol{\alpha}_{b_{k+1}}^{b_{k}}} \\ {\delta \boldsymbol{\theta}_{b_{k+1}}^{b_{k}}} \\{\delta \boldsymbol{\beta}_{b_{k+1}}^{b_{k}}} \\ {\delta \mathbf{b}_{a}} \\ {\delta \mathbf{b}_{g}}\end{array}\right]
=\left[ \begin{array}{c}{\mathbf{R}_{w}^{b_{k}}\left(\mathbf{p}_{b_{k+1}}^{w}-\mathbf{p}_{b_{k}}^{w}+\frac{1}{2} \mathbf{g}^{w} \Delta t_{k}^{2}-\mathbf{v}_{b_{k}}^{w} \Delta t_{k}\right)-\hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}}} \\ {2\left[\left(\hat{\gamma}_{b_{k+1}}^{b_{k}}\right)^{-1}\otimes\mathbf{q}_{b_{k}}^{w^{-1}} \otimes \mathbf{q}_{b_{k+1}}^{w}\right]_{x y z}} \\
{\mathbf{R}_{w}^{b_{k}}\left(\mathbf{v}_{b_{k+1}}^{w}+\mathbf{g}^{w} \Delta t_{k}-\mathbf{v}_{b_{k}}^{w}\right)-\hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}}} \\
{\mathbf{b}_{a b_{k+1}}-\mathbf{b}_{a b_{k}}} \\ {\mathbf{b}_{w b_{k+1}}-\mathbf{b}_{w b_{k}}}\end{array}\right]
\]
其中，\(\delta \theta_{b_{k + 1}}^{b_k}\)为相邻两个时刻的轴角误差。对于微小旋转，设旋转轴为\(n\)旋转角度为\(\theta\)：
\[
q = [w, x, y, z]^T = [cos\frac{\theta}{2}, n_x sin\frac{\theta}{2}, n_y sin\frac{\theta}{2}, n_z sin\frac{\theta}{2}]^T \\
2[q]_{xyz} = 2nsin\frac{\theta}{2} \approx 2n \cdot \frac{\theta}{2} = n\theta
\]
优化变量
\[
\left[p_{b_{k}}^{w}, q_{b_{k}}^{w}\right]^{T},\left[v_{b_{k}}^{w}, b_{a_{k}}, b_{\omega_{k}}\right]^{T}, \left[p_{b_{k+1}}^{w}, q_{b_{k+1}}^{w}\right]^{T},\left[v_{b_{k+1}}^{w}, b_{a_{k+1}}, b_{\omega_{k+1}}\right]^{T}
\]
Jacobian
\[
\begin{aligned}
&J[0]^{15 \times 7} = \left[\frac{\partial r_{B}}{\partial p_{b_{k}}^{w}}, \frac{\partial r_{B}}{\partial q_{b_{k}}^{w}}\right] = \begin{bmatrix}
-q^{b_{k}}_{w} & [R^{b_{k}}_{w}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w}+\frac{1}{2}g^{w}\Delta t^{2}-v_{b_{k}}^{w}\Delta t)]_{\times }\\
0 & -[q_{b_{k+1}}^{w^{-1}}q^{w}_{b_{k}}]_{L}[\hat{\gamma }^{b_{k}}_{b_{k+1}}]_{R}\\
0 & [R^{b_{k}}_{w}(v^{w}_{b_{k+1}}+g^{w}\Delta t-v_{b_{k}}^{w})]_{\times } \\
0 & 0
\end{bmatrix}\\
&J[1]^{15 \times 9} = \left[\frac{\partial r_{B}}{\partial v_{b_{k}}^{w}}, \frac{\partial r_{B}}{\partial b_{a_{k}}^{w}}, \frac{\partial r_{B}}{\partial b_{w{k}}^{w}}\right] = \begin{bmatrix}
-q^{b_{k}}_{w}\Delta t & -J^{\alpha }_{b_{a}} & -J^{\alpha }_{b_{a}}\\
0 & 0 & -[q_{b_{k+1}}^{w^{-1}}\otimes q^{w}_{b_{k}}\otimes \hat{\gamma }^{b_{k}}_{b_{k+1}}]_{L}J^{\gamma}_{b_{w}}\\
-q^{b_{k}}_{w} & -J^{\beta }_{b_{a}} & -J^{\beta }_{b_{a}}\\
0& -I &0 \\
0 &0 &-I
\end{bmatrix}\\
&J[2]^{15 \times 7} = \left[\frac{\partial r_{B}}{\partial p_{b_{k+1}}^{w}}, \frac{\partial r_{B}}{\partial q_{b_{k+1}}^{w}}\right] = \begin{bmatrix}
q^{b_{k}}_{w} &0\\
0 & [\hat{\gamma }^{b_{k}^{-1}}_{b_{k+1}}\otimes q_{w}^{b_{k}}\otimes q_{b_{k+1}}^{w}]_{L} \\
0 & 0 \\
0 & 0 \\
0 & 0
\end{bmatrix}\\
&J[3]^{15 \times 9} = \left[\frac{\partial r_{B}}{\partial v_{b_{k+1}}^{w}}, \frac{\partial r_{B}}{\partial b_{a_{k+1}}^{w}}, \frac{\partial r_{B}}{\partial b_{w{k+1}}^{w}}\right] = \begin{bmatrix}
0 &0 & 0\\
0 & 0 &0 \\
q^{b_{k}}_{w} & 0 & 0\\
0& I &0 \\
0 &0 &I
\end{bmatrix}\\
\end{aligned}
\]
Marginalization
Schur complement
在线性代数与矩阵论中，一个矩阵的子矩阵之舒尔补是一个与其余子阵同样大小的矩阵，定义如下：假设一个 (p+q)×(p+q)的矩阵M被分为A, B, C, D四个部分，分别是p×p、p×q、q×p以及q×q的矩阵，也就是说：
\[
M=\left[ \begin{array}{ll}{A} & {B} \\ {C} & {D}\end{array}\right]
\]
并且D是可逆的矩阵。则D在矩阵中的舒尔补是：
\[
A-B D^{-1} C
\]
假设方程组：
\[
\begin{array}{l}{A x+B y=a} \\ {C x+D y=b}\end{array}
\]
其中x以及a是p维的列向量，而y以及b则是q维的列向量。矩阵A、B、C、D则同上面假设。将第二个方程左乘上矩阵\(BD^{{-1}}\)，并将得到后的方程与第一个相减，就得到：
\[
\left(A-B D^{-1} C\right) x=a-B D^{-1} b
\]
因此，如果可以知道D以及D的舒尔补的逆矩阵，就可以解出未知量x之后带入第二个方程\(Cx+Dy=b\)就可以解出y。这样，就将\((p+q)\times (p+q)\)矩阵的求逆问题转化成了分别求解一个p×p矩阵以及一个 q×q矩阵的逆矩阵的问题。这样就大大减低了复杂度（计算量）。实际上，这要求矩阵D满足足够好的条件，以使得算法得以成立。
对于高斯牛顿非线性最小二乘的增量迭代方程：
\[
J(x)^{T} J(x) \delta x=-J(x)^{T} f(x) => H \delta x=b
\]
令\(\delta x=\left[\delta x_{1}, \delta x_{2}\right]^{T}\)其中\(\delta x_1\)是需要marginalize掉的变量，\(\delta x_2\)是和\(\delta x_1\)有约束的变量，因此：
\[
\left[ \begin{array}{c}{\Lambda_{a}, \Lambda_{b}} \\ {\Lambda_{b}^{T}, \Lambda_{c}}\end{array}\right] \left[ \begin{array}{c}{\delta x_{1}} \\ {\delta x_{2}}\end{array}\right]=\left[ \begin{array}{l}{b_{1}} \\ {b_{2}}\end{array}\right]
\]
进行舒尔消元，得到：
\[
\left[ \begin{array}{cc}{I,} & {0} \\ {-\Lambda_{b}^{T} \Lambda_{a}^{-1}, I}\end{array}\right] \left[ \begin{array}{c}{\Lambda_{a}, \Lambda_{b}} \\ {\Lambda_{b}^{T}, \Lambda_{c}}\end{array}\right] \left[ \begin{array}{c}{\delta x_{1}} \\ {\delta x_{2}}\end{array}\right]=\left[ \begin{array}{c}{I,} & {0} \\ {-\Lambda_{b}^{T} \Lambda_{a}^{-1}, I}\end{array}\right] \left[ \begin{array}{l}{b_{1}} \\ {b_{2}}\end{array}\right]
\]
即：
\[
\left[ \begin{array}{cc}{\Lambda_{a},} & {\Lambda_{b}} \\ {0,} & {\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}}\end{array}\right] \left[ \begin{array}{c}{\delta x_{1}} \\ {\delta x_{2}}\end{array}\right]=\left[ \begin{array}{c}{b_{1}} \\ {b_{2}-\Lambda_{b}^{T} \Lambda_{a}^{-1} b_{1}}\end{array}\right]
\]
由此即可得到在不求取\(\delta x_1\)的情况下求取\(\delta x_2\)的增量迭代公式：
\[
\underbrace{ \left(\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}\right)}_{H}\underbrace{\delta x_{2}}_{\delta x} = \underbrace{b_{2}-\Lambda_{b}^{T} \Lambda_{a}^{-1} b_{1}}_{b}
\]
通过观察上式发现，舒尔补消去了变量\(\delta x_{1}\)并把约束关系叠加到了变量\(\delta x_{2}\)上，因此保留了先验信息（prior）。
VINS中的Marginalization策略
实验
本次实验传感器为Realsense T265 (stereo fisheye + IMU)。
Reference
A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors, Tong Qin, Jie Pan, Shaozu Cao, Shaojie Shen, aiXiv
A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors, Tong Qin, Shaozu Cao, Jie Pan, Shaojie Shen, aiXiv
Online Temporal Calibration for Monocular Visual-Inertial Systems, Tong Qin, Shaojie Shen, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS, 2018)
VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Shaojie Shen, IEEE Transactions on Robotics
IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation
A Runge-Kutta numerical integration methods
Quaternion kinematics for the error-state Kalman filter
https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
https://github.com/ethz-asl/kalibr
https://mynt-eye-s-sdk.readthedocs.io/zh_CN/latest/src/slam/how_to_use_kalibr.html
https://blog.csdn.net/qq_39907831/article/details/80983623
https://www.zybuluo.com/Xiaobuyi/note/866099
https://github.com/gaochq/VINS-Mono/tree/comment
《主流VIO技术综述及VINS解析--崔华坤》
《VINS论文推导及代码解析--崔华坤》
https://fzheng.me/2016/11/20/imu_model_eq/
http://www.cnblogs.com/buxiaoyi/p/7541974.html
https://fzheng.me/2018/03/22/okvis-marginalization-base/
https://en.wikipedia.org/wiki/Schur_complement
https://blog.csdn.net/heyijia0327/article/details/52822104
https://github.com/ethz-asl/okvis
https://blog.csdn.net/Pancheng1/article/details/81008081
https://blog.csdn.net/weixin_38358435/article/details/82423511
https://www.cnblogs.com/buxiaoyi/p/8660854.html
https://cggos.github.io/slam/vinsmono_note_cg.html
https://www.youtube.com/watch?v=UQ4uJX7UDB8
https://image-matching-workshop.github.io/leaderboard/
https://www.visuallocalization.net/
Benchmarking Long-term Visual Localization