Wrong /odometry/filtered results generated by efk_localization in Unit 2

Hello there, I am following the tutorial setps of Unit 2 (Merging sensoe data). The resultI am expecting is the new filtered odom result () result as below in the Tut notebook:

<Pic here same as the lasr one gif image in Unit 1: Merging sensor data Notebook…>

Sorry, as a new user I can only put one image in a post. :hot_face:

However what I got(as below) is different, the /odometry/filtered results are obviously going astray (/noisy_odom and /odometry/filtered are not concentric). By the way, the green odoms are original /odom.


It makes me quite confusing, anyone can help?
Thanks a lot fellows!

odom_filtered

The result I am expecting from Tut notebook.

Two odom cilcles should be concentric.

Hi @ttqwer1,

I’ve just tested this and confirmed that there is an error in this example. I will check it and let you know once it is solved. Sorry for the inconvenience.

It’s quite confusing, thanks anyway.

Hello there,

it’s been a while.
Any update on this?

Experiencing this issue as well. Any updates on this?

Hello @ChristopherValles and @georg.novotny2 ,

Could you provide some more details on your case scenario? I just did some tests here and it’s working as expected for me:

Maybe you could record a short video to see exactly what’s going on?

Best,

Hi @albertoezquerro

You can see a screenshot below of what I get on my side. As you can see, the blue arrows, which are the filtered odom, move in an ellipsis pattern around the odom frame.

And this is my answer to exercise 1.3 which is highly likely where the issue lies.

#Configuation for robot odometry EKF

frequency: 50
    
two_d_mode: true
    
publish_tf: false

# Complete the frames section 
odom_frame: odom
base_link_frame: base_link
world_frame: odom
map_frame: map

# Complete the odom0 configuration
odom0: /odom_noisy
odom0_config: [false, false, false,
           false, false, false,
           true, true, false,
           false, false, true,
           false, false, false]
odom0_differential: false

# Complete the imu0 configuration
imu0: /imu/data
imu0_config: [false, false, false,
          false, false, true,
          false, false, false,
          false, false, true,
          true, false, false]
imu0_differential: false

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]

Hello again @ChristopherValles ,

Ok, I see the issue now. This happens because you don’t have the robot in the [0,0] position of the world (the center) when you start the filter. Since you are not providing the [X,Y] pose data of the robot to the filter (you are only using the velocities), the filtered odometry will always start in the [0,0] position and it has no way to know if the initial position has changed.

If you start moving the robot from the [0,0] position, this won’t happen because the filter will be able to update the pose of the robot from the velocity data.

Another option is to update the initial pose manually. You can add this to your filter configuration file:

initial_state: [1.0, 1.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]

If you launch it with this, then the initial pose of the filter will be [1, 1]. I will update the instructions of the notebook to avoid this kind of confusion in the future.

Does this make sense to you?

Best,

@albertoezquerro thanks for the quick response.

It totally makes sense and I know I started the filter after the robot had moved. Thinking about it, even if the robot was not on [0, 0] I would expect both arrows to create a perfect circle, just shifted around but in my case the filtered odom creates an ellipsis instead of a circle. I restarted the simulation entirely, making sure the robot was on [0, 0], and executed everything again and it is still not working. I recorded a video so you can see what I see. Any ideas on how can I debug what’s wrong?

Hello @ChristopherValles ,

That’s probably a configuration issue. I’ve sent to your registration e-mail the package I used, which is working correctly, so that you can compare it with yours and find out what’s wrong.

Best,

Hi @albertoezquerro

Thanks for sending over the config you had. After close inspection I noticed that I was using the wrong topic for the noisy odom. Instead of using

odom0: /noisy_odom

I was using

odom0: /odom_noisy

I guess mentally I kept thinking, if the original topic for odom is /odom then /odom_noisy is totally the right thing to use.

As the config for odom0 was wrong I guess that the only thing plotted was based on imu data hence the weird ellipse.

Oh I see, that makes sense. Well, I’m glad that you could finally solve the issue :ok_hand: