FAST-LIO论文公式解析

一、基础知识

1.1 指数映射/对数映射

1.2 BCH近似

1.3 SO3伴随

可以把李群和李代数的位置做交换：

二、Fx推导：

论文里的状态定义：

误差状态的定义如下：

• 由于是ESKF(误差状态卡尔曼滤波器)，因此我们要估计的是误差状态。卡尔曼滤波器中是误差状态转移方程的线性化系数(要把非线性->线性才能用，这部分可以参考一些EKF,ESKF的文章)。

在FAST-LIO中对于误差状态转移方程的描述以及线性化结果如下：

误差的估计值 = 状态的真值 - 状态的估计值。然后由状态转移方程从i+1时刻递推到i时刻。

接下来线性化呗，我们都知道KF用的是一阶泰勒展开，那么这里的F_x就是一阶导，又因为误差和噪声的期望都为0，所以这使得我们可以在0处展开，因此只需要求解在0处的导数即可：

带入公式1 FAST-LIO状态误差方程：

只看分子部分，我们令

链式求导展开

那么就可以计算出：

这里比较难计算的是第三行第一项：

至此，已经求得这三项，按照相加相乘起来就是最后的结果。