Fast-Lio 论文解读

一、系统框架

二、fast-lio的突出点

1. 采用紧耦合迭代卡尔曼滤波器融合激光雷达特征点IMU测量；
2. 提出一个正式的反向传播补偿运动失真的过程；
3. 计算卡尔曼增益及其与常规增益的等价性卡尔曼增益公式。新公式有一个计算复杂性取决于状态维度，而不是测量尺寸
4. combine this ICP method with a point- to-edge distance and developed a LiDAR odometry and mapping (LOAM) framework

三、系统模型

3.1 代码中的系统状态：

MTK_BUILD_MANIFOLD(state_ikfom,
((vect3, pos)) 	// 0-2
((SO3, rot))	// 3-5
((SO3, offset_R_L_I))	// 6-8
((vect3, offset_T_L_I))	// 9-11
((vect3, vel))	// 	12-14
((vect3, bg))	//	15-17
((vect3, ba))	//	18-20
((S2, grav))	//	21-22);

3.2 State Estimation

3.3  Forward Propagation

3.4 The propagated covariance

3.5 Backward Propagation and motion compensation

3.6 Residual Computation

3.7 Iterated State update

 3.8 prior distribution

3.9 the whole process

四、代码解读

论文与代码的中的状态量不对应的解答：

1: get_f 函数

//vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);
Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in)
{
	Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
	vect3 omega;
	in.gyro.boxminus(omega, s.bg);
	vect3 a_inertial = s.rot * (in.acc-s.ba); 
	for(int i = 0; i < 3; i++ ){
		res(i) = s.vel[i];
		res(i + 3) =  omega[i]; 
		res(i + 12) = a_inertial[i] + s.grav[i]; 
	}
	return res;
}

2: df_dx 函数

Eigen::Matrix<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in)
{
	Eigen::Matrix<double, 24, 23> cov = Eigen::Matrix<double, 24, 23>::Zero();
	cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
	vect3 acc_;
	in.acc.boxminus(acc_, s.ba);
	vect3 omega;
	in.gyro.boxminus(omega, s.bg);
	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_);
	cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix();
	Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
	Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
	s.S2_Mx(grav_matrix, vec, 21);
	cov.template block<3, 2>(12, 21) =  grav_matrix; 
	cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); 
	return cov;
}

3： df_dw 函数

Eigen::Matrix<double, 24, 12> df_dw(state_ikfom &s, const input_ikfom &in)
{
	Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::Zero();
	cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
	cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity();
	cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity();
	cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity();
	return cov;
}

4. Fw 与Fi 与论文不对应的原因：