Lab 4 1.Get an image into a ROS node 2.Find all the orange pixels (suggest HSV) 3.Identify the midpoint of all the orange pixels 4.Explore the findContours and simpleBlobDetection functions in OpenCV. Give a 1-2 sentence description of both, and then use one of them 5.Extra credit: use a video rather than an image

Matching ® ® ® Global Map Local Map … … … obstacle Where am I on the global map?                                            Examine different possible robot positions.

General approach: A: action S: pose O: observation Position at time t depends on position previous position and action, and current observation

1.Uniform Prior 2.Observation: see pillar 3.Action: move right 4.Observation: see pillar

Localization Sense Move Initial Belief Gain Information Lose Information

Bayes Formula

Simple Example of State Estimation

Example  P(z|open) = 0.6P(z|  open) = 0.3  P(open) = P(  open) = 0.5

Actions Often the world is dynamic since – actions carried out by the robot, – actions carried out by other agents, – or just the time passing by change the world. How can we incorporate such actions?

Typical Actions Actions are never carried out with absolute certainty. In contrast to measurements, actions generally increase the uncertainty. (Can you think of an exception?)

Modeling Actions

Example: Closing the door

for : If the door is open, the action “close door” succeeds in 90% of all cases.

Integrating the Outcome of Actions Applied to status of door, given that we just (tried to) close it?

Integrating the Outcome of Actions P(closed | u) = P(closed | u, open) P(open) + P(closed|u, closed) P(closed)

Example: The Resulting Belief

OK… but then what’s the chance that it’s still open?

Example: The Resulting Belief

Summary Bayes rule allows us to compute probabilities that are hard to assess otherwise. Under the Markov assumption, recursive Bayesian updating can be used to efficiently combine evidence.

Example

s P(s)

Example

How does the probability distribution change if the robot now senses a wall?

Example 2 0.2

Example Robot senses yellow. Probability should go up. Probability should go down.

Example 2 States that match observation Multiply prior by 0.6 States that don’t match observation Multiply prior by Robot senses yellow

Example 2 ! The probability distribution no longer sums to 1! Normalize (divide by total) Sums to 0.36

Nondeterministic Robot Motion R The robot can now move Left and Right

Nondeterministic Robot Motion R The robot can now move Left and Right When executing “move x steps to right” or left:.8: move x steps.1: move x-1 steps.1: move x+1 steps

Nondeterministic Robot Motion Right 2 The robot can now move Left and Right When executing “move x steps to right”:.8: move x steps.1: move x-1 steps.1: move x+1 steps

Nondeterministic Robot Motion Right 2 The robot can now move Left and Right ( ) 0.1 When executing “move x steps to right”:.8: move x steps.1: move x-1 steps.1: move x+1 steps

Nondeterministic Robot Motion What is the probability distribution after 1000 moves?

Example

Right

Example

Right

Kalman Filter Model Gaussian σ2σ2

Kalman Filter Sense Move Initial Belief Gain Information Lose Information Bayes Rule (Multiplication) Convolution (Addition) Gaussian: μ, σ 2

Measurement Example μ, σ 2 v, r 2

Measurement Example μ, σ 2 v, r 2

Measurement Example μ, σ 2 v, r 2 σ 2 ’?

Measurement Example μ, σ 2 v, r 2 μ', σ 2 ’ To calculate, go through and multiply the two Gaussians and renormalize to sum to 1 Also, the multiplication of two Gaussian random variables is itself a Gaussian

Multiplying Two Gaussians 1. (ignoring the constant term in front) 2.To find the mean, need to maximize this function (i.e. take derivative) 3.Set equal to and solve for 4.…something similar for the variance μ, σ 2 v, r 2 1.(ignoring constant term in front) 2.To find mean, maximize function (i.e., take derivative) Set equal to 0 and solve for x 5. 6.… something similar for the variance:

Another point of view… μ, σ 2 v, r 2 μ', σ 2 ’ : p(x) : p(z|x) : p(x|z)

Example

μ’=12.4 σ 2 ’=1.6

Kalman Filter Sense Move Initial Belief Lose Information Convolution (Addition) Gaussian: μ, σ 2

Motion Update For motion Model of motion noise μ’=μ+u σ 2 ’=σ 2 +r 2 u, r 2

Motion Update For motion Model of motion noise μ’=μ+u σ 2 ’=σ 2 +r 2 u, r 2

Motion Update For motion Model of motion noise μ’=μ+u=18 σ 2 ’=σ 2 +r 2 =10 u, r 2

Kalman Filter Sense Move Initial Belief Gaussian: μ, σ 2 μ’=μ+u σ 2 ’=σ 2 +r 2

Kalman Filter in Multiple Dimensions 2D Gaussian