Setting covarince matrix for Robot Localization

asked 2020-05-05 11:45:46 -0600

elgarbe
106 ●27 ●34 ●41

updated 2020-05-07 11:35:05 -0600

Hi, I'm working with a custom made rover. I has 4 wheels, 2 of them with encoders. I have an onboard computer with an STM32F722 uC. I have an IMU and a Magnetometer connected to it. Usin rosserial node I publish sensor data to ROS master on a ODROID XU4. There I will run an imu_madgick_filter node in order to get orientation and then fusing it with odometry from wheel encoder. Now I want to set covariance matrices field values for each sensor. Each ROS message expect a covariance matrix for that sensor, in particular IMU and Odom. I would like to know a way to get this values. Can somenone point me out to some reading material for this subject? or can someone gives me some starting values?

EDIT1: I need to get a reliable orientation with imu_filter_madgwick as RayROS suggest. So I opened another question here.

EDIT2: I already test imu_filter_madgwick and it doesn't take into account covariance matrix from sensor data. It pass through whatever covariance the original message have to the output filtered imu data. So how can I get reliable values for angular_velocity_covariance and linear_acceleration_covariance on IMU messages from my sensor data

Thank

edit retag flag offensive close merge delete

add a comment

answered 2020-05-05 16:21:18 -0600

RayROS
108 ●43 ●47 ●52

Hi, covariance matrices are an indication of how much you trust your sensor and therefore the quantity of noise you should introduce. Tuning the covariance matrix is an indication of the degree of uncertainity. You could get this values either from the example template. Or below is how I tuned my robot. My robot had: a pressure sensor, a depth sensor (very accurate, you can see from the low uncertainty I gave), an altitude sensor (this also was good enough), I also used madgick_filter in order to get the heading from imu and from an underwater positioning sensor (sort of underwater gps). It is very important, that you carefully read this official source on how to set the parameters required by EKF.

process_noise_covariance: [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,    0,    0,    0,
                           0,    0,    1e-1, 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,    0.000002,  0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.000002,  0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.5, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    1.0,  0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    1.0,  0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    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,     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]

initial_estimate_covariance: [1000.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1000.0,  0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    100.0, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    100.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    100.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    100.0, 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.02,  0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    100.0,  0,     0,     0,     0 ...

(more)

edit flag offensive delete link

Comments

Thank you very much for your explanation. In fact I'm wondering about covariance of the sensor_msg/IMU it needs an angular_velocity_covariance and linear_acceleration_covariance. The same for Magnetómeter msg and for Odometry data Those messages are build inside my uC and I'm not sure what to write there.

elgarbe ( 2020-05-05 16:56:28 -0600 )edit

Sure, ok I have also worked with Odroid systems putting Ubuntu Mate as a portable system. I have looked at the technical sheet of the product you mentioned STM32F722. If I were you I would: initial estimate covariance: Use high values as a starting point and for spreading the noise in an homogenous way. process_noise_covariance: I would for x, y, z use very low values as it is easy to calculate them and you are sure about the calculation, for example 0.001 could be a good start for both x, y, for z I would use a more safe value such as 0.0001 as movement along z-axis is very limited. Magnetometr covariance: you need to see the output of the filter_madgwick and how values are. Publish them here once you have them. For Odometry : Plot xdot, ydot, zdot and see their oscillation. From there you see the value ...(more)

RayROS ( 2020-05-06 09:04:24 -0600 )edit

Ok, I just tested imu_madgwick filter and I check that sensors covariance are not used by the filter and the filter does not write values to that filed. imu_filter just pass through whatever the original IMU message have on those fields. So the question still remain the same. How to get angular_velocity_covariance and linear_acceleration_covariance on IMU messages from my sensor data. If I understand right RL expect that I get reliable values on those fields. In this section it is stated that:

Covariance: Echoing the advice for odometry, make sure your covariances make sense. Do not use large values to get the filter to ignore a given variable. Set the configuration for the variable you’d like to ignore to false.

And in this section it talk about covariance.

elgarbe ( 2020-05-07 11:31:43 -0600 )edit

I have been thinking about your problem and from the EDIT2 you posted I wanted to ask you a question before suggesting something: Did you remap the filter_madgwick topic according to the documentation here?

RayROS ( 2020-05-11 17:20:47 -0600 )edit

add a comment

Setting covarince matrix for Robot Localization

1 Answer

Comments

Question Tools

Stats

Related questions

Setting covarince matrix for Robot Localization edit

1 Answer

Comments

Question Tools

Stats

Related questions

Setting covarince matrix for Robot Localization