Is AMCL's implementation of the odometry model correct?

asked 2013-02-06 04:29:39 -0600

updated 2013-02-08 05:00:18 -0600

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)

edit retag flag offensive close merge delete

Comments

Great write up of your question. With your new karma you should be able to edit your question to add in the link tags if you so desire.

Thomas D ( 2013-02-06 04:44:14 -0600 )edit

done. thanks for the hint.

Christof Schroeter ( 2013-02-06 06:58:43 -0600 )edit

Can anyone comment on the assumption that the final estimated distribution should not depend on the frequency of updating the estimation, and the finding that this conflicts with variance being a linear function of squared distance?

Christof Schroeter ( 2013-02-08 11:37:40 -0600 )edit

add a comment

7

answered 2013-02-07 11:42:05 -0600

Chad Rockey

4541 ●63 ●79 ●98 http://chadrockey.com/

updated 2013-02-08 11:15:54 -0600

Good find, the AMCL software package doesn't have this right. I made an issue to track this: https://github.com/ros-planning/navigation/issues/20

As for the distributions, summing the variance is absolutely required, as seen here: http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html

The rest of the algorithm is explained in the 3rd edition in section 5.4.3. d_trans must be in units of meters since it is the translation component of the odometry update. The lines you quote are the ones described as "To model the motion error, we assume that the "true" values of the rotation and translation are obtained from the measured ones by subtracting independent noise (SAMPLE) with zero mean and variance b^2). So the formula from edition 1 cannot be correct since d_trans and d_trans_hat are in meters, not meters^2 (see equation 5.38 and 5.40).

Finally, it may help to remember that this is sampling the noise for a particle filter where the overall distribution is tracked by the particles, not a closed-form estimate. Each particle in the filter experiences these updates, so the 'A' compoment of AMCL allows an adaptive (KLD sampling) approach that increases and decreases the number of particles in order to keep the particles from diverging and to ensure there are enough to track the important high-probability areas of distribution.

Edit to show units of alpha, along with the units of the other values:

image description

edit flag offensive delete link

Comments

Thank you, an interesting read about the variance summation, but please note that I did not at all question this property, but assumed it as the foundation of my argument.

Regarding the derivation of the variance formula from the variables' units I am sorry it is not entirely convincing:

Christof Schroeter ( 2013-02-08 02:07:20 -0600 )edit

you assume that the variance must be proportional to the square of the distance because its unit is in m^2. But then how do you explain summing distance and rotation within this formula, and calculating rotation variance from distance and vice versa?

Christof Schroeter ( 2013-02-08 02:09:13 -0600 )edit

Obviously, the alphas must have units themselves in these formulas, and that means you cannot equal the unit of one parameter with the unit of the result, which invalidates your argument in my opinion.

Christof Schroeter ( 2013-02-08 02:11:46 -0600 )edit

The eventual question remains whether the variance is proportional to the distance or the squared distance. In my original question I gave an argument why it should be the former.

Christof Schroeter ( 2013-02-08 02:13:33 -0600 )edit

I've posted a quick notebook page of the units in the equations. The only question left then is the sample space. Sample takes a random value from the distribution specified by that variance.

Chad Rockey ( 2013-02-08 11:19:26 -0600 )edit

I don't agree that your formula and units assignment is the only possible way. The units are right, but you can as well assign units to the alphas so that the units are right without squaring d_trans/d_rot. This is not a proof for one or the other formula.

Christof Schroeter ( 2013-02-08 11:35:10 -0600 )edit

The book does not seem to give a reason for why the noise is modeled as a sum of weighted squares, but the definition of variance is the squared deviation of the variable from its expected value.

hersh ( 2013-02-11 13:38:27 -0600 )edit

add a comment

2

answered 2013-02-12 07:49:27 -0600

hersh
1351 ●16 ●21 ●28

I agree with Chad, good catch finding the bug in AMCL.

The sampling function used in AMCL expects standard deviation as a parameter, and we are giving it something that acts like a variance.

The solution is to take the square root of the argument passed to the sampling function. As mentioned elsewhere, this may change behavior significantly, so I won't just change it and commit it. I'll first test on recorded data and on some running robots and see how different it looks. If the difference is small I might just commit the change, but if it looks like it will require everyone to re-tune their alpha values I will make new odometry model types with the corrected math, and deprecate the existing ones.

As for the math, this http://en.wikipedia.org/wiki/Variance defines variance as the expected value of the square of the difference between the variable and the mean. So

n = E[(B - b)²]

Therefore defining n = αb² just says that our error model assumes the difference between the actual and the average (B - b) grows linearly with distance (b√α), where the square root of the parameter α relates the error to the distance.

Since the variances of independent random variables add when the random variables are added, when we have different sources of error going into our model, we can add the variances together. Each variance piece is of the form αᵤᵥmᵥ², where αᵤᵥ relates to the error in the u measurement caused by motion in the v dimension, and mᵥ is the measured motion in the v dimension. α₁, α₂, α₃, and α₄ together seem to form something like a covariance matrix for the distance and rotation variables. The units of them are meters²/rad², rad²/meters², meters²/meters², and rad²/rad², respectively.

edit flag offensive delete link

add a comment

1

answered 2016-08-10 06:02:08 -0600

quentin
123 ●8 ●11 ●15

Hi there,

Thanks a lot for these explanations. I am concerned about a way to compute these α₁, α₂, α₃, and α₄ for a physical robot? This paper suggest some methods but I don't understand how to link the results of these tests to the AMCL param...

Any idea? Thanks a lot! Quentin

edit flag offensive delete link

Comments

Hi, did you find an answer to your question? I'm currently struggling with this as well.

SleepyTurtle ( 2019-02-01 00:46:48 -0600 )edit

add a comment

Is AMCL's implementation of the odometry model correct?

Comments

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

Is AMCL's implementation of the odometry model correct? edit

Comments

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

Is AMCL's implementation of the odometry model correct?