Incorrect calculation in AMCL for angular part of the covariance matrix

[VorpalBlade]

AMCL computes the variance for the angle component in pf_cluster_stats() in pf.c, specifically in the following two blocks code:

Per cluster:

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

For the entire filter:

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

I believe these are actually incorrect, it is computing here not the variance, but the standard deviation. This is based on my understanding from (Mardia and Jupp, 2000), section 2.3.1 and 2.3.3. Given that it is put in a covariance matrix it should be the variance, not the standard deviation that is used.

The equation for circular variance should be (I think):

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

(and similar for the other case).

Of course, fixing this might break existing users that might depend on this bug (it is directly visible to the user via the topic to /amcl_pose).

References: Mardia, K.V., Jupp, P.E., 2000. Directional statistics, Wiley series in probability and statistics. J. Wiley, Chichester ; New York.

[JzHuai0108]

According to here, S(z) S(z) = 2Var(z). That is, in code, -2 log(sqrt(m[2] m[2] + m[3] m[3])) = 2(1 - sqrt(m[2] m[2] + m[3] m[3])). So no worry, the angle covariance part is OK.

Even the above calculation is wrong, it would not affect the performance as the calculated covariance, as a by-product, does not affect the subsequent steps of AMCL.

Thanks to Zhiwei Wang and Xiaonan Ji from Segway Robotics for fruitful discussions on this issue.

[VorpalBlade]

According to here, S(z) S(z) = 2_Var(z). That is, in code, -2 log(sqrt(m[2] m[2] + m[3] m[3])) = 2_(1 - sqrt(m[2] m[2] + m[3] m[3])).

So you are saying that the two equations are basically identical? I looked at the equations and could not figure out a way to rewrite one into the other. I will have to dig deeper into this at some point. Sounds interesting.

[JzHuai0108]

A non-rigorous but intuitive derivation can be found by using the truncated Taylor series. ln(1+x) ~ x when x is small. The factor "2" in S(z) * S(z) = 2Var(z), I believe, comes from the definition of circular variance.

[VorpalBlade]

Thanks. I'll close this bug then.