Fix: Prevent NaN or zero dt in IMU propagation

[BenderBot-es]

Issue

During the execution of the code, I observed that due to possible desynchronizations, sensor instabilities, or very fast IMU readings, the time interval dt between IMU measurements could sometimes become very close to zero or exactly zero.

This issue destabilizes the system and propagates NaN values through all IMU state calculations, leading to incorrect state estimations, from which it never recovers.

Observed Behavior

I identified this phenomenon while using FAST-LIVO2 with a rotating LiDAR and no camera, specifically when the LiDAR was close to obstacles and the resulting point cloud was small. In these scenarios, the system exhibited inconsistencies, and NaN values started propagating within the IMU state.

Solution

Added the following safeguard to skip IMU propagation steps where dt is NaN or too small:

if (dt != dt || dt == 0.0)
{
    std::cerr << "ERROR: dt is NaN or zero in IMU Propagation" << std::endl;
    continue;
}

I am aware that simply skipping the iteration with continue may not be the best solution. There might be a better way to handle this issue instead of ignoring the propagation step. If anyone has suggestions for a better approach, I’d appreciate any feedback or insights on how to handle this more effectively.