0
$\begingroup$

Rosanswers logo

I have a odometry/filtered fuse by wheel odometry odom and IMU imu_data with ekf_localization. I let my robot facing a wall and do some test with the odometry/filtered. I have two problem right now:

Problem 1

  • My odometry/filtered when I move the robot forward and backward, rotate the robot on the spot, the odometry/filtered output seem to be like good in rviz.
  • https://gph.is/g/4wg9DPD The rviz output, green arrow > odometry/filtered, blue arrow > odom, the axis in front the base link axis is my laser axis.

But when I lift the robot up to make the wheel loss contact with the ground and move the robot forward and backward, both the odometry/filtered and odom output in rviz will move forward and backward too. When I rotate robot on the spot without wheel and ground contact, only the wheel odom move, the odometry/filtered is not moving, this make sense to me because the robot is not rotating only the wheel itself is running.

  • When I leave the robot not moving and rotate only the IMU, for sure the odom won't move, but the odometry/filtered will be move exactly same with how I rotate my IMU manually.
  • Rviz output: When the robot lift up, first: Rotate the robot , Second: Move robot forward and backward, Third: Rotate only the IMU. https://gph.is/g/EvdnpAY
  • Is my odometry/filtered depend too much on the IMU? And is it related to covariance? How to make a better configuration on this?

Problem 2

  • When I rotate the robot with the odometry/filtered, my laser scan will drift when the robot rotate and the laser will back on right position when the robot stop.
  • This is the rviz output when the robot is facing a wall and not rotating. The green arrow is odometry/filtered and the blue arrow is pure odom from wheel encoder. https://ibb.co/Y33m99M
  • This is the rviz output when I set the laser scan decay time to 20s and rotate the robot. The laser scan will rotate when the robot rotate. https://ibb.co/TB3ZpmB
  • Rviz video https://gph.is/g/ZnM9XNJ

ODOM topic##

header: 
  seq: 42461
  stamp: 
    secs: 1560329405
    nsecs: 936365909
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: -0.210383832846
      y: -0.0374875475312
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.145501269807
      w: 0.98935806485
  covariance: [0.01, 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.01, 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.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.01, 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.01, 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.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]

IMU topic

header: 
      seq: 86075
      stamp: 
        secs: 1560329453
        nsecs: 734069150
      frame_id: "base_imu"
    orientation: 
      x: -0.00954644712178
      y: 0.0186064635724
      z: 0.145939352166
      w: 0.989072479826
    orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    angular_velocity: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    linear_acceleration: 
      x: -0.383203125
      y: -0.129331054688
      z: 9.733359375
    linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

My configuration

frequency: 20
sensor_timeout: 1
two_d_mode: true
transform_time_offset: 0.01
transform_timeout: 0.01
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false

# map_frame: map              
odom_frame: odom         
base_link_frame: base_link  
world_frame: odom

odom0: /odom
imu0: /imu_data

odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false

odom0_config: [false,   false,  false,
               false,   false,  false,
               true,    true,   false,
               false,   false,  true,
               false,   false,  false]

imu0_config: [false,  false,  false,
              false,  false,  false,
              false,  false,  false,
              false,  false,  true,
              false,  false,  false]

imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 5

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.05, 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.03, 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.06, 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.025, 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.01, 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.02, 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.01, 0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

initial_estimate_covariance: [1e-9, 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,    1e-9, 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,    1e-9, 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,    1e-9, 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,    1e-9, 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,     1e-9,  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,     1e-9, 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,    1e-9]

Originally posted by jynhao_low on ROS Answers with karma: 1 on 2019-06-12

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

But when I lift the robot up to make the wheel loss contact with the ground and move the robot forward and backward, both the odometry/filtered and odom output in rviz will move forward and backward too. When I rotate robot on the spot without wheel and ground contact, only the wheel odom move, the odometry/filtered is not moving, this make sense to me because the robot is not rotating only the wheel itself is running.

Right, this normal/expected, as are your other bullet points. The reason for this is that your IMU is also measuring yaw velocity, and it has an all-zero covariance matrix (which you should fix), so the filter trusts it much, much more than it trusts the wheel velocity for detecting turning. But the IMU doesn't report and kind of linear velocity, so the filter is only using the wheel encoder data for linear motion.

So to answer your question, the filter isn't too dependent on the IMU; you are telling the filter to trust the IMU much more than it trusts the wheel encoders. In general, I'd say this is correct, but your IMU may not report velocities very accurately. In any case, if you want the wheel encoders to have more of an effect, then you need to make sure the IMU and wheel encoder covariance values for yaw velocity are on the same order of magnitude.

When I rotate the robot with the odometry/filtered, my laser scan will drift when the robot rotate and the laser will back on right position when the robot stop.

This says to me that you need to do more covariance tuning, or that your IMU data may be lagged. There will always be some amount of lag in a filter, and you will experience drift if your IMU is under-reporting its error. If I were you, I'd see how it does with just wheel encoder data as an input. If there's less laser "smearing," then the issue is with the IMU data. I'd also try increasing the initial and process noise covariance for yaw velocity.


Originally posted by Tom Moore with karma: 13689 on 2019-07-03

This answer was ACCEPTED on the original site

Post score: 2

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.