IMU timestamp issue

[akorycki]

Hello,

I ran into some issues running LiDAR inertial SLAM on this data set (base_2023-07-16-18-31-20_0.bag , base_2023-07-16-18-39-54_1.bag). Specifically many of the IMU timestamps are not advancing. Please see the attached file which consists of two IMU messages with identical timestamp fields.

/Adam

error.txt

[hanzheteng]

@akorycki Sorry for the late reply and thank you for bringing this up.

The reason behind this is that we are using the software time or the PC time for IMU data, and by the time CPU scheduler processes the IMU data thread, there may be two messages in the queue already, such that both are assigned the same timestamp. This is an known observation of the MicroStrain IMU driver, and our setting is the same as the default or the recommended settings in the driver (config file line 69). There are also some discussions on the time delay (See this issue).

(More discussions on this: if using the hardware time, the temporal synchronization at the system level as a whole may have issues, since different sensors are using different crystal oscillators to generate the hardware time; for low-end products, this can lead to roughly 1 second deviation per hour, which can be very bad for a sequence of half an hour long.)

For some algorithms using Kalman filter internally, this shall not be a problem since the arrival time of IMU data itself is of some uncertainty, and the algorithm is expected to be capable of handling this. In this case, you may advance the second duplicated timestamp by a random number, let's say 1ms, and then hand it over to the Kalman filter-based algorithm.

In some other cases, if you want to resolve this ahead of time before feeding into any algorithms, we may implement a simple Kalman filter to process the incoming IMU messages online, and publish the filtered ones out to another ros topic. The following is an example. You may do chmod +x and put it under the scripts folder of a package, and then run it together with other ros nodes. Hope it helps.

#!/usr/bin/env python3

import rospy
import numpy as np
from sensor_msgs.msg import Imu

def kalman_filter(x, P, measurement, dt):
    # Process noise covariance
    Q = 0.000001
    # Measurement noise covariance (std is 0.003, so variance is 0.003^2)
    R = 0.003 ** 2

    # Time Update (Prediction)
    x_pred = x + dt
    P_pred = P + Q

    # Measurement Update (Correction)
    K = P_pred / (P_pred + R)
    x_update = x_pred + K * (measurement - x_pred)
    P_update = (1 - K) * P_pred

    return x_update, P_update

class KalmanFilterNode:
    def __init__(self):
        rospy.init_node('kalman_filter_imu_node', anonymous=True)
        self.subscriber = rospy.Subscriber('/microstrain/imu/data', Imu, self.callback)
        self.publisher = rospy.Publisher('/microstrain/imu/data_filtered', Imu, queue_size=10)

        # Initial state and covariance
        self.x = 0.0
        self.P = 1.0

    def callback(self, data):
        # Extract timestamp and convert to seconds
        timestamp = data.header.stamp.to_sec()
        
        # Assuming the time increment is fixed (0.005 seconds)
        dt = 0.005

        # Apply Kalman filter
        self.x, self.P = kalman_filter(self.x, self.P, timestamp, dt)

        # Replace timestamp in the original IMU message
        filtered_stamp = rospy.Time.from_sec(self.x)
        data.header.stamp = filtered_stamp

        # Publish the updated IMU message
        self.publisher.publish(data)

if __name__ == '__main__':
    try:
        node = KalmanFilterNode()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

[hanzheteng]

Adding a bit more info: This microstrain IMU is placed underneath the LiDAR, and is intended to be used for LiDAR inertial algorithms. For those who work on vision-based algorithms, there is also an IMU integrated in the Zed camera available, and the topic is /zed2i/zed_node/imu/data.