EKF filter node not working correcty

Question

I'm using a two-fixed-wheeled differential robot with Nav2 on ROS2 Humble, I succesfully achieved to code a standard odom publisher which publishes on /odom and /tf topics like this:

void OdomPublisherNode::timerCallback() 
{
  // Calculate current time and dt
  current_time = now();
  double dt = (current_time - last_time).seconds();

  // Calculate delta x, delta y and delta th
  double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
  double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
  double delta_th = vth * dt;
  x += delta_x;
  y += delta_y;
  th += delta_th;

  //Only for logging
  degrees = fmod((th * 180.0)/M_PI,360);
  RCLCPP_DEBUG(this->get_logger(), "Position on X: %.2f | Position on Y: %.2f | Orientation (TH) : %.2f º", x , y, degrees );
  
  // Create TransformStamped
  geometry_msgs::msg::TransformStamped odom_trans;
  odom_trans.header.stamp = current_time;
  odom_trans.header.frame_id = "odom";
  odom_trans.child_frame_id = "base_link";

  odom_trans.transform.translation.x = x;
  odom_trans.transform.translation.y = y;
  odom_trans.transform.translation.z = 0.0;

  // Create tf2::Quaternion
  tf2::Quaternion tf2_quat;
  tf2_quat.setRPY(0, 0, th);  // RPY: Roll, Pitch, Yaw

  // Create geometry_msgs::msg::Quaternion and fill
  geometry_msgs::msg::Quaternion odom_quat;
  odom_quat.x = tf2_quat.x();
  odom_quat.y = tf2_quat.y();
  odom_quat.z = tf2_quat.z();
  odom_quat.w = tf2_quat.w();
  odom_trans.transform.rotation = odom_quat;

  // Publish tf
  tf_broadcaster->sendTransform(odom_trans);

  // Create odom msg
  nav_msgs::msg::Odometry odom;
  odom.header.stamp = current_time;
  odom.header.frame_id = "odom";

  // Fill position and orientation and speed
  odom.pose.pose.position.x = x;
  odom.pose.pose.position.y = y;
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = odom_quat;  
  odom.child_frame_id = "base_link";
  odom.twist.twist.linear.x = vx;
  odom.twist.twist.linear.y = vy;
  odom.twist.twist.angular.z = vth;

  // Publish odometry on /odom
  odom_pub->publish(odom);

  tf2_msgs::msg::TFMessage tf_message;
  tf_message.transforms.push_back(odom_trans);

  last_time = current_time;
}

Describe the bug I connected a D435i Intel Realsense camera with integrated IMU, which publishes on camera/camera/imu topic

While using the EKF node, my robot does not recognize that it is stationary, even though its wheels are spinning (constantly slipping). So, in Rviz2, when executing Nav2, the robot appears to move as if it never slipped. (I already changed the nav2_params.yaml setting to read from /odometry/filtered topic, which is publishing the EKF node)

To Reproduce Steps to reproduce the behavior:

/camera/camera/imu (SensorIMU) Message example: 

header:
  stamp:
    sec: 1710490254
    nanosec: 384912640
  frame_id: camera_imu_optical_frame
orientation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance:
- -1.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: 0.001745329238474369
  y: -0.012217304669320583
  z: 0.0
angular_velocity_covariance:
- 0.01
- 0.0
- 0.0
- 0.0
- 0.01
- 0.0
- 0.0
- 0.0
- 0.01
linear_acceleration:
  x: -0.19613298773765564
  y: -9.688969612121582
  z: -0.0784531980752945
linear_acceleration_covariance:
- 0.01
- 0.0
- 0.0
- 0.0
- 0.01
- 0.0
- 0.0
- 0.0
- 0.01

/odom (Odometry) message: 
header:
  stamp:
    sec: 1710490360
    nanosec: 225816625
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.3328242252657941
      y: -0.0003743422538225482
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.016578521394064526
      w: 0.9998625668702606
  covariance:
  - 0.0
  - 0.0 .... (all zeros)
twist:
  twist:
    linear:
      x: 0.20350000000000001
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.0482142857142857
  covariance:
  - 0.0
  - 0.0 (all zeros) ....

EKF config file:

ekf_filter_node:
    ros__parameters:
    use_sim_time: false
    frequency: 10.0
    sensor_timeout: 0.1
    two_d_mode: true
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: true
    publish_tf: true
    publish_acceleration: false
    reset_on_time_jump: true
    
   odom0: odom                               # Odometry published from node mentioned above
   odom0_config: [true, true, false,
                  false, false, true,
                  false, false, false,
                  false, false, false,
                  false, false, false]
 
    odom0_queue_size: 5
    odom0_nodelay: false
    odom0_differential: false
    odom0_relative: false
    odom0_pose_rejection_threshold: 5.0
    odom0_twist_rejection_threshold: 1.0

    
    imu0: camera/camera/imu         # Intel Realsense D435i IMU topic using unite_imu_method=2
    imu0_config: [false, false, false,
                  false, false, false,
                  false, false, false,
                  true,  true,  true,
                  true,  true,  true]
        
     imu0_nodelay: false
     imu0_differential: false
     imu0_relative: true
     imu0_queue_size: 7
     imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
     imu0_twist_rejection_threshold: 0.8                #
     imu0_linear_acceleration_rejection_threshold: 0.8  #

     imu0_remove_gravitational_acceleration: true

process_noise_covariance: [0.05,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.05,   0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.06,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.03,   0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.03,   0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.06,   0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025,   0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01,   0.0,    0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01,   0.0,    0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.02,   0.0,    0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.2,   0.0,    0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.2,   0.0,
                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.2]

initial_estimate_covariance: [1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,   0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,    0.0,     0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1e-9,    0.0,     0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9,    0.0,    0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1e-9,   0.0,    0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1e-9,   0.0,
                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]

Expected behavior I expected that the EKF filter node smoothed something on its /tf and /odom publications but it is not doing it.

Desktop (please complete the following information):

OS: Ubuntu 22.04
ROS Distribution: Humble
robot_localization Package Version: 3.5.2

automatom · Accepted Answer · 2024-03-21 08:49:57Z

0

Please edit your question to include a sample sensor message from every sensor input.

But this is not a bug. If I understand you correctly, your robot's wheels are slipping, but you want the EKF to detect that the robot is not moving forward based on the IMU data.

If so, two things:

You are making your wheel encoders publish the odom->base_link transform, but your EKF is also publishing that transform. I recommend reading the documentation on tf2 and maybe REP-105. Only one node should be publishing a given transform.
No filter is going to magically erase wheel slip. Garbage data in, garbage data out. You are fusing absolute pose data from your wheel encoders into the EKF state estimate, so if your wheel encoders say your robot is at some position, the filter will believe you.

If it were me, I'd:

Turn off publication to /tf from your wheel encoder node
Make the wheel encoder node publish velocity, and not pose data (or just publish both). Make sure it's published in the body frame, and that you fill out the child_frame_id in the odom message. EDIT: I see you've already done this.
Fuse the velocity data from the wheel encoders into the EKF, and not the pose data.
Add some other input for velocity data, like using ICP on your Realsense data. There are plenty of packages out there for this.
Not critical, but you have two_d_mode on, but are trying to fuse 3D variables in your IMU config. Won't hurt anything, but it will be ignored. You also have imu0_relative enabled, but that's only used on absolute orientation data. You may want to read the r_l wiki as well.

Wheel slip can be tough. If you wanted to really add some smarts, you could look for strong differences between your wheel encoder velocity and the Realsense-generated velocity, and inflate your wheel encoder velocity covariance accordingly.

edited Mar 21 at 8:49

answered Mar 20 at 11:33

automatom

5,7472 gold badges14 silver badges16 bronze badges

$\begingroup$ Thank you for your response!! I added an example of the two messages (odom and IMU) that I have when the robot is moving $\endgroup$
– Jesús
Commented Mar 20 at 14:10
$\begingroup$ Wouldn't be enough with just simply setting to false the x_pos and y_pos values from the odom0_config? Does it matter if my odom_publisher's still publishing both pose and twist when I have set those variables to false? Which source (odom or imu) is going to tell x_pos and y_pos? Thx $\endgroup$
– Jesús
Commented Mar 20 at 14:20
$\begingroup$ A nav_msgs/Odometry message contains pose and velocity (twist) data. You can fuse whatever variables you want from that message; that's what the odom0_config is for. So if you set the pose variables to false and the velocity variables to true, it will fuse those instead. If you only fuse velocity data, it will get integrated into pose data. Your covariance will slowly grow (and without bound), but that won't matter. $\endgroup$
– automatom
Commented Mar 21 at 8:50
$\begingroup$ Hi again. I did what you told me: I made the odom_publisher only publish linear.x velocities (not poses or orientation) and I achieved to "mix" the rotation speed read by the IMU (gyro) with the linear from the odom. The question is, is there any way using robot_localization pkg who allows me to detect linear slip? When my robot rotates while slipping (in the same point) all goes good, the problem is correcting linear slips, as the node takes the linear.x from odom and ignores x_accel or x_speed from IMU $\endgroup$
– Jesús
Commented Mar 22 at 13:36
$\begingroup$ I address wheel slip in the answer to the question above. The filter won't solve that for you. $\endgroup$
– automatom
Commented Mar 22 at 13:48

Add a comment |

Stack Exchange Network

EKF filter node not working correcty

1 Answer 1

Your Answer

Not the answer you're looking for? Browse other questions tagged
ros-humble
ekf
or ask your own question.

Hot Network Questions

EKF filter node not working correcty

1 Answer 1

Your Answer

Sign up or log in

Post as a guest

Not the answer you're looking for? Browse other questions tagged ros-humbleekf or ask your own question.

Related

Hot Network Questions

Not the answer you're looking for? Browse other questions tagged
ros-humble
ekf
or ask your own question.