Revision history [back]

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:

1. 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.
2. 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.

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:

1. 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.
2. 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 for pose data_. 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.