/Odometry appears to be estimated in the wrong direction when there is very little translation but a large rotation

[ruredot]

Hi, thank you for the great work.

While trying FAST_LIO launch on various driving scenarios in Gazebo, there seems to be an issue when the robot stay near the starting area and repeatedly rotate in place (due to obstacle avoidance triggering frequent replanning)

The first attached figure shows trajectory estimated by FAST_LIO, the second shows the trajectory estimated by LIO-SAM for comparison using the same rosbag.

My current understanding is that the issue may be related to how correspondences are built from the current pose estimate, and how correspondence recalculate is limited when a large correction is needed. In the IEKF update, each lidar point is transformed into the world frame using the current pose estimate, nearest neighbors are searched at that location, a local plane is built from those neighbors, and the residual and Jacobian (H) are constructed to update the pose (current rotation/translation estimate → correspondence generation → residual/H construction → pose correction)
With this structure, if the yaw estimate is already wrong, the world frame position of each point also becomes wrong. This can lead to wrong correspondences, and the residual and Jacobian built from them can also point in the wrong direction. As a result, the update may fail to recover toward correct pose and instead continue moving in a direction induced by those wrong correspondences.

This seems to become worse when a large correction is needed. From what I understood, nearest neighbor search is triggered only when ekfom_data.converge is true. If a large correction makes converge=false, the next iteration may continue using the previous correspondences instead of recalculating with the updated pose. In that case, stale correspondences may persist and make recovery toward the correct yaw direction more difficult.

There also seems to be a safety helper check_safe_update() intended to reject oversized updates, but it does not appear to be called in the current code. Even if enabled, it would only suppress a large correction, not recalculate correspondences and recover from a wrong yaw estimate. So the core issue seems to be less about blocking large updates, and more about wrong correspondences being created first and then retained when correspondence refresh is limited.

FAST_LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp

Lines 1985 to 2001 in a4743b0

	T check_safe_update( T _temp_vec )
	{
	T temp_vec = _temp_vec;
	if ( std::isnan( temp_vec(0, 0) ) )
	{
	temp_vec.setZero();
	return temp_vec;
	}
	double angular_dis = temp_vec.block( 0, 0, 3, 1 ).norm() * 57.3;
	double pos_dis = temp_vec.block( 3, 0, 3, 1 ).norm();
	if ( angular_dis >= 20 || pos_dis > 1 )
	{
	printf( "Angular dis = %.2f, pos dis = %.2f\r\n", angular_dis, pos_dis );
	temp_vec.setZero();
	}
	return temp_vec;
	}

Could this behavior be intended, or could it be considered a bug?
Also, would it make more sense to recalculate correspondences at every iteration, or at least force recalculate when the correction becomes larger than a certain threshold?