@[toc]

一、运动模型

激光：点到面的距离残差
视觉：重投影残差
\boxplus和\boxminus: 流型上的加减法

1.1 连续时间的运动模型

a_m和\omega_m是IMU的加速度和角速度的原始测量值
b_a和b_g是加速度和角速度的bias
n_a = n_{ba}和n_g = n_{bg} 是随机游走的高斯白噪声

1.2 离散时间的IMU模型

定义第i个IMU测量的系统状态为

分别表示IMU在世界坐标系下的旋转，位置。相机和IMU的外参(旋转+位置)。IMU在世界坐标系下的速度。IMU的bias
得到IMU的状态传播方程：

二、ESIKF（迭代误差状态卡尔曼滤波器）

2.1 系统误差状态

\hat{x}表示(先验)估计值（在lidar或者camera测量修正前的值） \check{x}后验估计（在lidar或者camera测量修正前的值）
误差状态定义为：

其中的高斯分布是一个先验的假设分布。

2.2 传播方程

每收到一个IMU测量，设置公式(5)中的过程噪声为0，因此状态传播方程为：

因此状态误差的传播过程表示为：

Q是噪声的对角协方差矩阵。

通过如下简化
得到两个F矩阵

2.3 计算后验误差分布

由公式（15）我们可以很容易得到后验误差分布为

三、激光测量

残差的建模和LOAM相似
{}^Lp_j 表示在lidar坐标系下的第j个点。当我们知道x_{k+1}时刻的状态估计\hat{x}_{k+1},可以将这个点变换到世界坐标系下：

{}^Gp_j={ }^{G} \check{\mathbf{R}}_{I_{k+1}}\left({ }^{I} \mathbf{R}_{L}{ }^{L} \mathbf{p}_{j}+{ }^{I} \mathbf{p}_{L}\right)+{ }^{G} \check{\mathbf{p}}_{I_{k+1}}

残差定义为scan的点到对应map的平面的距离：

u_j表示平面法向量。
假设每个激光点都由测量噪声$n_j$，所以测量值和真值有如下关系：

因此公式(17)中的残差可以由状态误差表示如下：

四、相机测量

{ }^{C} \mathbf{p}_{s}=\left[\begin{array}{ll}u_{s} & v_{s}\end{array}\right]^{T} \in \mathcal{C}_{k+1}表示第s个特征点的像素坐标，其对应的3D空间点为{}^CP_s:

\pi(·)是相机投影模型
测量噪声为：

残差的误差状态表示:

五、ESIKF的correct过程

最大后验估计通过最小化：先验状态残差、激光测量、相机测量实现
定义：

根据fast-lio中的卡尔曼增益公式：

更新状态估计：

下一个循环：