# Monte Carlo Localization ## Algorithm MCL (*Monte Carlo Localization*) is applicable to both local and global localization problem. It represents the belief $$bel(x\_t)$$by particles. The algorithm itself is basically a small modification of the previous particle filter algorithm we have discussed. $$ \overline{\chi}\_t = \chi\_t = \emptyset $$ $$ \text{for $m=1$ to $M$ do:} \ x^m\_t = \text{sample\_motion\_model}(u\_t, x^m\_{t-1}) \\ w^m\_t = \text{measurement\_model}(z\_t, x\_t^m, m) \\ \overline{\chi}\_t = \overline{\chi}\_t + \langle x^m\_t, w^m\_t\rangle \\ \text{endfor} $$ $$ \text{for $m=1$ to $M$ do:} \\ \text{draw $i$ with probability} \propto w^i\_t \\ \text{add $x^i\_t$ to $\chi\_t$} \\ \text{endfor} \\;\\ \text{return $\chi\_t$} $$ The algorithm is obtained by substituting the appropriate probabilistic model and perceptual model into the particle filter algorithm. MCL represents the belief by a set of $$M$$ particles where $$\chi\_t = { x^1\_t, x^2\_t, ..., x^M\_t}$$. The motion model takes a control command and produces a new state. The measurement model assigns importance weight to the particle based on the likelihood of measurement given the new predicted state and map environments, i.e. the landmarks. The two motion and measurement models can be implemented by any motion/measurement models described in **Robot Perception** section. ## Properties MCL can approximate almost any distribution of practical importance. Increasing the total number of particles increases the accuracy of the approximation. The number of particles $$M$$ is a parameter that enables the user to trade off the accuracy of the computation and the computational resources necessary to run MCL. A common strategy for setting $$M$$ is to keep sampling until the next pair of $$u\_t$$ and $$z\_t$$ has arrived. ## Random Particle Augmented MCL MCL, in its present form, solves the global localization problem but cannot recover from robot kidnapping p failures. We can solve this problem by introducing random particles to the set on every iteration. The question is how many random particles to add and when to add? One idea is to add particle based on some estimate of the localization performance. We need to monitor the probability of sensor measurements. $$ p(z\_t \mid z\_{1:t-1}, u\_{1:t}, m) $$ And relate it to the average measurement probability. By definition, an importance weight is a stochastic estimate of this probability. The average value approximates the desired probability as stated above. $$ \frac{1}{M}\sum\_{m=1}^{M} w^m\_t \approx p(z\_t \mid z\_{1:t-1}, u\_{1:t}, m) $$ There exist multiple reasons why the measurement probability may be low. The amount of sensor noise might be unnaturally high, or the particles may still be spread out during a global localization phase. For these reasons, it is a good idea to maintain a short-term average of the measurement likelihood, and relate it to the long-term average when determining the number of random samples. $$ \text{static $\omega\_{slow}$ $\omega\_{fast}$} \\ \overline{\chi}\_t = \chi\_t = \emptyset \\;\\ $$ $$ \text{for $m=1$ to $M$ do:} \ x^m\_t = \text{sample\_motion\_model}(u\_t, x^m\_{t-1}) \\ w^m\_t = \text{measurement\_model}(z\_t, x\_t^m, m) \\ \overline{\chi}*t = \overline{\chi}*t + \langle x^m\_t, w^m\_t\rangle \\ \omega*{avg} = w*{avg} + \frac{w^m\_t}{M} \\ \text{endfor} $$ $$ \omega\_{slow} = \omega\_{slow} + \alpha\_{slow}(\omega\_{avg} - \omega\_{slow}) \\ \omega\_{fast} = \omega\_{fast} + \alpha\_{fast}(\omega\_{avg} - \omega\_{fast}) $$ $$ \text{for $m=1$ to $M$ do:} \\ \text{with probability $max{0.0, 1.0 - \frac{\omega\_{fast}}{\omega\_{slow}}}$: add random particle to set} \\ \text{ else: draw $i$ with probability} \propto w^i\_t \\ \text{add $x^i\_t$ to $\chi\_t$} \\ \text{endfor} \\;\\ \text{return $\chi\_i$} $$ The algorithm requires that $$0 \leq \alpha\_{slow} \ll \alpha\_{fast}$$. The parameters $$\alpha\_{fast}$$ and $$\alpha\_{slow}$$ are decay rates for the exponential filters that estimate the long-term and short-term averages respectively. During the re-sampling process, a random sample is added with the following probability. $$ max{ 0.0, 1.0 - \frac{\omega\_{fast}}{\omega\_{slow}}} $$ Otherwise, the re-sampling proceeds in the familiar way. The probability of adding a random sample takes into consideration the divergence between the short-term and long-term average of the measurement likelihood. If the short-term likelihood is better or equal to the long-term likelihood, no random sample is added. However, if the short-term likelihood is much worse than the long-term one, random samples are added in proportion to the quotient of these values. In this way, a sudden decay in measurement likelihood induces an increased number of random samples. --- # Agent Instructions: Querying This Documentation If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question. Perform an HTTP GET request on the current page URL with the `ask` query parameter: ``` GET https://calvinfeng.gitbook.io/probabilistic-robotics/localization/grid-and-monte-carlo/monte-carlo-localization.md?ask= ``` The question should be specific, self-contained, and written in natural language. The response will contain a direct answer to the question and relevant excerpts and sources from the documentation. Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.