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.

Lab 4

1. Get an image into a ROS node2. Find all the orange pixels (suggest HSV)3. Identify the midpoint of all the orange pixels4. 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 MapLocal Map

…… …

obstacle

Where am I on the global map?

Examine different possible robot positions.

• General approach:

• A: action• S: pose• O: observationPosition 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

evidence

prior likelihood

)(

)()|()(

yP

xPxyPyxP

)()|()()|(),( xPxyPyPyxPyxP

x

xPxyP

xPxyPyxP

)()|(

)()|()(

Simple Example of State Estimation

• Suppose a robot obtains measurement • What is ?

Example

P(z|open) = 0.6 P(z|open) = 0.3 P(open) = P(open) = 0.5

67.03

2

5.03.05.06.0

5.06.0)|(

)()|()()|(

)()|()|(

zopenP

openpopenzPopenpopenzP

openPopenzPzopenP

raises the probability that the door is open.

)()()|(

)|(zP

openPopenzPzopenP

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

• To incorporate the outcome of an action u into the current “belief”, we use the conditional pdf

• This term specifies the pdf that executing changes the state from to .

Example: Closing the door

for :

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

open closed0.1 1

0.9

0

Integrating the Outcome of Actions

')'()',|()|( dxxPxuxPuxP

)'()',|()|( xPxuxPuxP

Continuous case:

Discrete case:

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

Integrating the Outcome of Actions

')'()',|()|( dxxPxuxPuxP

)'()',|()|( xPxuxPuxP

Continuous case:

Discrete case:

P(closed | u) = P(closed | u, open) P(open) + P(closed|u, closed) P(closed)

Example: The Resulting Belief

1615

83

11

85

109

)(),|(

)(),|(

)'()',|()|(

closedPcloseduclosedP

openPopenuclosedP

xPxuclosedPuclosedP


1615

83

11

85

109

)(),|(

)(),|(

)'()',|()|(

closedPcloseduclosedP

openPopenuclosedP

xPxuclosedPuclosedP

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


)|(1161

83

10

85

101

)(),|(

)(),|(

)'()',|()|(

uclosedP

closedPcloseduopenP

openPopenuopenP

xPxuopenPuopenP

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

Example

s

P(s)

Example

Example

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

Example 2

0.2 0.2 0.2 0.2 0.2

Example 2

0.2 0.2 0.2 0.2 0.2

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 0.2

0.2 0.2 0.2 0.2 0.2

Robot senses yellow.

0.04 0.12 0.12 0.04 0.04

Example 2

!

The probability distribution no longer sums to 1!

0.04 0.12 0.12 0.04 0.04

Normalize (divide by total)

.111 .333 .333 .111 .111

Sums to 0.36

Nondeterministic Robot Motion

R

The robot can now move Left and Right.

.111 .333 .333 .111 .111


R


.111 .333 .333 .111 .111

When executing “move x steps to right” or left:.8: move x steps.1: move x-1 steps.1: move x+1 steps


Right 2


0 1 0 0 0

0 0 0.1 0.8 0.1

When executing “move x steps to right”:.8: move x steps.1: move x-1 steps.1: move x+1 steps


Right 2


0 0.5 0 0.5 0

0.4 0.05 0.05 0.4 (0.05+0.05)0.1

When executing “move x steps to right”:.8: move x steps.1: move x-1 steps.1: move x+1 steps


What is the probability distribution after 1000 moves?

0 0.5 0 0.5 0

Example

ExampleRight

Example

ExampleRight

Kalman Filter Model

Area under the curve sums to

𝜇

Gaussian

(in for now)

σ2

Kalman Filter

Sense Move

Initial Belief

Gain Information Lose

Information

Bayes Rule(Multiplication)

Convolution(Addition)

Gaussian:μ, σ2

Measurement ExamplePrior Position Estimate Measurement Estimate

μ, σ2

v, r2


Where is the new mean ?

μ, σ2

v, r2


What is the new covarance ?

μ, σ2

v, r2

σ2’?

Measurement ExamplePrior Position Estimate Measurement Estimate New Estimate

μ, σ2

v, r2

μ', σ2’

To calculate, go through and multiply the two Gaussians and renormalize to sum to 1Also, the multiplication of two Gaussian random variables is itself a Gaussian

Another point of view…

Prior Position Estimate Measurement Estimate New Estimate

μ, σ2

v, r2

μ', σ2’

: p(x): p(z|x): p(x|z)

ExamplePrior Position Estimate Measurement Estimate



μ’=12.4σ2’=1.6

Kalman Filter

Sense Move

Initial Belief

LoseInformation

Convolution(Addition)

Gaussian:μ, σ2

Motion Update

• For motion

Model of motion noise

μ’=μ+u

σ2’=σ2+r2

u, r2

Motion Update

• For motion


μ’=μ+u

σ2’=σ2+r2

u, r2

Prior Position Estimate

M

Motion Update

• For motion


μ’=μ+u=18

σ2’=σ2+r2=10

u, r2

Prior Position Estimate

M

Kalman Filter

Sense Move

Initial Belief

Gaussian:μ, σ2

μ’=μ+uσ2’=σ2+r2

Kalman Filter in Multiple Dimensions

2D Gaussian

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.

Documents

x steps

resulting belief example

robot senses yellow

outcome of actions pclosed

resulting belief ok

open popen pclosedu

position previous position

orange pixelsexplore