Question about AMCL covariance calculation
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.
