Is AMCL's implementation of the odometry model correct?

edit

Hi there, after a lengthy internal discussion about a motion model issue my colleague Erik discovered, I decided to take this question here. We have been experimenting with motion models for differential drive and compared to various available implementions, one of them being ROS AMCL.

AMCL implements the motion model in file (https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp). A comment at line 150 refers to an algorithm (sample_motion_model_odometry) presented in "Thrun,Burgard,Fox: Probabilistic Robotics". The uncertainty parameters are calculated exactly following the reference (lines 180-189). However, the implementation differs in one detail: the algorithm in the book passes the calculated uncertainty to function sample(), which takes the variance as parameter, while the AMCL implementation calls function pf_ran_gaussian(), which expects as parameter the standard deviation (and indeed samples accordingly). (Note that function sample() is not explained directly for algorithm sample_motion_model_odometry, but for sample_motion_model_velocity at page 124, and there is no reason to assume it should be different in these two algorithms.)

The discussion is complicated considerably by the fact that several printings of the book exist which differ significantly in the algorithms for the motion model. Eventually, we'd question the validity of the uncertainty formula given in the book at all. While this is not originally a ROS issue, it is related to the implementation error above (and in particular its correction) and I present it here in the hope to find some verification (or falsification) of our view:

In the second printing (and it seems all later ones), which apparently has been used as the reference for AMCL, the formula for the translation uncertainty is

d_trans_hat = d_trans - sample(alpha3 * d_trans * d_trans + alpha4 * (d_rot1 * d_rot1 + d_rot2 * d_rot2))

In the first printing it was

d_trans_hat = d_trans - sample(alpha3 * d_trans + alpha4 * (|d_rot1| + |d_rot2|)

but the sample() function was described as taking the standard deviation as parameter.

The errata for the first version, available at http://www.probabilistic-robotics.org, says:

Throughout chapters 5 and 7, the book uses standard deviations as parameters of the function prob() and the error variables \varspeilon. The problem with those expressions is that standard deviations are not additive. The correct notation (which will be in the second printing) involves variances, which are additive.

In our opinion, this is right, but the change in the formula (from uncertainty being linear to quadratic in motion distance) "compensates" this correction.

Imagine driving a straight line forward for a certain distance a. When sampling the motion (distance only), we end up with a normal distribution

A ~ N(a,m) (mean a, variance m)

When we decide to sample twice during the same motion, we get estimated distances for 2 sections, which we have to sum up for the estimation of the overall distance:

B ~ N(b,n)

C ~ N(c,o)

B+C ~ N(b+c, n+o)

(http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables)

Since the overall motion was the same, distribution A should be the same as B+C, and therefore m=n+o ... (more)