ekf_localization_node strange divergence

asked 2015-02-23 07:43:37 -0600

Michael Stoll
71 ●6 ●8 ●12

I experienced some behavior of the ekf_localization_node that I cannot explain.

To demenstrate this case, I use an ekf_localization_node with only one odometry input. The configuration is very simple:

frequency: 20
  sensor_timeout: 0.5
  two_d_mode: true
  map_frame: map
  world_frame: odom
  odom_frame: odom
  base_link_frame: base_footprint

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

And the result looks like this (Red: wheel odometry, Orange: ekf odometetry) rviz

I supposed to get idendentical tracks. What's wrong?

answered 2015-02-23 08:58:46 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

Short answer: if you want it to look identical, change your configuration to this:

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

(Note that I left Z position as false because you have two_d_mode set to true). Note, however, that if you plan on fusing other sensors such as IMUs, then you want to continue to use velocities as you are, with one caveat (see below).

The long answer has a few possibilities:

Your configuration has Y velocity set to true. If this is a nonholonomic UGV operating in 2D, then you should only have X velocity and yaw velocity set to true (you can't instantaneously move sideways, so you don't need Y).
However, I find it strange that you get +Y velocity as you turn. In other words, your raw odometry data should never have positive Y velocity when it turns. Can you verify this in your bag data? Incidentally, what kind of robot is this?
What do your covariances look like in your odometry messages?
Regardless, the two may never line up perfectly: the EKF implementation in ekf_localization_node has its own motion model. At every time step, it's going to predict the next state, and then correct it using your sensor data. However, the filter has some trust in its own estimate, so when a measurement comes in, it may not fully accept it.

edit flag offensive delete link

Comments

It's running on a simple kinematic simulation. The odometry is good (y=0, sign is correct). But the covariance was way to high (diag 1.0). With lower odometry covariance the tracks get more similar. BTW: Are there any guides on how to tune the covariances?

Michael Stoll ( 2015-02-23 09:55:49 -0600 )edit

I don't know any guides that Google wouldn't give you, but for velocity, I'm sure a naive method like driving for 20 seconds in a straight line and comparing your odometry and actual displacement would work. You could even repeat the test a bunch of times to get a proper distribution.