Do you need a map to run robot_localisation in map_frame with gps

asked 2021-02-25 13:59:19 -0500

Gherkins
31 ●14 ●17 ●18

updated 2021-02-26 04:20:13 -0500

I want to localise my robot using gps, imu and wheel odometry. I'm fusing imu and wheel odometry in odom_frame which works great (except if I fuse absolute yaw orientation). However when I run a second instance in map_frame which fuses gps,imu and wheel odometry the map frame drifts away from the odom frame very fast.

As I understand it you should fuse gps in a separate frame because it jumps around,

frequency: 30

sensor_timeout: 0.1

two_d_mode: true

transform_time_offset: 0.1

transform_timeout: 0.0


print_diagnostics: true

debug: false
debug_out_file: /path/to/debug/file.txt

publish_tf: true

publish_acceleration: false

# map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

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

odom0_nodelay: true
odom0_differential: true
odom0_relative: false
odom0_queue_size: 10

imu0: imu/data
imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              false, false, true,
              true, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
#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 ...

(more)

edit retag flag offensive close merge delete

Comments

what happens to your odometry message when you only run the localization in the odom frame ?

ejalaa12 ( 2021-02-25 15:49:18 -0500 )edit

C:\fakepath\fused_odom.png

Yellow line is odometry/gps
Red line is raw odom message
blue line is x_pos,y_pos from odom fused differentially with imu yaw_vel and x_accel.

Fusing just raw odometry results in a straight line

Gherkins ( 2021-02-26 02:30:58 -0500 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2021-03-02 03:48:10 -0500

Gherkins
31 ●14 ●17 ●18

Okay so I found the fix. I used the output from the ekf node which outputs odom>base_link instead of map>odom for the navsat node. Using the correct node the map frame still drifts but the map>odom and odom_base_link transform are on top of eachother when standing still so it looks like it's working now.

edit flag offensive delete link

add a comment

answered 2021-02-26 02:52:59 -0500

turtleMaster20
41 ●1 ●7 ●5 https://github.com/tur...

You do not need a map for this task. (Still need a 'map_frame'). Please check the effects of the imu as well. If you are running it in a simulation, the imu would be designed with a big error.

Without SLAM the small drifts in odometry are not exactly compensated for.

One thing to try would be to x and y velocity of odom=0 into the filter rather than directly feeding the absolute pose values.

Have a look at these parameters.

If you decide to try out the parameters then you have to kill your old instance of odom and start two nodes ekf_se_odom and ekf_se_map. Hope it works out.

edit flag offensive delete link

Comments

Do I need to create the map_frame? I thought running the ekf node in map_frame would create it.

The driver does not output any speed just orientation/position. That's why I'm using those differentially to get a speed (at least that's how I think it works). How would fusing a x/y velocity of 0 help?

Gherkins ( 2021-02-26 02:58:50 -0500 )edit

You are right, you already have the map_frame. As the information from the imu and odom are not entirely reliable, you want your position/orientation to be affected indirectly by the inaccurate sensors. Same with the theta, you want imu to only influence the theta_dot as imu would also not provide a single value but will swing between two an upper value and a lower value.

So your odom does not publish any twist data?

The general idea is that x,y would be received from the GPS (given your GPS is somewhat reliable). And you used the odom and imu to smoothen out the discrete GPS data.

Can you also post which direction the drift occurs in (x or y or x and theta etc.)?

turtleMaster20 ( 2021-02-26 03:54:15 -0500 )edit

I figured out why my driver was publishing empty twist messages. I now run ekf_se_odom with imu x_accel and yaw_vel and odom x_vel and y_vel. ekf_se_map fuses those with x/y position from gps. However when I do that the map frame drifts away to north-west infinetely (towards y).

C:\fakepath\map_frame_drift.png

Gherkins ( 2021-02-26 04:19:36 -0500 )edit

Is this with or without moving the robot? This behavior is due to the IMU. Now remove roll and pitch dependency on the imu in both ekf_se_odom and ekf_se_imu, also the accelerations, so only roll_vel, pitch_vel, yaw_vel are from the IMU.

Although your localization is in 2D, the EKF would perform better with these information. Check this and see if the values are stable.

turtleMaster20 ( 2021-02-26 04:54:34 -0500 )edit

This is while moving the robot. The data is from a rosbag. I now use these imu configurations for odom and map:

imu0: imu/data
imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              true, true, true,
              false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

This results in the map frame randomly rotating around the odom frame and very rapidly moving away from the odom frame

(edit) Could it be because the gps sensor is only accurate to ~0.5m?

(edit2) When the robot is stationary the ekf_se_map publishes a position that continuously increases

Gherkins ( 2021-02-26 04:56:51 -0500 )edit

Could be due to the navsat_trasform the heading of the robot must be correct. You can check which topic is causing the error by checking each topic that is fed into the localizer:

rostopic echo -c /imu/data

Can you check for the imu, odom and the gps (converted into odom) and check which one has the velocity bias.

turtleMaster20 ( 2021-02-26 08:31:17 -0500 )edit

imu/data consistently outputs values around this when standing still:

orientation: 
  x: -0.0169671956266
  y: -0.0181011912016
  z: -0.985738086175
  w: -0.166447849532
orientation_covariance: [1e-06, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 1e-06]
angular_velocity: 
  x: -0.00106258222102
  y: 0.00108459413857
  z: 0.000965583408742
angular_velocity_covariance: [1.0966208586777776e-06, 0.0, 0.0, 0.0, 1.0966208586777776e-06, 0.0, 0.0, -0.0, 1.0966208586777776e-06]
linear_acceleration: 
  x: 0.263741593241
  y: 0.348855266673
  z: 9.9081989879
linear_acceleration_covariance: [0.0015387262937311438, 0.0, 0.0, 0.0, 0.0015387262937311438, 0.0, 0.0, -0.0, 0.0015387262937311438]

odometry/gps has it's x slowly increasing and y slowly decreasing when standing still

Gherkins ( 2021-03-02 03:35:55 -0500 )edit

add a comment

Do you need a map to run robot_localisation in map_frame with gps

Comments

2 Answers

Comments

Question Tools

Stats

Related questions

Do you need a map to run robot_localisation in map_frame with gps edit

Comments

2 Answers

Comments

Question Tools

Stats

Related questions

Do you need a map to run robot_localisation in map_frame with gps