FAST-LIO 雅可比推导方法一

论文地址 arxiv.org/pdf/2010.08196.pdf

1.基本公式

在两个李代数中有一个为小量时，可以用BCH近似表达：

2.状态的定义

误差状态的定义如下：

由于是ESKF(误差状态卡尔曼滤波器)，因此我们要估计的是误差状态。

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

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

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

分子部分，令

链式求导展开：

导数是单位阵。分子上的东西全消掉了，只剩误差项，所以是单位阵。

构建一个更通用的形式来求解：

对两个小量进行展开近似，因为小量趋近于0，所以使用泰勒公式0处展开：

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

原文是 https://zhuanlan.zhihu.com/p/670130932

写的很清晰，记录下来，值的学习