Using robot_localization package does not filter odometry noise

asked 2021-11-22 14:47:10 -0500

bugino
11 ●1 ●3 ●2

Hello.

I'm trying to implement fusion of visual odometry (stereocamera) and IMU using the robot_localization package (EKF) for my autonomous robot vehicle. I've customized the ekf_template.yaml file with respect to my configuration, especially the input topics and matrices.

To verify, that the robot_localization package does its job well, I've added a random noise to the y pose coordinate of the visual odometry messages. However, the result is not satisfactory. In fact, the fused pose (published on the /odometry/filtered topic) does not improve the noisy odometry information at all!

I've also tried to change values in the process_noise_covariance parameter of the .yaml file, but couldn't get better results either. Unfortunately, I couldn't find a tutorial showing configuration of this matrix.

Here is my ekf_template.yaml:

frequency: 10
silent_tf_failure: false
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false

odom_frame: odom
base_link_frame: imu
world_frame: odom

odom0: noisy_odom
odom0_config: [true,  true,  false,
           false, false, true,
           false, false, false,
           false, false, false,
           false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1

imu0: imu/data
imu0_config: [false, false, false,
          false,  false,  true,
          false, false, false,
          false,  false,  true,
          true,  false,  false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
imu0_remove_gravitational_acceleration: true

use_control: false
stamped_control: false
control_timeout: 0.2
control_config: [false, false, false, false, false, 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 ...

(more)

Comments

Hi @bugino, I recently answered a similar question in general terms: https://answers.ros.org/question/3914...

You are following the right steps. Adjustment of these parameters is a bit of more art than science. However there are systematic ways to adjust them, please refer to the papers in the answer to get some ideas.

osilva ( 2021-11-26 08:15:34 -0500 )edit

add a comment

answered 2021-12-22 05:34:14 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

updated 2022-02-11 10:55:45 -0500

An EKF isn't going to necessarily clean up noise in your data. Every time the EKF processes a measurement, it's just doing fancy (optimal) weighted averaging between its current state and the measurement you gave it. How much weight is given to each is determined by their relative covariance values.

So it goes something like this:

Your EKF gets a new measurement. It carries out a prediction from its last updated time to the time of the measurement. As part of that prediction, it adds the process_noise_covariance (scaled by the time delta) to the current state covariance.
It then computes the weights (Kalman Gain) using the relative covariance of the state and the measurement you just gave it, and then combines the weighted current state and the weighted measurement into a new state.

So the two quantities that matter w.r.t. how much a given measurement is "trusted" are the process_noise_covariance and the covariance in the message itself.

Let's look at just the X variance in your messages. In the raw odometry message, you have an X variance of 4.407756932778284e-06, which is a standard deviation of 0.0021. Your X variance in your process_noise_covariance is set to 0.05, which is a standard deviation of 0.2236. Let's say your odometry is published at 10 Hz. That means you'd likely be looking at a state with an X standard deviation of at least 0.02236, which is an order of magnitude larger than your measurement covariance. So when the new state is computed, you're telling the filter to trust the odometry measurement at least 10x more than its predicted state.

Secondly, you said you added noise to the measurement. The variance from your noisy measurement is 4.446595085028093e-06, which is pretty much identical to the original odometry message. If you are adding noise to the values, then that error needs to be represented in the covariance. Otherwise, your measurement is lying.

Finally, your odometry data has tiny covariance values for pose data. But if your robot is driving around for a long time, the pose covariance from odometry should grow without bound. If it was providing the velocity (twist) data, then those covariance values might be more or less static, but what you're telling the filter is that no matter how far the robot travels, the X standard deviation on the odometry pose is only 0.0021.

EDIT in response to comment.

Although, I am still quite ambiguous about which input data is used for a prediction step and which one for a correction

At least in r_l, no specific sensor is used for prediction or correction. Every sensor measurement causes both steps to happen. Let's pretend we have an EKF with a state vector that is just [X, Y, Z, X velocity, Y velocity, Z velocity].

Initialize filter to some value at time t0. We'll use [0, 0, 0, 0, 0, 0].
Receive measurement from sensor A at ...

(more)

edit flag offensive delete link

Comments

Thank you @Tom Moore for the explanation. Although, I am still quite ambiguous about which input data is used for a prediction step and which one for a correction. Let's assume, we use the same sensor setup as stated in a question - visual odometry from the stereo camera and IMU data. Does the choice of input to particular EKF steps depend on the type of information (position, velocities, accel) or the data uncertainty? E.g. more uncertain data are used for a prediction, while more precise data for correction step? May the user somehow assign the odometry source to EKF steps?

martinlucan ( 2022-01-25 09:41:11 -0500 )edit

add a comment

Using robot_localization package does not filter odometry noise

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Using robot_localization package does not filter odometry noise edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Using robot_localization package does not filter odometry noise