0
$\begingroup$

Rosanswers logo

Hello,

I have differential drive Jaguar 4x4 wheel mobile robot. I am trying to fuse wheel odometry data with IMU sensor measurements using robot_localization. The problem is that after some time (about 1-2 minutes) /odometry/filtered starts showing uncorrect results of yaw or starts changing rotational velocity when robot is stationary.

My yaml file:

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

two_d_mode: true

frequency: 30

odom0: /odom
odom0_config: [false, false, false,
           false, false, false,
           true,  true,  false,
           false, false, true,
           false, false, false]
odom0_differential: false

imu0: imu/data_raw
imu0_config: [false, false, false,
          false, false, false,
          false, false, false,
          false, false, true,
          true, false, false]
imu0_differential: false
imu0_remove_gravitational_acceleration: false
imu0_relative: false

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, 0.01, 0, 0, 0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0, 0, 0, 0, 1e-6, 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, 0.1, 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, 1e-9, 0,
                           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]


process_noise_covariance:     [0.001, 0,  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                           0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                           0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0,
                           0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0,
                           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 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.001, 0, 0,
                           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0,
                           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001]

Sample odometry message:

rostopic echo /odom
header: 
  seq: 2210
  stamp: 
    secs: 1545421824
    nsecs: 130384158
  frame_id: "odom"
 child_frame_id: "base_link"
 pose: 
  pose: 
position: 
  x: -0.149969377856
  y: 0.350952485627
  z: 0.0
orientation: 
  x: 0.0
  y: -0.0
  z: -0.999999639534
  w: -0.000849077197677
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,          1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 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.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
 ---

Sample Imu (/imu/data_raw) message:

header: 
seq: 37920
stamp: 
  secs: 1545422149
  nsecs: 924128060
frame_id: "new_imu"
orientation: 
   x: 0.0
   y: 0.0
   z: 0.0
   w: 0.0
 orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 angular_velocity: 
    x: -0.00779479229823
    y: 0.00229472899809
    z: 0.0
 angular_velocity_covariance: [5.24675e-06, 0.0, 0.0, 0.0, 1.10824e-05, 0.0, 0.0, 0.0, 0.000469529]
 linear_acceleration: 
 x: 0.0
 y: 0.0432844012976
 z: 9.84547615051
linear_acceleration_covariance: [0.001167135, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001482162]
---

rqt graph: https://ibb.co/NnWnNDr

tf: https://ibb.co/5nT5rNX

average rate of /odom:

subscribed to [/odom]
    average rate: 29.890
min: 0.025s max: 0.040s std dev: 0.00333s window: 30 
 average rate: 29.944
min: 0.024s max: 0.042s std dev: 0.00358s window: 60
 average rate: 29.936
min: 0.024s max: 0.042s std dev: 0.00364s window: 90

average rate of /imu/data_raw:

subscribed to [/imu/data_raw]
average rate: 95.318
  min: 0.009s max: 0.012s std dev: 0.00041s window: 92
average rate: 95.592
  min: 0.008s max: 0.012s std dev: 0.00040s window: 188
average rate: 95.696
   min: 0.008s max: 0.012s std dev: 0.00036s window: 284
average rate: 95.736
  min: 0.008s max: 0.012s std dev: 0.00034s window: 380

Originally posted by ar4angel on ROS Answers with karma: 1 on 2018-12-21

Post score: 0

Original comments

Comment by gvdhoorn on 2018-12-21:
Could you please come up with a more descriptive title? "robot_localization problem" is rather generic and does not really describe anything.

Improve this question
$\endgroup$

2 Answers 2

Reset to default
0
$\begingroup$

Rosanswers logo

What grade IMU are you using? If you only give the ekf angular speed around the vertical axis, it will have to integrate it to deduce yaw. Any small bias/error in angular speed is going to grow really fast since you are squaring angular speed to get the yaw. Rather than feeding angular speed to the ekf, try to give it the actual yaw from an electronic compass.

Also, unless the robot is perfectly flat, you will have issues with gravity interfering with linear acceleration measurements. Robot_localization can remove gravity, but for that it must know the pitch. In my experience, using acceleration to deduce change in position (unless using extremely high grade accelerometers) is a waste of time. You're better off relying on the wheel odometry for that.

Originally posted by SamM with karma: 11 on 2018-12-22

This answer was NOT ACCEPTED on the original site

Post score: 1

Improve this answer
$\endgroup$
0
$\begingroup$

Rosanswers logo

Can you clarify what you mean by "incorrect" yaw? What exactly is incorrect about it? I'm assuming you realise that when you fuse only yaw velocity data, the estimate will start to drift over time. Moving while stationary is a different issue, though.

I agree with the sentiment that you should perhaps turn off linear acceleration, though it's less of a problem when you have a velocity reference, as you do.

How is your IMU mounted? You posted sample IMU data (many thanks), but you didn't show me what the base_link -> new_imu transform is. I'm assuming it's mounted in a neutral position, given the linear acceleration values that I see, but I want to be sure.

Finally, at the point that you notice false motion in the robot when it is stationary, have you tried actually plotting the raw IMU values for Z angular velocity (or whatever axis makes sense, given the IMU's mounting)? Can you post values from all the sensors, as well as the EKF output, while you see the robot showing false rotational motion? Thanks!

Originally posted by Tom Moore with karma: 13689 on 2019-01-09

This answer was NOT ACCEPTED on the original site

Post score: 1

Improve this answer
$\endgroup$

Your Answer

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