Fast-LIO论文阅读

一、系统流程与符号说明

1、定义流形中的＋－

从上面的定义，很容易验证

IEKF

符号	含义
${\mathbf{z}}_j^{\kappa }={\mathbf{G}}_j\left({}^G{\mathbf{p}}_{f_j}^{\kappa }-{}^G{\mathbf{q}}_j\right)$	${\mathbf{z}}_j^{\kappa }$ LOAM的点到线、 点到面误差作为残差
G()	计算点到线、点到面残差的函数
${}^G{\mathbf{q}}_j$	真值
$x$	IMU积分的当前位姿
${\mathbf{P}}_k$	IMU预积分的状态协方差
${\mathbf{P}}_k$	状态变量协方差
${}^{L_j}{\mathbf{n}}_{f_j}$	雷达测量噪声
${}^{L_j}{\mathbf{p}}_{f_j}^{\mathrm{g}\mathrm{t}}={}^{L_j}{\mathbf{p}}_{f_j}-{}^{L_j}{\mathbf{n}}_{f_j}$	雷达真值 = 测量值 - 噪声
T	变换矩阵

观察方程：

用它在${\mathbf{x}}_k^{\kappa }$处的一阶近似来逼近上面的方程会得到

${\mathbf{H}}_j^{\kappa }$ 是$h_j()$关于 ${\mathbf{x}}_k^{\kappa }$的雅克比
$v_j$ 测量噪声，这里定义为了一个白噪声

$J^k$是$\left({\mathbf{x}}_k^{\kappa }\square {\mathbf{x}}_k^{\kappa }\right)\square {\mathbf{x}}_k$ 的雅克比

迭代初值为： ${\mathbf{x}}_k^{\kappa }={\mathbf{x}}_k$, then ${\mathbf{J}}^{\kappa }=\mathbf{I}$
将(15)中的先验与(14)中的后验分布相结合，得到最大后验估计(MAP)：

得到迭代卡尔曼公式
注意： 这里的公式个人推出来好像不是（I-KH），而是（-KH）
R是雷达测量噪声矩阵
重复上面的步骤直至收敛,得到

其中卡尔曼增益公式（18）可以用下面的公式替换，他们是等价的。这避免了对测量矩阵H求逆

算法步骤