lab 4 1.get an image into a ros node 2.find all the orange pixels (suggest hsv) 3.identify the...
TRANSCRIPT
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
Example: The Resulting Belief
1615
83
11
85
109
)(),|(
)(),|(
)'()',|()|(
closedPcloseduclosedP
openPopenuclosedP
xPxuclosedPuclosedP
OK… but then what’s the chance that it’s still open?
Example: The Resulting Belief
)|(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
Nondeterministic Robot Motion
R
The robot can now move Left and Right.
.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
Nondeterministic Robot Motion
Right 2
The robot can now move Left and Right.
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
Nondeterministic Robot Motion
Right 2
The robot can now move Left and Right.
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
Nondeterministic Robot Motion
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
Measurement ExamplePrior Position Estimate Measurement Estimate
Where is the new mean ?
μ, σ2
v, r2
Measurement ExamplePrior Position Estimate Measurement Estimate
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
ExamplePrior Position Estimate Measurement Estimate
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
Model of motion noise
μ’=μ+u
σ2’=σ2+r2
u, r2
Prior Position Estimate
M
Motion Update
• For motion
Model of motion noise
μ’=μ+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