Fix to avoid dt = 0

[TS0321]

If differential == true and last_massage_times == msg.header.stamp, calculated dt is equal to zero. So, velocity is to be Nan and robot_localization 's output is Nan.
I think that it is necessary to modify the judgment condition so that dt = 0 does not occur.

[ayrton04]

This suggests you have an issue with your sensor data. Why would one sensor produce two measurements with identical timestamps and different values?

[TS0321]

I think you're right about sensor data having problems. However, there is a possibility that dt=0, so I don't think the equal sign should be included in the judgment condition, what do you think?

[ayrton04]

I am trying to determine a valid use case for dt = 0. Those time deltas are calculated per sensor, so if you get dt = 0, then your sensor data is wrong. There's a delicate balance to be had between making the filter robust and masking bad data.

[TS0321]

The correct sequence is if curTimestamp > lastTimestamp.
If curTimestamp <= lastTimestamp then the sensor value is the wrong sequence. What I'm wondering is that the equality condition is classified to the correct sequence.

[ayrton04]

Trying to figure out why the IMU differential tests are failing. I'll have to look further into that when I have time.

[ayrton04]

Sorry, looking at this once more, and what I said originally is still true: if you have to identical time stamps coming in for the same sensor, then your sensor is producing faulty data. However, I agree that the filter can recognize the error and yell at the user instead of using the measurement, especially if differential mode is true.

[ayrton04]

Can we do something like this?

  if (last_message_times_[topic_name] <= msg->header.stamp) {
    RF_DEBUG(
      "Update vector for " << topic_name << " is:\n" <<
        callback_data.update_vector_);
    
    if (last_message_times_[topic_name] == msg->header.stamp && callback_data.differential_) {
       ROS_WARN_STREAM_ONCE("Detected messages with identical time stamps for topic " << topic_name <<
         " and differential mode is true. Ignoring measurement.");
       return;
    }
    ...