AMCL mixes units when sampling the gaussian distribution

[chadrockey]

From ROS Answers:
http://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/

I looked into this and found that AMCL does mix units:
At
https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp#L119
and
https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp#L181
It passes in alpha * delta*delta, which results in units^2 passed into pf_ran_gaussian

However, this conflicts with the implementation of pf_ran_gaussian:
https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/pf/pf_pdf.c#L132

This generates a sampled normalized random gaussian number with zero_mean and standard deviation one (http://www.taygeta.com/random/gaussian.html), and then multiplies it by the standard_deviation passed into the function for the output in order to stretch the domain (http://en.wikipedia.org/wiki/Normal_distribution#General_normal_distribution). However, when we pass in a variance, we get a variance * gaussian out, which leaves us with unit^2. We then take this value and add it to the standard_deviation, thus mixing units of unit - unit^2.

The solution seems to be taking the square-root of the term we pass into pf_ran_gaussian.

[hershwg]

Agree totally. I'll work on it.

[mgruhler]

It seems to me, that the corrected models are never used.

In lines 518 and 647 of amcl_node.cpp, the function SetModelDiff is called for all odom_model_type_ parameters with the exception of ODOM_MODEL_OMNI, which leads to always having the ODOM_MODEL_DIFF in those cases.

Or did I miss anything?

Cheers

[hershwg]

Looks like I got distracted and stopped working on this, sorry. I'll see what I can do now...

[hershwg]

Marking this as fixed, but remember the change is in the hydro-devel branch, so won't be released for a while yet.

[ntraft]

As far as I can figure out, this is a secret feature that is not used by default, nor is documented on the wiki. Imagine my surprise when I found it in the source code. Is there a reason for that?

[DLu]

It is likely just an oversight. Please update the wiki in a way that you feel documents it appropriately.

[ntraft]

So do you folks use the corrected model in practice, then? Y'all have more experience than I do. Should the old model be deprecated?

[mikeferguson]

Given the lack of documentation, I would imagine most robots use the old model. Thus, their configuration is tuned to the old model. I presume the parameters need updated values to converge properly with the updated model? If so, we certainly don't want to remove the old model, and we can't really deprecate it until K-turtle (since Jade is already released) so we have until next year to make such a decision. It would not be removed until at earliest L-turtle (2017).

[ntraft]

Makes sense to me. Yes, the alpha params need to be much lower for the corrected model.

[chadrockey]

Unfortunately, it was in this state in the earliest code I was able to find (2003?). Given that the two models would behave much differently, the old model will probably have to remain in the code (and unless new parameter names are added, most likely the default as well). It has over a decade of inertia of people tuning these changes, and that's something I wouldn't feel comfortable breaking.