Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Is AMCL's implementation of the odometry model correct?

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 (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 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)

(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, which implies that the variance should be a linear function of the distance. With the above formula from the second printing, the variance for repeated sampling will be proportional to the sum of the squared sections, while for sampling once it will be proportional to the squared sum of the sections. That means the estimation for the same observed motion will differ depending on how often and when exactly the estimation is done.

So to sum it up: We think the formula from the first printing is indeed right, just that d_trans_hat is the variance and not the standard deviation. The second printing corrected the standard deviation/variance mixup, but at the same time introduced a wrong formula for the previous correct one.

(sorry tried to post links as such but was not allowed to)

Is AMCL's implementation of the odometry model correct?

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 (github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp). (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 www.probabilistic-robotics.org, 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)

(en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables)(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, which implies that the variance should be a linear function of the distance. With the above formula from the second printing, the variance for repeated sampling will be proportional to the sum of the squared sections, while for sampling once it will be proportional to the squared sum of the sections. That means the estimation for the same observed motion will differ depending on how often and when exactly the estimation is done.

So to sum it up: We think the formula from the first printing is indeed right, just that d_trans_hat is the variance and not the standard deviation. The second printing corrected the standard deviation/variance mixup, but at the same time introduced a wrong formula for the previous correct one.

(sorry tried to post links as such but was not allowed to)

Is AMCL's implementation of the odometry model correct?

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, which implies that the variance should be a linear function of the distance. With the above formula from the second printing, the variance for repeated sampling will be proportional to the sum of the squared sections, while for sampling once it will be proportional to the squared sum of the sections. That means the estimation for the same observed motion will differ depending on how often and when exactly the estimation is done.

So to sum it up: We think the formula from the first printing is indeed right, just that d_trans_hat is the variance and not the standard deviation. The second printing corrected the standard deviation/variance mixup, but at the same time introduced a wrong formula for the previous correct one.

(sorry tried to post links as such but was not allowed to)

click to hide/show revision 4
added further explanation of difference between noise variance as linear vs. quadratic function of driven distance

Is AMCL's implementation of the odometry model correct?

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, which implies that the variance should be a linear function of the distance. distance:

n=alpha*b, o=alpha*c, m=alpha*a = alpha*(b+c) = n+o

With the above formula from the second printing, the variance for repeated sampling will be proportional to the sum of the squared sections, while for sampling once it will be proportional to the squared sum of the sections. :

n=alpha*b*b, o=alpha*c*c, m=alpha*(b+c)*(b+c) != n+o

That means the estimation for the same observed motion will differ depending on how often and when exactly the estimation is done.

So to sum it up: We think the formula from the first printing is indeed right, just that d_trans_hat is the variance and not the standard deviation. The second printing corrected the standard deviation/variance mixup, but at the same time introduced a wrong formula for the previous correct one.