Lecture 03:localization + bayes filters

Katie DC

Jan. 28, 2020

Notes from Probabilistic Robotics Chapter 2 and 8

Admin

• HW1 due this Friday at 8pm

• Project Update 0 due Sunday 2/2 at midnight• 1pg pdf with your team name, team members, and a one paragraph summary

of your proposed project task

• Submit via Gradescope

• HW3 is a programming assignment to be demoed in lab

Fun facts about Monte Carlo Methods… a question which occurred to me in 1946 as I was convalescing from an illness and playingsolitaires. The question was what are the chances that a Canfield solitaire laid out with 52cards will come out successfully? After spending a lot of time trying to estimate them by purecombinatorial calculations, I wondered whether a more practical method than "abstractthinking" might not be to lay it out say one hundred times and simply observe and count thenumber of successful plays. This was already possible to envisage with the beginning of thenew era of fast computers, and I immediately thought of problems of neutron diffusion andother questions of mathematical physics, and more generally how to change processesdescribed by certain differential equations into an equivalent form interpretable as asuccession of random operations. Later [in 1946], I described the idea to John von Neumann,and we began to plan actual calculations. - Stanislaw Ulam, ~1940s

Monte Carlo methods vary, but tend to follow this pattern:1. Define a domain of possible inputs2. Generate inputs randomly from a probability distribution over the domain3. Perform a deterministic computation on the inputs4. Aggregate the results

Monte Carlo methods were central to the simulations required for theManhattan Project, and more recently has been extended to Sequential MonteCarlo in advanced signal processing and Bayesian inference. Also, the extensionMC Tree Search enabled AlphaGo.

Thought Experiment

Localization and Mapping

Maps and Representations

• Mapping is one of the fundamental problems in (mobile) robotics

• Maps allow robots to efficiently carry out their tasks and enable localization

• Successful robot systems rely on maps for localization, navigation, path planning, activity planning, control, etc.

Robotic system

The General Problem of Mapping

• Sensor interpretation• How do we extract relevant

information from raw sensor data?

• How do we represent and integrate this information over time?

• Robot locations must be estimated• How can we identify that we are at a

previously visited place?

• This problem is the so-called data association problemWhat does the environment look like?

Resulting Map Obtained with Ultrasound Sensors

8

Example: Incremental Updating of Occupancy Grids

9

The Localization Problem

• Determine the pose (state) of the robot relative to the given map of the environment.

• This is also known as position or state estimation problem.

• Given uncertainty in our measurements and ambiguity from locally symmetric environment, we need to recursively update our estimate or belief.

Bayes Filters

Bayes’s Formula

Notation and Definitions

• Discrete time model

𝑥𝑡1:𝑡2 = 𝑥𝑡1 , 𝑥𝑡1+1, 𝑥𝑡1+2, … , 𝑥𝑡2 sequence of robot states 𝑡1 to 𝑡2

• Robot takes one measurement at a time

𝑧𝑡1:𝑡2 = 𝑧𝑡1 , … , 𝑧𝑡2 sequence of all measurements from 𝑡1 to 𝑡2

• Control also exercised at discrete steps

𝑢𝑡1:𝑡2 = 𝑢𝑡1 , 𝑢𝑡1+1, 𝑢𝑡1+2, … , 𝑢𝑡2 sequence control inputs

State Evolution / Models

Evolution of the state and measurements are governed by probabilistic laws:

𝑝 𝑥𝑡 𝑥0:𝑡−1, 𝑧1:𝑡−1, 𝑢1:𝑡 describes state evolution / motion model

If the state is complete, we can succinctly state:

𝑝 𝑥𝑡 𝑥0:𝑡−1, 𝑧1:𝑡−1, 𝑢1:𝑡 = 𝑝 𝑥𝑡 𝑥𝑡−1, 𝑢𝑡Measurement process given by:

𝑝 𝑧𝑡 𝑥0:𝑡 , 𝑧1:𝑡−1, 𝑢0:𝑡−1Similarly, if measurement is complete:

𝑝 𝑧𝑡 𝑥0:𝑡 , 𝑧1:𝑡−1, 𝑢1:𝑡 = 𝑝 𝑧𝑡 𝑥𝑡

Discrete Bayes Filter Algorithm: Setup

• Evolution of the state is governed by probabilistic state transition:

𝑝 𝑥𝑡 𝑥𝑡−1, 𝑢𝑡• Measurement process given by:

𝑝 𝑧𝑡 𝑥𝑡

Robot’s Belief over statesBelief: Robot’s knowledge about the state of the environment

𝑏𝑒𝑙(𝑥𝑡) = 𝑝(𝑥𝑡|𝑧1:𝑡 , 𝑢1:𝑡)

Posterior distribution over state at time t given all past measurements and control

Prediction: 𝑏𝑒𝑙(𝑥𝑡) = 𝑝(𝑥𝑡|𝑧1:𝑡−1, 𝑢1:𝑡)

Calculating 𝑏𝑒𝑙(𝑥𝑡) from 𝑏𝑒𝑙(𝑥𝑡) is called correction or measurement update

Robotic system

Recursive Bayes Filter

Algorithm Bayes_Filter(𝑏𝑒𝑙 𝑥𝑡−1 , 𝑢𝑡 , 𝑧𝑡)

for all 𝑥𝑡 do:

𝑏𝑒𝑙 𝑥𝑡 = ∫ 𝑝(𝑥𝑡|𝑢𝑡,𝑥𝑡−1)𝑏𝑒𝑙(𝑥𝑡−1)𝑑𝑥𝑡−1

𝑏𝑒𝑙 𝑥𝑡 = 𝜂 𝑝 𝑧𝑡 𝑥𝑡 𝑏𝑒𝑙(𝑥𝑡)

end for

return 𝑏𝑒𝑙(𝑥𝑡)

𝑏𝑒𝑙 𝑥𝑡−1

𝑥𝑡

1

2

3

𝑝 𝑥𝑡|𝑢𝑡 , 1

𝑝 𝑥𝑡|𝑢𝑡 , 2

𝑝 𝑥𝑡|𝑢𝑡 , 3

𝑏𝑒𝑙 𝑥𝑡

𝑏𝑒𝑙(𝑥𝑡)

𝑝 𝑧𝑡 𝑥𝑡

tim

e =

1ti

me

= 2

tim

e =

0Discrete Bayes Filter - Illustration

Bayes Filters (1)

Bayes Filters (2)

Bayes Filters (3)

Bayes Filters (4)

Bayes Filters (5)

In summary

Bayes Filters are everywhere!

• Kalman filters

• Particle filters

• Hidden Markov models

• Dynamic Bayesian networks

• Partially Observable Markov Decision Processes (POMDPs)

• Bayes rule allows us to compute probabilities that are hard to assess otherwise

• Recursive Bayesian updating can efficiently combine evidence

• Bayes filters are a probabilistic tool for estimating the state of dynamic systems