Covariance matrix setting

[ricber]

Hi methylDragon, thanks for the significant tutorial you have made, there was nothing like this before and I think it's really necessary to have a complete and clear tutorial about the obscure field of sensor fusion.

So, I would like to make some questions about two points I think they really need to be explained deeper:

• Sensors covariance matrices

It's really tough to understand how to set it and to get the general view about the process. For example, I'm trying to fuse an odometry source with the data coming from an IMU. To do this I'm using an ekf filter. At the moment my IMU has a covariance matrix filled with zeros. As you can read from here this is an error:

Missing covariances. If you have configured a given sensor to fuse a given variable into the state estimation node, then the variance for that value (i.e., the covariance matrix value at position (i,i), where i is the index of that variable) should not be 0. If a 0 variance value is encountered for a variable that is being fused, the state estimation nodes will add a small epsilon value (1e−6) to that value. A better solution is for users to set covariances appropriately.

Ok, so let's set the covariances appropriately. I think I only have two options: calculate it or get the values from the datasheet.

So, let's look at the datasheet. I'm using the X-NUCLEO-IKS01A1 board with LSM6DS0 IMU. The datasheet is here. As you can see there's a table on page 9 that talks about noise, like 'Gyroscope RMS noise in normal/low-power mode', etc. But how can I relate these values to variances? I didn't find anything on the web apart from this answer here where they say:

If you haven't got a background in random processes and signal analysis then you're going to have a rough time relating this back to real-world numbers, particularly if you're doing any kind of sensor fusion. Even the "big boys" in the sensor fusion game can't easily map sensor noise to system behavior without lots of simulation and head-scratching.

The other option is to calculate/approximate the covariance matrix. Again, I didn't find any standard approach to do it. I came up with the idea to just collect data for a while, placing the IMU in a very firm way, then calculating the variance (so assuming the covariance matrix diagonal). Does it make sense an approach like this? I see that you calculated an estimate of the covariance from the sensor resolution and then you tuned it (from 0.0004 to 0.1404). How did you tune it and how did you get this number?

Moreover, do you know if there's a standard approach to set the covariance matrix? What's the complete spectrum of the alternatives?

• initial_estimate_covariance and process_noise_covariance

You've also tuned the initial_estimate_covariance and process_noise_covariance matrices, how did you do it?

Lastly, if you have some references to books or papers about this stuff could you please share them?

Thanks for all what you're doing to make the sensor fusion topic clearer for everyone.

[methylDragon]

Hi again! Thanks for bringing this up.

I've added a section in the first tutorial on kalman filter tuning, and my experiences with this.

But the long and short of it is I did most of it with organised trial and error. While I'm sure there are more systematic ways to do it, I didn't find a need to delve so deep. You're welcome to go search for papers though, there are a lot of them!

Good luck, and have fun! CH3EERS

[ricber]

In the newly added section of Kalman Filter Tuning, Covariance matrices paragraph, shouldn't be:

Lower variances cause the Kalman Filter to trust incoming measurements more

Insetad of:

Higher variances cause the Kalman Filter to trust incoming measurements more

The same for Initial Estimate(?)

[methylDragon]

Hiya!

That point was gotten from the generated .yaml for robot_localisation that describes the effect of changing the process noise matrix values.

# 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.

Remember that the process noise matrix models the uncertainty in your physical model/kinematic equations. As such, increasing the variances for this causes the other factor, your sensor data, to be trusted more.

Likewise for initial estimates, if you distrust your estimates, you'll trust new incoming sensor readings more, leading to faster convergences.

Hope it helps, CH3EERS!

[ricber]

Yep, you're right :) Thanks for the help :+1: