Added ability to reset IMU filters when ROS time jumps back

imu_complementary_filter/include/imu_complementary_filter/complementary_filter.h

@@ -87,6 +87,9 @@ class ComplementaryFilter
     void update(double ax, double ay, double az, double wx, double wy,
                 double wz, double mx, double my, double mz, double dt);
 
+    // Reset the filter to the initial state.
+    void reset();
+
   private:
     static const double kGravity;
     static const double gamma_;

imu_complementary_filter/include/imu_complementary_filter/complementary_filter_ros.h

@@ -55,6 +55,9 @@ class ComplementaryFilterROS
                            const ros::NodeHandle& nh_private);
     virtual ~ComplementaryFilterROS();
 
+    // Reset the filter to the initial state.
+    void reset();
+
   private:
     // Convenience typedefs
     typedef sensor_msgs::Imu ImuMsg;
@@ -89,10 +92,12 @@ class ComplementaryFilterROS
     bool publish_debug_topics_;
     std::string fixed_frame_;
     double orientation_variance_;
+    ros::Duration time_jump_threshold_;
 
     // State variables:
     ComplementaryFilter filter_;
     ros::Time time_prev_;
+    ros::Time last_ros_time_;
     bool initialized_filter_;
 
     void initializeParams();
@@ -103,6 +108,8 @@ class ComplementaryFilterROS
 
     tf::Quaternion hamiltonToTFQuaternion(double q0, double q1, double q2,
                                           double q3) const;
+    // Check whether ROS time has jumped back. If so, reset the filter.
+    void checkTimeJump();
 };
 
 }  // namespace imu_tools

imu_complementary_filter/src/complementary_filter.cpp

@@ -456,6 +456,16 @@ double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay,
     return factor * alpha;
 }
 
+void ComplementaryFilter::reset()
+{
+    initialized_ = false;
+    steady_state_ = false;
+    q0_ = 1.0;
+    q1_ = q2_ = q3_ = 0.0;
+    wx_bias_ = wy_bias_ = wz_bias_ = 0.0;
+    wx_prev_ = wy_prev_ = wz_prev_ = 0.0;
+}
+
 void normalizeVector(double& x, double& y, double& z)
 {
     double norm = sqrt(x * x + y * y + z * z);

imu_complementary_filter/src/complementary_filter_ros.cpp

@@ -45,6 +45,8 @@ ComplementaryFilterROS::ComplementaryFilterROS(
     ROS_INFO("Starting ComplementaryFilterROS");
     initializeParams();
 
+    last_ros_time_ = ros::Time::now();
+
     int queue_size = 5;
 
     // Register publishers:
@@ -117,6 +119,11 @@ void ComplementaryFilterROS::initializeParams()
     if (!nh_private_.getParam("orientation_stddev", orientation_stddev))
         orientation_stddev = 0.0;
 
+    double time_jump_threshold{0.0};
+    nh_private_.param("time_jump_threshold", time_jump_threshold,
+                      time_jump_threshold);
+    time_jump_threshold_ = ros::Duration(time_jump_threshold);
+
     orientation_variance_ = orientation_stddev * orientation_stddev;
 
     filter_.setDoBiasEstimation(do_bias_estimation);
@@ -148,6 +155,8 @@ void ComplementaryFilterROS::initializeParams()
 
 void ComplementaryFilterROS::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
 {
+    checkTimeJump();
+
     const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
     const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity;
     const ros::Time& time = imu_msg_raw->header.stamp;
@@ -179,6 +188,8 @@ void ComplementaryFilterROS::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
 void ComplementaryFilterROS::imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
                                             const MagMsg::ConstPtr& mag_msg)
 {
+    checkTimeJump();
+
     const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
     const geometry_msgs::Vector3& w = imu_msg_raw->angular_velocity;
     const geometry_msgs::Vector3& m = mag_msg->magnetic_field;
@@ -295,4 +306,32 @@ void ComplementaryFilterROS::publish(
     }
 }
 
+void ComplementaryFilterROS::checkTimeJump()
+{
+    const auto now = ros::Time::now();
+    if (last_ros_time_.isZero() || last_ros_time_ <= now + time_jump_threshold_)
+    {
+        last_ros_time_ = now;
+        return;
+    }
+
+    ROS_WARN("Detected jump back in time of %.1f s. Resetting IMU filter.",
+             (last_ros_time_ - now).toSec());
+
+    if (time_jump_threshold_.isZero() && ros::Time::isSystemTime())
+        ROS_INFO(
+            "To ignore short time jumps back, use ~time_jump_threshold "
+            "parameter of the filter.");
+
+    reset();
+}
+
+void ComplementaryFilterROS::reset()
+{
+    filter_.reset();
+    time_prev_ = {};
+    last_ros_time_ = ros::Time::now();
+    initialized_filter_ = false;
+}
+
 }  // namespace imu_tools

imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h

@@ -97,6 +97,9 @@ class ImuFilter
                                float az, float dt);
 
     void getGravity(float& rx, float& ry, float& rz, float gravity = 9.80665);
+
+    //! \brief Reset the filter to the initial state.
+    void reset();
 };
 
 #endif  // IMU_FILTER_IMU_MADWICK_FILTER_H

imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h

@@ -57,6 +57,9 @@ class ImuFilterRos
     ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private);
     virtual ~ImuFilterRos();
 
+    //! \brief Reset the filter to the initial state.
+    void reset();
+
   private:
     // **** ROS-related
 
@@ -90,11 +93,13 @@ class ImuFilterRos
     bool remove_gravity_vector_;
     geometry_msgs::Vector3 mag_bias_;
     double orientation_variance_;
+    ros::Duration time_jump_threshold_;
 
     // **** state variables
     boost::mutex mutex_;
     bool initialized_;
     ros::Time last_time_;
+    ros::Time last_ros_time_;
 
     // **** filter implementation
     ImuFilter filter_;
@@ -114,6 +119,8 @@ class ImuFilterRos
     void checkTopicsTimerCallback(const ros::TimerEvent&);
 
     void applyYawOffset(double& q0, double& q1, double& q2, double& q3);
+    //! \brief Check whether ROS time has jumped back. If so, reset the filter.
+    void checkTimeJump();
 };
 
 #endif  // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H

imu_filter_madgwick/src/imu_filter.cpp

@@ -352,3 +352,8 @@ void ImuFilter::getGravity(float& rx, float& ry, float& rz, float gravity)
             break;
     }
 }
+
+void ImuFilter::reset()
+{
+    setOrientation(1, 0, 0, 0);
+}

imu_filter_madgwick/src/imu_filter_ros.cpp

@@ -45,6 +45,10 @@ ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private)
         remove_gravity_vector_ = false;
     if (!nh_private_.getParam("publish_debug_topics", publish_debug_topics_))
         publish_debug_topics_ = false;
+    double time_jump_threshold{0.0};
+    nh_private_.param("time_jump_threshold", time_jump_threshold,
+                      time_jump_threshold);
+    time_jump_threshold_ = ros::Duration(time_jump_threshold);
 
     double yaw_offset = 0.0;
     if (!nh_private_.getParam("yaw_offset", yaw_offset)) yaw_offset = 0.0;
@@ -97,6 +101,8 @@ ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private)
     else
         ROS_INFO("The gravity vector is kept in the IMU message.");
 
+    last_ros_time_ = ros::Time::now();
+
     // **** register dynamic reconfigure
     config_server_.reset(new FilterConfigServer(nh_private_));
     FilterConfigServer::CallbackType f =
@@ -153,6 +159,8 @@ ImuFilterRos::~ImuFilterRos()
 
 void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
 {
+    checkTimeJump();
+
     boost::mutex::scoped_lock lock(mutex_);
 
     const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
@@ -213,6 +221,8 @@ void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
 void ImuFilterRos::imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
                                   const MagMsg::ConstPtr& mag_msg)
 {
+    checkTimeJump();
+
     boost::mutex::scoped_lock lock(mutex_);
 
     const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
@@ -443,3 +453,32 @@ void ImuFilterRos::checkTopicsTimerCallback(const ros::TimerEvent&)
                         << ros::names::resolve("imu") << "/data_raw"
                         << "...");
 }
+
+void ImuFilterRos::reset()
+{
+    boost::mutex::scoped_lock lock(mutex_);
+    filter_.reset();
+    initialized_ = false;
+    last_time_ = {};
+    last_ros_time_ = ros::Time::now();
+}
+
+void ImuFilterRos::checkTimeJump()
+{
+    const auto now = ros::Time::now();
+    if (last_ros_time_.isZero() || last_ros_time_ <= now + time_jump_threshold_)
+    {
+        last_ros_time_ = now;
+        return;
+    }
+
+    ROS_WARN("Detected jump back in time of %.1f s. Resetting IMU filter.",
+             (last_ros_time_ - now).toSec());
+
+    if (time_jump_threshold_.isZero() && ros::Time::isSystemTime())
+        ROS_INFO(
+            "To ignore short time jumps back, use ~time_jump_threshold "
+            "parameter of the filter.");
+
+    reset();
+}

imu_filter_madgwick/test/madgwick_test.cpp

@@ -25,6 +25,12 @@ void filterStationary(float Ax, float Ay, float Az, float Mx, float My,
     }
 
     filter.getOrientation(q0, q1, q2, q3);
+
+    // test resetting the filter
+    filter.reset();
+    double rq0, rq1, rq2, rq3;
+    filter.getOrientation(rq0, rq1, rq2, rq3);
+    ASSERT_QUAT_EQUAL(rq0, rq1, rq2, rq3, 1.0, 0.0, 0.0, 0.0);
 }
 
 template <WorldFrame::WorldFrame FRAME>
@@ -48,6 +54,12 @@ void filterStationary(float Ax, float Ay, float Az, double& q0, double& q1,
     }
 
     filter.getOrientation(q0, q1, q2, q3);
+
+    // test resetting the filter
+    filter.reset();
+    double rq0, rq1, rq2, rq3;
+    filter.getOrientation(rq0, rq1, rq2, rq3);
+    ASSERT_QUAT_EQUAL(rq0, rq1, rq2, rq3, 1.0, 0.0, 0.0, 0.0);
 }
 
 #define TEST_STATIONARY_ENU(in_am, exp_result)                                \