r3live:从r2live进行公式解读

总述

r2live提出的框架由基于滤波方法的里程计模块和基于因子图的优化模块组成。

系统框图：

基于滤波器的里程计利用LiDAR、相机和惯性传感器的测量值，在误差状态迭代卡尔曼滤波器框架内估计状态；
为了进一步改善视觉测量，利用因子图优化来优化局部滑动窗口内的视觉路标点；
r2live框架的运动方程由IMU传播建立，观测方程由相机和雷达建立，迭代卡尔曼滤波得到的值其实就是MAP的结果，十四讲介绍过MAP就是最小化噪声的二次型，也就是马氏距离；
噪声包括三项：

1. 运动方程
2. lidar观测方程
3. camer观测方程

运动方程得到先验，观测方程得到后验；

结合先验分布、激光雷达测量的后验分布和视觉测量的后验分布，得到了δˇxk+1的最大后验(MAP)估计，最重要的公式：

什么是运动？
	从 K-1 时刻到 K 时刻，机器人的位置 X是如何变化的。
什么是观测？
    假设机器人在 K 时刻，于 x_{k} 处探测到了某一个路标 y_{j} ,我们要考虑这件事情是如何用数学语言来描述的。
    
《视觉SLAM十四讲》中把SLAM问题的数学描述抽象成了两个数学方程，一个方程叫做运动方程、另一个方程叫做观测方程。

1. 运动方程

先验为

所以运动方程的噪声项为
但是需要求得是状态x的后验分布，就是
所以在ESIKF中的优化变量是
然而运动方程的噪声项是
并没有优化变量，所以必须建立两者的联系，故有了矩阵H，关系如下：

（15）本质上对δˇxk+1施加了一个先验分布，如下：

这样就得到了式（30）的第一项，H是一个雅可比矩阵，线性化的点为δˇxk+1=0，因为在非线性化系统迭代卡尔曼滤波中，每次迭代的点是在先验均值处然后将后验变为先验进入下次迭代，迭代点在先验均值处，直到收敛，在ESIKF中也就是δˇxk+1=0的点。

2. lidar观测方程

观测数据为：
观测矩阵：
噪声：
就得到了式（30）的第二项

2. camera观测方程

同理