ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Question about AMCL covariance calculation

asked 2018-05-13 13:10:41 -0500

Umkipp gravatar image

Hello everyone,

I'm quite new to robotics and ROS and have worked the past several months with nav2d. In that context I have tried to understand how AMCL (used in the localization of nav2d) works.

I have stumbled upon a little mystery (for me, at least) concerning the calculation of the covariance matrix cov.m[][], more precicely the last of the nine values of it.

As far as I have understood the covariance matrix in general, its size depends on the dimensionality of the problem; if we move in a 2-dimensional space, we have a 2x2 matrix, while in a 3-dimensional space, we have a 3x3 matrix. As AMCL is probably developed to be used for 2D as well as 3D localization, it offers a 3x3 matrix, which I would assume would only be parially filled with data in a 2D-environment.

While looking at the data inside this matrix during a run in nav2d (2-dimensional problem), I indeed found, that the matrix does not contain values for the covariance of the (presumed) third dimension. But it does for the third variance. Example of the matrix, refined for a little bit better overview:

(9.792434; -7.420428; 0.000000; )

(-7.420428; 19.095132; 0.000000; )

(0.000000; 0.000000; 3.259822; )

The third variance (m[2][2]) gets automatically calculated in the file pf.c, in the function called pf_cluster_stats, right at the bottom of it, with the formula:

set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));

My question ist now, what this third variance represents and why it gets automatically computed. I would have assumed, that it would only be computed if a third dimension was present, but as nav2d is generally 2-dimensional and this value has a content non-the-less (which is no wonder, as it gets computed by the AMCL anyway, no matter what it is used for), it would seem, that this assumption is wrong?

I would be thankful for any hint in that matter.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2018-05-13 17:43:19 -0500

There are a few things to clear up here. Firstly the size of the covariance matrix is not determined by the number of Cartesian dimensions but by the number of degrees of freedom. So in the case of a robot driving over a flat surface those degrees of freedom are the x & y position and the rotation angle. Hence why there is a 3x3 covariance matrix.

If the robot were an aerial drone for example there would be 6 degrees of freedom: x, y, z, roll, pitch and yaw. In this case the pose error would be represented by a 6x6 covariance matrix.

Covariance matrices describe not only the error in each variable (represented by the diagonal values) but also the coupling of these errors. This page has a good 2D example assuming the errors are Gaussian showing the meaning of the two off diagonal values.

Coming back to your 3x3 covariance matrix, the three diagonal values represent the variance of the error in x, y, and angle respectively. The two off diagonal values in the top left corner representing the coupling of the x and y errors, in this case they are negatively correlated. The remaining off diagonal values are all zero because there is no correlation between the error in the angle and the error in the position.

I hope this makes more sense now.

edit flag offensive delete link more


Yes, thank you for that clarification, and also for the link to the Website. I found going through that quite informative.

Umkipp gravatar image Umkipp  ( 2018-05-15 18:22:49 -0500 )edit

answered 2018-05-13 17:35:43 -0500

In most robotic systems, this estimation problem refers to estimating both position and orientation of robots. In a 2 dimensional Cartesian coordinate system, the number of dimensionality for position is 2 (x and y) and the dimensionality number for orientation is 1 (yaw). Therefore, you can find the state vector of each particle is 3-dimensional. It results in 3 by 3 matrix of covariance representing the covariances of x, y, and yaw. There is the answer you want. The 3,3-entry is orientation variance. Hope this would answer your question.

edit flag offensive delete link more

Question Tools

1 follower


Asked: 2018-05-13 13:10:41 -0500

Seen: 1,822 times

Last updated: May 13 '18