ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Fri, 01 Feb 2019 00:46:48 -0600Is AMCL's implementation of the odometry model correct?https://answers.ros.org/question/54467/is-amcls-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](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](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](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:
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.Wed, 06 Feb 2013 04:29:39 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/Comment by Christof Schroeter for <div class="snippet"><p>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.</p>
<p>AMCL implements the motion model in file (<a href="https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp"></a><a href="https://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>). 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.)</p>
<p>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:</p>
<p>In the <strong>second printing</strong> (and it seems all later ones), which apparently has been used as the reference for AMCL, the formula for the translation uncertainty is</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans * d_trans +
alpha4 * (d_rot1 * d_rot1 + d_rot2 * d_rot2))</p>
</blockquote>
<p>In the <strong>first printing</strong> it was</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans + alpha4 * (|d_rot1| + |d_rot2|)</p>
</blockquote>
<p>but the sample() function was described as taking the standard deviation as parameter.</p>
<p>The errata for the first version, available at <a href="http://www.probabilistic-robotics.org"></a><a href="http://www.probabilistic-robotics.org">http://www.probabilistic-robotics.org</a>, says:</p>
<blockquote>
<p>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.</p>
</blockquote>
<p>In our opinion, this is right, but the change in the formula (from uncertainty being linear to quadratic in motion distance) "compensates" this correction.</p>
<p>Imagine driving a straight line forward for a certain distance a. When sampling the motion (distance only), we end up with a normal distribution</p>
<p>A ~ N(a,m) (mean a, variance m)</p>
<p>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:</p>
<p>B ~ N(b,n)</p>
<p>C ~ N(c,o)</p>
<p>B+C ~ N(b+c, n+o) </p>
<p>(<a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables"></a><a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables">http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables</a>)</p>
<p>Since the overall motion was the same, distribution A should be the same as B+C, and therefore m=n+o ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54782#post-id-54782Can 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?Fri, 08 Feb 2013 11:37:40 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54782#post-id-54782Comment by Thomas D for <div class="snippet"><p>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.</p>
<p>AMCL implements the motion model in file (<a href="https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp"></a><a href="https://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>). 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.)</p>
<p>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:</p>
<p>In the <strong>second printing</strong> (and it seems all later ones), which apparently has been used as the reference for AMCL, the formula for the translation uncertainty is</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans * d_trans +
alpha4 * (d_rot1 * d_rot1 + d_rot2 * d_rot2))</p>
</blockquote>
<p>In the <strong>first printing</strong> it was</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans + alpha4 * (|d_rot1| + |d_rot2|)</p>
</blockquote>
<p>but the sample() function was described as taking the standard deviation as parameter.</p>
<p>The errata for the first version, available at <a href="http://www.probabilistic-robotics.org"></a><a href="http://www.probabilistic-robotics.org">http://www.probabilistic-robotics.org</a>, says:</p>
<blockquote>
<p>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.</p>
</blockquote>
<p>In our opinion, this is right, but the change in the formula (from uncertainty being linear to quadratic in motion distance) "compensates" this correction.</p>
<p>Imagine driving a straight line forward for a certain distance a. When sampling the motion (distance only), we end up with a normal distribution</p>
<p>A ~ N(a,m) (mean a, variance m)</p>
<p>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:</p>
<p>B ~ N(b,n)</p>
<p>C ~ N(c,o)</p>
<p>B+C ~ N(b+c, n+o) </p>
<p>(<a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables"></a><a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables">http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables</a>)</p>
<p>Since the overall motion was the same, distribution A should be the same as B+C, and therefore m=n+o ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54470#post-id-54470Great 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.Wed, 06 Feb 2013 04:44:14 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54470#post-id-54470Comment by Christof Schroeter for <div class="snippet"><p>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.</p>
<p>AMCL implements the motion model in file (<a href="https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp"></a><a href="https://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>). 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.)</p>
<p>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:</p>
<p>In the <strong>second printing</strong> (and it seems all later ones), which apparently has been used as the reference for AMCL, the formula for the translation uncertainty is</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans * d_trans +
alpha4 * (d_rot1 * d_rot1 + d_rot2 * d_rot2))</p>
</blockquote>
<p>In the <strong>first printing</strong> it was</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans + alpha4 * (|d_rot1| + |d_rot2|)</p>
</blockquote>
<p>but the sample() function was described as taking the standard deviation as parameter.</p>
<p>The errata for the first version, available at <a href="http://www.probabilistic-robotics.org"></a><a href="http://www.probabilistic-robotics.org">http://www.probabilistic-robotics.org</a>, says:</p>
<blockquote>
<p>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.</p>
</blockquote>
<p>In our opinion, this is right, but the change in the formula (from uncertainty being linear to quadratic in motion distance) "compensates" this correction.</p>
<p>Imagine driving a straight line forward for a certain distance a. When sampling the motion (distance only), we end up with a normal distribution</p>
<p>A ~ N(a,m) (mean a, variance m)</p>
<p>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:</p>
<p>B ~ N(b,n)</p>
<p>C ~ N(c,o)</p>
<p>B+C ~ N(b+c, n+o) </p>
<p>(<a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables"></a><a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables">http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables</a>)</p>
<p>Since the overall motion was the same, distribution A should be the same as B+C, and therefore m=n+o ...<span class="expander"> <a>(more)</a></span></p></div>https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54484#post-id-54484done. thanks for the hint.Wed, 06 Feb 2013 06:58:43 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54484#post-id-54484Answer by quentin for <div class="snippet"><p>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.</p>
<p>AMCL implements the motion model in file (<a href="https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp"></a><a href="https://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>). 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.)</p>
<p>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:</p>
<p>In the <strong>second printing</strong> (and it seems all later ones), which apparently has been used as the reference for AMCL, the formula for the translation uncertainty is</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans * d_trans +
alpha4 * (d_rot1 * d_rot1 + d_rot2 * d_rot2))</p>
</blockquote>
<p>In the <strong>first printing</strong> it was</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans + alpha4 * (|d_rot1| + |d_rot2|)</p>
</blockquote>
<p>but the sample() function was described as taking the standard deviation as parameter.</p>
<p>The errata for the first version, available at <a href="http://www.probabilistic-robotics.org"></a><a href="http://www.probabilistic-robotics.org">http://www.probabilistic-robotics.org</a>, says:</p>
<blockquote>
<p>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.</p>
</blockquote>
<p>In our opinion, this is right, but the change in the formula (from uncertainty being linear to quadratic in motion distance) "compensates" this correction.</p>
<p>Imagine driving a straight line forward for a certain distance a. When sampling the motion (distance only), we end up with a normal distribution</p>
<p>A ~ N(a,m) (mean a, variance m)</p>
<p>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:</p>
<p>B ~ N(b,n)</p>
<p>C ~ N(c,o)</p>
<p>B+C ~ N(b+c, n+o) </p>
<p>(<a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables"></a><a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables">http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables</a>)</p>
<p>Since the overall motion was the same, distribution A should be the same as B+C, and therefore m=n+o ...<span class="expander"> <a>(more)</a></span></p></div> https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?answer=241473#post-id-241473Hi there,
Thanks a lot for these explanations. I am concerned about a way to compute these α₁, α₂, α₃, and α₄ for a physical robot? This [paper](http://www-personal.umich.edu/~johannb/Papers/paper60.pdf) 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!
QuentinWed, 10 Aug 2016 06:02:08 -0500https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?answer=241473#post-id-241473Comment by SleepyTurtle for <p>Hi there, </p>
<p>Thanks a lot for these explanations. I am concerned about a way to compute these α₁, α₂, α₃, and α₄ for a physical robot? This <a href="http://www-personal.umich.edu/~johannb/Papers/paper60.pdf">paper</a> suggest some methods but I don't understand how to link the results of these tests to the AMCL param...</p>
<p>Any idea?
Thanks a lot!
Quentin</p>
https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=314435#post-id-314435Hi, did you find an answer to your question? I'm currently struggling with this as well.Fri, 01 Feb 2019 00:46:48 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=314435#post-id-314435Answer by hersh for <div class="snippet"><p>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.</p>
<p>AMCL implements the motion model in file (<a href="https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp"></a><a href="https://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>). 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.)</p>
<p>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:</p>
<p>In the <strong>second printing</strong> (and it seems all later ones), which apparently has been used as the reference for AMCL, the formula for the translation uncertainty is</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans * d_trans +
alpha4 * (d_rot1 * d_rot1 + d_rot2 * d_rot2))</p>
</blockquote>
<p>In the <strong>first printing</strong> it was</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans + alpha4 * (|d_rot1| + |d_rot2|)</p>
</blockquote>
<p>but the sample() function was described as taking the standard deviation as parameter.</p>
<p>The errata for the first version, available at <a href="http://www.probabilistic-robotics.org"></a><a href="http://www.probabilistic-robotics.org">http://www.probabilistic-robotics.org</a>, says:</p>
<blockquote>
<p>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.</p>
</blockquote>
<p>In our opinion, this is right, but the change in the formula (from uncertainty being linear to quadratic in motion distance) "compensates" this correction.</p>
<p>Imagine driving a straight line forward for a certain distance a. When sampling the motion (distance only), we end up with a normal distribution</p>
<p>A ~ N(a,m) (mean a, variance m)</p>
<p>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:</p>
<p>B ~ N(b,n)</p>
<p>C ~ N(c,o)</p>
<p>B+C ~ N(b+c, n+o) </p>
<p>(<a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables"></a><a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables">http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables</a>)</p>
<p>Since the overall motion was the same, distribution A should be the same as B+C, and therefore m=n+o ...<span class="expander"> <a>(more)</a></span></p></div> https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?answer=55048#post-id-55048I 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](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.Tue, 12 Feb 2013 07:49:27 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?answer=55048#post-id-55048Answer by Chad Rockey for <div class="snippet"><p>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.</p>
<p>AMCL implements the motion model in file (<a href="https://github.com/ros-planning/navigation/blob/groovy-devel/amcl/src/sensors/amcl_odom.cpp"></a><a href="https://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>). 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.)</p>
<p>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:</p>
<p>In the <strong>second printing</strong> (and it seems all later ones), which apparently has been used as the reference for AMCL, the formula for the translation uncertainty is</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans * d_trans +
alpha4 * (d_rot1 * d_rot1 + d_rot2 * d_rot2))</p>
</blockquote>
<p>In the <strong>first printing</strong> it was</p>
<blockquote>
<p>d_trans_hat = d_trans -
sample(alpha3 * d_trans + alpha4 * (|d_rot1| + |d_rot2|)</p>
</blockquote>
<p>but the sample() function was described as taking the standard deviation as parameter.</p>
<p>The errata for the first version, available at <a href="http://www.probabilistic-robotics.org"></a><a href="http://www.probabilistic-robotics.org">http://www.probabilistic-robotics.org</a>, says:</p>
<blockquote>
<p>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.</p>
</blockquote>
<p>In our opinion, this is right, but the change in the formula (from uncertainty being linear to quadratic in motion distance) "compensates" this correction.</p>
<p>Imagine driving a straight line forward for a certain distance a. When sampling the motion (distance only), we end up with a normal distribution</p>
<p>A ~ N(a,m) (mean a, variance m)</p>
<p>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:</p>
<p>B ~ N(b,n)</p>
<p>C ~ N(c,o)</p>
<p>B+C ~ N(b+c, n+o) </p>
<p>(<a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables"></a><a href="http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables">http://en.wikipedia.org/wiki/Sum_of_normally_distributed_random_variables</a>)</p>
<p>Since the overall motion was the same, distribution A should be the same as B+C, and therefore m=n+o ...<span class="expander"> <a>(more)</a></span></p></div> https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?answer=54691#post-id-54691Good 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](/upfiles/13603653332947028.png)Thu, 07 Feb 2013 11:42:05 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?answer=54691#post-id-54691Comment by Christof Schroeter for <p>Good find, the AMCL software package doesn't have this right. I made an issue to track this:
<a href="https://github.com/ros-planning/navigation/issues/20">https://github.com/ros-planning/navigation/issues/20</a></p>
<p>As for the distributions, summing the variance is absolutely required, as seen here: <a href="http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html">http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html</a></p>
<p>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).</p>
<p>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.</p>
<hr>
<p>Edit to show units of alpha, along with the units of the other values:</p>
<p><img alt="image description" src="/upfiles/13603653332947028.png"></p>
https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54779#post-id-54779I 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.Fri, 08 Feb 2013 11:35:10 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54779#post-id-54779Comment by Chad Rockey for <p>Good find, the AMCL software package doesn't have this right. I made an issue to track this:
<a href="https://github.com/ros-planning/navigation/issues/20">https://github.com/ros-planning/navigation/issues/20</a></p>
<p>As for the distributions, summing the variance is absolutely required, as seen here: <a href="http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html">http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html</a></p>
<p>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).</p>
<p>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.</p>
<hr>
<p>Edit to show units of alpha, along with the units of the other values:</p>
<p><img alt="image description" src="/upfiles/13603653332947028.png"></p>
https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54778#post-id-54778I'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.Fri, 08 Feb 2013 11:19:26 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54778#post-id-54778Comment by Christof Schroeter for <p>Good find, the AMCL software package doesn't have this right. I made an issue to track this:
<a href="https://github.com/ros-planning/navigation/issues/20">https://github.com/ros-planning/navigation/issues/20</a></p>
<p>As for the distributions, summing the variance is absolutely required, as seen here: <a href="http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html">http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html</a></p>
<p>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).</p>
<p>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.</p>
<hr>
<p>Edit to show units of alpha, along with the units of the other values:</p>
<p><img alt="image description" src="/upfiles/13603653332947028.png"></p>
https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54740#post-id-54740you 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?Fri, 08 Feb 2013 02:09:13 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54740#post-id-54740Comment by Christof Schroeter for <p>Good find, the AMCL software package doesn't have this right. I made an issue to track this:
<a href="https://github.com/ros-planning/navigation/issues/20">https://github.com/ros-planning/navigation/issues/20</a></p>
<p>As for the distributions, summing the variance is absolutely required, as seen here: <a href="http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html">http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html</a></p>
<p>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).</p>
<p>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.</p>
<hr>
<p>Edit to show units of alpha, along with the units of the other values:</p>
<p><img alt="image description" src="/upfiles/13603653332947028.png"></p>
https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54739#post-id-54739Thank 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:Fri, 08 Feb 2013 02:07:20 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54739#post-id-54739Comment by Christof Schroeter for <p>Good find, the AMCL software package doesn't have this right. I made an issue to track this:
<a href="https://github.com/ros-planning/navigation/issues/20">https://github.com/ros-planning/navigation/issues/20</a></p>
<p>As for the distributions, summing the variance is absolutely required, as seen here: <a href="http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html">http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html</a></p>
<p>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).</p>
<p>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.</p>
<hr>
<p>Edit to show units of alpha, along with the units of the other values:</p>
<p><img alt="image description" src="/upfiles/13603653332947028.png"></p>
https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54742#post-id-54742The 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.Fri, 08 Feb 2013 02:13:33 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54742#post-id-54742Comment by Christof Schroeter for <p>Good find, the AMCL software package doesn't have this right. I made an issue to track this:
<a href="https://github.com/ros-planning/navigation/issues/20">https://github.com/ros-planning/navigation/issues/20</a></p>
<p>As for the distributions, summing the variance is absolutely required, as seen here: <a href="http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html">http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html</a></p>
<p>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).</p>
<p>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.</p>
<hr>
<p>Edit to show units of alpha, along with the units of the other values:</p>
<p><img alt="image description" src="/upfiles/13603653332947028.png"></p>
https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54741#post-id-54741 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.Fri, 08 Feb 2013 02:11:46 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54741#post-id-54741Comment by hersh for <p>Good find, the AMCL software package doesn't have this right. I made an issue to track this:
<a href="https://github.com/ros-planning/navigation/issues/20">https://github.com/ros-planning/navigation/issues/20</a></p>
<p>As for the distributions, summing the variance is absolutely required, as seen here: <a href="http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html">http://apcentral.collegeboard.com/apc/members/courses/teachers_corner/50250.html</a></p>
<p>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).</p>
<p>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.</p>
<hr>
<p>Edit to show units of alpha, along with the units of the other values:</p>
<p><img alt="image description" src="/upfiles/13603653332947028.png"></p>
https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54978#post-id-54978The 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.Mon, 11 Feb 2013 13:38:27 -0600https://answers.ros.org/question/54467/is-amcls-implementation-of-the-odometry-model-correct/?comment=54978#post-id-54978