Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Technical Cognitive Systems
Michael Beetz
Intelligent Autonomous Systems
Technische Universität München
Summer Term 2012
Part IV
State Estimation
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Outline
Basic Concepts
Bayesian Filtering
Self Localization
State Representations
Sensor and Motion Models
State Estimation in RoboCup
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Where are we?
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Motivation
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Probabilistic Robotics
• Key idea: explicit representation of uncertainty
• Using the calculus of probability theory
• perception = state estimation
• Action = selecting actions with the highest expected utility
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Demonstrations of State Estimation
• Global localization
• Adaptive global localization
• Fast SLAM
• Distributed Mapping
• People Tracking
• Inferring Transportation Routines
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Outline
Basic Concepts
Bayesian Filtering
Self Localization
State Representations
Sensor and Motion Models
State Estimation in RoboCup
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
The axioms of probability theory
• P(A) denotes the probability that proposition A is true
• 0 <= P(A) <= 1
• P(True) = 1
P(False) = 0
• P(A or B) = P(A) + P(B) − P(A and B)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Discrete random variables
• X denotes a random variable
• X can take a countable number of values in {x1 , ..., xn }
• P(X = xi ), or P(xi ), is the probability that the random variable X has the
value xi
• P(Room) = h0.7, 0.2, 0.08, 0.02i
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Continuous random variables
• X takes on values in the continuum
• P(X = x) or P(x) is a probability density function
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Joint and conditional probability
• P(X = x ∧ Y = y ) = P(x, y )
• If X and Y are independent then P(x, y ) = P(x) ∗ P(y )
• P(x | y ) is the probability of x given y
• P(x | y ) = P(x, y )/P(y )
• P(x, y ) = P(x | y )P(y )
• If X and Y are independent then P(x | y ) = P(x)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Law of total probability, marginals
Discrete case
•
P
Continuous case
P(x) = 1
P
• P(x) = y P(x, y )
P
• P(x) = y P(x | y )P(y )
•
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
R
P(x) = 1
R
• P(x) = P(x, y )dy
R
• P(x) = P(x | y )P(y )dy
x
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Bayes formula
• P(x, y ) = P(x | y )P(y ) = P(y | x)P(x)
• P(x | y ) =
P(y |x)P(x)
P(y )
• Bayes rule and background knowledge:
P(x | y , z) =
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
P(y | x, z)P(x | z)
P(y | z
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
State
• state = all aspects of the environment that can have impact the future
• state may change over time such as the location of people
• state that changes will be called dynamic state
• contrast: stationary state
• notation:
– state x
– state at time t: xt
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Typical state variables
• robot pose: location and orientation relative to a global coordinate frame;
often called kinematic state
• configuration of the robot’s actuators such as the joints of robotic
manipulators. each degree of freedom in a robot arm is characterized by a
one-dimensional configuration
• robot velocities and velocities of its joints. Velocities are commonly referred
to as dynamic state
• location and features of surrounding objects in the environment
• location and velocites of moving objects and people
• huge number of other state variables: battery charge, ...
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Completeness of States
• a state xt will be complete if it is the best predictor of the future, in that
knowledge of past state measurements, or controls carry no additional
information that would help us to predict the future more accurately.
– the future may be stochastic but no variables prior to xt may influence the
stochastic evolution of future states, unless the dependence is mediated
through xt
– temporal processes that meet these conditions are known as Markov chains
• incomplete state for real robots state cannot be complete
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Environment Interaction
• two
–
–
• two
types of interactions
sensor measurements: measurements, observations, percept
control actions
kinds of data:
– measurement data provides information about the momentary state of the
environment
measurement data zt
zt1 :t2 = zt1 , zt1 +1 , zt1 +2 , . . . , zt2
– control data: change of state
• odometers are sensors that measure the revolution of a robot’s wheels. Even
though odometers are sensors we treat them as control data because they are
directly tight to the robots’ actions
control data: ut , ut1 :t2
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Probabilistic Generative Laws
• The evolution of state and measurements is governed by probabilistic laws.
In general, the state xt is generated probabilistically. Thus it makes sense
to specify the probaility distribution from which xt is generated:
p(xt | x0:t−1 , z1:t−1 , u1:t )
• if state x is complete: conditional independence
p(xt | x0:t−1 , z1:t−1 , u1:t ) = p(xt | xt−1 , ut )
• p(zt | x0:t , z1:t−1 , u1:t ) = p(zt | xt )
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Probabilistic State Estimation as a Bayes Net
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Belief Distributions
• belief = the robot’s internal knowledge about the state of the environment
• bel(xt ) = p(xt | z1:t , u1:t )
• bel(xt ) = p(xt | z1:t−1 , u1:t )
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Example
Situation
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Example
Measurement and state transition probabilities
• Characterization of the robot’s sensors:
– p(Zt = senseOpen | Xt = isOpen) = 0.6
– p(Zt = senseClosed | Xt = isOpen) = 0.4
– p(Zt = senseOpen | Xt = isClosed) = 0.2
– p(Zt = senseClosed | Xt = isClosed) = 0.8
• Characterization of the action pushOpen
– p(Xt = isOpen | Ut = push, Xt−1 = isOpen) = 1
– p(Xt = isClosed | Ut = push, Xt−1 = isOpen) = 0
– p(Xt = isOpen | Ut = push, Xt−1 = isClosed) = 0.8
– p(Xt = isClosed | Ut = push, Xt−1 = isClosed) = 0.2
• Characterization of the action doNothing
–
–
–
–
p(Xt
p(Xt
p(Xt
p(Xt
Basics
Michael Beetz
Summer Term 2012
= isOpen | Ut = doNothing , Xt−1 = isOpen) = 1
= isClosed | Ut = doNothing , Xt−1 = isOpen) = 0
= isOpen | Ut = doNothing , Xt−1 = isClosed) = 0
= isClosed | Ut = doNothing , Xt−1 = isClosed) = 1
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Example
The robot’s prior belief
• bel(X0 = open) = bel(X0 = closed) = 0.5
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Example
Belief after doNothing and senseOpen
• bel(x1 )
R
= Pp(x1 | u1 , x0 )bel(x0 )dx0
=
x0 p(x1 | u1 , x0 )bel(x0 )dx0
= p(x1 | U1 = doNothing , X0 = isOpen) ∗ bel(X0 = isOpen)
+p(x1 | U1 = doNothing , X0 = isClosed) ∗ bel(X0 = isClosed)
• bel(X1 = isOpen)
= p(X1 = isOpen | U1 = doNothing , X0 = isOpen) ∗ bel(X0 = isOpen)
+p(X1 = isOpen | U1 = doNothing , X0 = isClosed) ∗ bel(X0 = isClosed)
= 1 ∗ 0.5 + 0 ∗ 0.5 = 0.5
• bel(X1 = isClosed) = ... = 0.5
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Example
Belief bel(x1 )
• bel(x1 ) = ηp(Z1 = senseOpen | x1 )bel(x1 )
• bel(X1 = isOpen)
= ηp(Z1 = senseOpen | X1 = isOpen)bel(X1 = isOpen)
= η0.6 ∗ 0.5 = 0.3η
• bel(X1 = isClosed)
= ηp(Z1 = senseOpen | X1 = isClosed)bel(X1 = isClosed)
= η0.2 ∗ 0.5 = 0.1
• η = (0.3 + 0.1)−1 = 2.5
• bel(X1 = isOpen) = 0.75
• bel(X1 = isClosed) = 0.25
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Example
Belief bel(x1 ) for u2 = push and z2 = senseOpen
• bel(X2 = isOpen) = 1 ∗ 0.75 + 0.8 ∗ 0.25 = 0.95
• bel(X2 = isClosed) = 0 ∗ 0.75 + 0.2 ∗ 0.25 = 0.05
• bel(X2 = isOpen) = η0.6 ∗ 0.95 ≈ 0.983
• bel(X2 = isClosed) = η0.2 ∗ 0.05 ≈ 0.017
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Outline
Basic Concepts
Bayesian Filtering
Self Localization
State Representations
Sensor and Motion Models
State Estimation in RoboCup
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Bayesian Filter
• Why should you care?
– Robot and environmental state estimation is a fundamental problem!
• Many algorithms that exist for spatial reasoning make use of this approach
– If you’re working in mobile robotics, you’ll see it over and over!
– Very important to understand and appreciate
• Efficient state estimator
– Recursively compute the robots current state based on the previous state of
the robot
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Bayesian Filter
• Estimate state x from data d
– What is the probability of the robot being at x?
• x could be robot location, map information, locations of targets, etc
• d could be sensor readings such as range, actions, odometry from encoders,
etc.
• This is a general formalism that does not depend on the particular probability
representation
• Bayes filter recursively computes the posterior distribution:
Bel(Xt ) = P(Xt | Zt )
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Bayesian Filtering: two phases
• Two phases:
1. Prediction Phase
2. Measurement Phase
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
1. Prediction Phase
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
2. Measurement Phase
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Derivation of the Bayes filter (1)
• let dt have the form dt−1 , ut−1 , zt where dt−1
• applying Bayes rule with dt−1 and ut−1 as background knowledge
P(A | B, C ) =
P(B|A,C )∗P(A|C )
P(P|C )
P(xt | dt−1 , ut−1 , zt ) =
P(zt | dt−1 , ut−1 , xt )P(xt | dt−1 , ut−1 )
.
P(zt | dt−1 , ut−1 )
(1)
• applying the Markov assumption:
P(zt | dt−1 , ut−1 , xt ) = P(zt | xt )
P(xt | dt−1 , ut−1 , zt ) =
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
P(zt | xt )P(xt | dt−1 , ut−1 )
.
P(zt | dt−1 , ut−1 )
Self Localization
State
Models
(2)
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Derivation of the Bayes filter (2)
• the P(zt | dt−1 , ut−1 ) does not depend on the robot’s pose
replace it by a normalization constant αt :
P(xt | dt−1 , ut−1 , zt )
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
=
αt P(zt | xt )P(xt | dt−1 , ut−1 ).
Self Localization
State
Models
(3)
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Derivation of the Bayes filter (3)
• consider P(xt | dt−1 , ut−1 )
If we use the law of total probability we derive
P(xt | dt−1 , ut−1 ) =
X
P(xt | dt−1 , ut−1 , xt−1 )P(xt−1 | dt−1 , ut−1 ).
(4)
xt−1 ∈X
• expand P(xt | dt−1 , ut−1 )
P(xt | dt−1 , ut−1 , xt−1 ) = P(xt | u0 , z1 , . . . , zt−1 , ut−1 , xt−1 )
(5)
• apply Markov assumption:
P(xt | dt−1 , ut−1 , xt−1 ) = P(xt | ut−1 , xt−1 )
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
(6)
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Derivation of the Bayes filter (4)
• simplify the right-hand side of Equation 4
P(xt | dt−1 , ut−1 ) =
X
P(xt | ut−1 , xt−1 )P(xt−1 | dt−1 , ut−1 ).
(7)
xt−1 ∈X
• ut−1 causes the transition from xt−1 to xt
thus, xt−1 is independent of ut−1
P(xt−1 | dt−1 , ut−1 ) = P(xt−1 | dt−1 )
(8)
• Substituting Equation 8 into Equation 7 leads to
P(xt | dt−1 , ut−1 ) =
X
P(xt | ut−1 , xt−1 )P(xt−1 | dt−1 )
(9)
xt−1 ∈X
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Derivation of the Bayes filter (5)
If we now substitute this equation into Equation 3 we obtain
X
0
0
P(xt | dt ) = α P(zt | xt )
P(xt | ut−1 , xt−1
) P(xt−1
| dt−1 ).
(10)
0
xt−1
∈X
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Outline
Basic Concepts
Bayesian Filtering
Self Localization
State Representations
Sensor and Motion Models
State Estimation in RoboCup
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Localization: the problem
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Localization: the basic problem
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Demonstrations of Self Localization
• Global localization
• Adaptive global localization
• Kidnapped robot problem
• Localization with adaptive sample size
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Probabilistic Localization through recursive filtering
• x = state, ie pose given as its position and orientation
• P(xt ) = robot’s probabilistic belief about its pose xt at time t
• zt = sensor measurements
• ut = odometry measurements
• dt = dt = u0 , z1 , . . . , ut−1 , zt
• P(xt | dt ) = posterior probability: conditional probability over all possible
poses given all information gathered so far.
P(xt | dt )
=
=
P(xt | u0 , z1 , . . . , ut−1 , zt )
X
0
0
α P(zt | xt )
P(xt | ut−1 , xt−1 ) P(xt−1 | u0 , z1 , . . . , ut−2 , zt−1 )
x0
∈X
t−1
=
α P(zt | xt )
X
0
0
P(xt | ut−1 , xt−1 ) P(xt−1 | dt−1 )
(11)
x0
∈X
t−1
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Probabilistic Localization through recursive filtering (2)
P(xt | dt )
=
=
P(xt | u0 , z1 , . . . , ut−1 , zt )
X
0
0
α P(zt | xt )
P(xt | ut−1 , xt−1 ) P(xt−1 | u0 , z1 , . . . , ut−2 , zt−1 )
x0
∈X
t−1
=
X
α P(zt | xt )
0
0
P(xt | ut−1 , xt−1 ) P(xt−1 | dt−1 )
(12)
x0
∈X
t−1
• P(xt | dt ) is the posterior
• P(zt | xt ) = observation model which specifies the likelihood of the
measurement zt given the robot is at location xt
0
• P(xt | ut−1 , xt−1
) = motion model aka transition probability. likelihood
0
that the movement action ut−1 carried out at location xt−1
carries the
robot to the location xt
0
• P(xt−1
| dt−1 ) = prior as it quantifies the probability that the robot is at
0
location xt−1
before the integration of zt and ut−1
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Outline
Basic Concepts
Bayesian Filtering
Self Localization
State Representations
Sensor and Motion Models
State Estimation in RoboCup
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Open points
1. how the robot’s belief P(x) is represented and
2. how the conditional probabilities
0
– P(xt | ut−1 , xt−1
) and
– P(zt | xt ) are computed.
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
State Space Representations
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Kalman Filter: Expected Value & Variance of the State
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Kalman Filter: The Gaussian Distribution
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Kalman Filter: What is Covariance?
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Kalman Filter: Example
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Kalman Filter: Algorithm
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Discrete Approximations
• discretize the enviroment into a grid of
cells
typical size: 10-20cm (indoor)
• the probability distribution for the
robot’s position is represented by a set
of random variables cx,y
representing the event that the robot is
located in the grid cell cx,y
• P(cx,y ) then is the belief of the robot
being in the grid cell cx,y
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Probabilistic localization with discrete state space
for each state x do
initialize P(x)
forever do
if new sensory input zt is received do
αt = 0
for each state x do
P(x) = P(zt | x) · P(x)
αt = αt + P(x)
for each state x do
P(x) = αt −1 · P(x)
if an odometry reading ut is received do
for each state
P x do
P 0 (x) = x 0 ∈X P(x | x 0 , ut ) · P(x)
P(x) = P 0 (x)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
/* Initialization */
/* apply the perception model */
/* normalize the belief */
/* apply the motion model */
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Occupancy grid map and data set
C
22m
3m
A
B
31m
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
20m
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Density plots ...
after incorporating 5, 18, and 24 sonar scans (darker positions are more likely)
Robot position (A)
Robot position (B)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Particle Filter
• key idea:
– represent the posterior be a set M of N samples
• sample m = hx, ωi
– x = state vector
– ω = weighing factor
ω used to store the importance of the corresponding particle
• posterior = distribution of samples and their importance factors.
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Particle Filter: 1. Prediction Phase
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Particle Filter: 2. Measurement Phase
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Particle Filter: 3. Resampling
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Particle Localization: the example revisited
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Particle Filter-based Localization (1)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Particle Filter-based Localization (2)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Particle Filter-based Localization (3)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Particle Filter-based Localization (4)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Sequential importance sampling algorithm
Initialize M according to Bel(X0 = x)
forever do
if new sensory input zt is received do
α=0
for each sample hxi , ωi i ∈ M do
wi = P(zt | xi )
α = α + wi
for each sample hxi , ωi i ∈ M do
wi = α−1 · wi
M = resample(M)
/* apply the perception model */
/* normalize the weights */
if an odometry reading ut is received do
for each sample hxi , ωi i ∈ M do
/* apply the motion model */
compute a new state x 0 by sampling from P(x 0 | xi , ut ).
xi = x 0
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Resampling Step
Let M consist of N samples hxi , ωi i and M 0 consist of N samples hxi0 , ωi0 i
∆ = rand(]0; N −1 ])
/* draw random number from ]0; N −1 ] */
c = ω0
i =0
for j = 0 to N − 1 do
u = ∆ + j · N −1
while u > c do
i =i +1
c = c + ωi
hxj0 , ωj0 i = hxi , ωi i
return M 0
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Outline
Basic Concepts
Bayesian Filtering
Self Localization
State Representations
Sensor and Motion Models
State Estimation in RoboCup
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Sensor and Motion Models
Sensor Models
Motion Models
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Motion Models
Parameters α, β, and d specifying any
incremental motion of a robot in the
hx, y , θi-space.
measured pose
β’
Sample-based approximation of the
belief of the robot after a repeatedly
executing motion commands.
β
d’
initial pose
d
α
final pose
α’
path
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Sensor Models
Ultrasound scan perceived with a B21
robot in an office environment
22
Laser scan perceived in a populated
museum
23
21
0
20
1
19
18
17
16
15
14
Basics
Michael Beetz
Summer Term 2012
13
12 11 10
2
3
4
5
6
7
8
9
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Beam-based Sensor Models
• A scan consists of k measurements
Z = {z1 , ..., zk }
• Individual measurements are independent given the robot position
K
X
P(z | x, m) =
P(zk | x, m)
k=1
22
23
21
0
20
1
19
18
17
16
15
14
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
13
12 11 10
Self Localization
2
3
4
5
6
7
8
9
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Uncertainty in proximity measurements
• Measurements can be caused by:
– A known obstacle
– Cross talk
– An unexpected obstacle (persons, chairs, ...)
– Missing all obstacles (reflection, glass, ...)
• Noise is due to uncertainty
– In measuring distance to known obstacle
– In position of known obstacle
– In position of additional obstacles
– Whether obstacle is missed
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Components of the beam-based sensor models
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Mixture model for beam-based sensors
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Sensor Models: from Data sets
Sonar (distance 1)
0.2
Laser (distance 1)
0.2
Data
Approximation
0.15
0.15
0.1
0.1
0.05
0.05
0
0
Sonar (distance 2)
0.2
Laser (distance 2)
0.2
Data
Approximation
0.15
0.15
0.1
0.1
0.05
0.05
0
0
Basics
Michael Beetz
Summer Term 2012
Data
Approximation
Bayesian Filtering
Self Localization
State
Data
Approximation
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Approximation results
Measured and approximated probability of sonar (a,b) and laser (c,d) measurements:
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Outline
Basic Concepts
Bayesian Filtering
Self Localization
State Representations
Sensor and Motion Models
State Estimation in RoboCup
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Vision-based Cooperative State Estimation
in Robot Soccer
joint work with: Thorsten Schmitt and Robert Hanek
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
AAAI 2002, AAMAS 2002
Transactions
of
Robotics
andModels
Automation RoboCup
2003
Self Localization
State
Autonomous Robots 2004
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Game State Estimation in Robot Soccer
Computational Problem:
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Game State Estimation in Robot Soccer
Computational Problem:
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Game State Estimation in Robot Soccer
Computational Problem:
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Game State Estimation in Robot Soccer
Computational Problem:
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
State Estimation in Dynamical Systems
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
State Estimation in Dynamical Systems
• Environment State:
Robot 1 , ..., Robot 4 , Ball, Opponent 1 , . . . , Opponent n
with: Robot i = hx i , y i , θi , ẋ i , ẏ i , θ̇i i
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
State Estimation in Dynamical Systems
• Environment State:
Robot 1 , ..., Robot 4 , Ball, Opponent 1 , . . . , Opponent n
with: Robot i = hx i , y i , θi , ẋ i , ẏ i , θ̇i i
• Belief State:
Belt (st ) = p(st |dt , ..., d0 ; m)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
The AGILO State Estimation Module (Version 1)
Perception Module: VISBOT 1
Sensor−data Processing
Own Sensor−data
Odometry
State Estimators of Teammates
Camera Image
Ball State
Ball State
Ball State
Ball State
Environment
Feature
Detector
Ball
Feature
Detector
Opponents
Feature
Detector
Environment
Features
Ball
Features
Opponents
Features
Ball State
Opponents State
Opponents State
Opponents State
Opponents State
Team Mate State
Team Mate State
Team
TeamMate
MateState
State
Opponents State
State Estimators
Self
Localization
(incremental)
Ball
Localization
(incremental)
Opponents
Localization
(MHT)
Belief State
Own State
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Ball State
Self Localization
Opponents State
Opponents State
Opponents State
Opponents State
State
Team Mate State
Team Mate State
Team Mate State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
The AGILO State Estimation Module (Version 2)
Perception Module: VISBOT 2
Sensor−data Processing
Own Sensor−data
Odometry
State Estimators of Teammates
Camera Image
Environment
Feature
Detector
CCD
Tracker
Environment
Features
Ball State
Ball State
Ball State
Ball State
Opponents State
Opponents State
Opponents State
Opponents State
Team Mate State
Team Mate State
Team Mate State
Team Mate State
Opponents
Feature
Detector
Opponents
Features
Opponents State
Ball State
State Estimators
Self Localization
incremental
Monitor
Kalman−
filter−
based
Particle−
filter−
based
Ball Localization
active
Self−
localization
increm.
Monitor
Kalman−
filter−
based
Particle−
filter−
based
active
Ball−
local.
Opponents
Localization
learned
Observation
filtering
Situation
Recognizer
learned
Perception−
model
Belief State
Own State
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Ball State
Opponents State
Opponents State
Opponents State
Opponents State
Self Localization
Situation Class
State
Team Mate State
Team Mate State
Team Mate State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Color Clustering
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Color Segmentation
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
AGILO Field Model
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Model Projection and Matching
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting in Images:
The Contracting Curve Density Algorithm
joint work with: Robert Hanek
IJCV 2004
RoboCup Engineering Challenge Award
Best Paper DAGM 2001
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Image Segmentation
edge-based
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
region-based
Self Localization
graph-based
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Image Segmentation
edge-based
region-based
graph-based
Our Approach:
Acquire, use, and update models of the region and the background
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: the computational problem
• Given:
image
blurred curve model
• Compute:
Basics
argmaxΦ p(I | Φ)p(Φ)
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: the algorithm
∗
Input: image data I ∗ , curve function c, mean mΦ
and covariance Σ∗
Φ
Output: estimate mΦ of model parameters and covariance ΣΦ
∗
Initialization: mean mΦ = mΦ
, covariance ΣΦ = c1 · Σ∗
Φ
repeat
1. learn local statistics of image data from the vicinity of the curve
1.1 determine pixels v in vicinity V of curve from c, mΦ and ΣΦ
∀v ∈ V compute probabilistic assignment av (mΦ , ΣΦ )
1.2 ∀v ∈ V compute local statistics Sv of image data I ∗ V
which characterize the two sides of the curve
2. refine estimate of model parameter vector
2.1 update mean mΦ through MAP estimation:
2
mΦ = arg min χ (mΦ )
mΦ
2.2 updated covariance ΣΦ from the Hessian of χ2 (mΦ )
until changes of mΦ and ΣΦ are small enough
return mean mΦ and covariance ΣΦ
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Animation (1)
CCD Algorithm
repeat
1. learn local statistics
1.1 determine vicinity
1.2 compute local statistics
2. refine parameter estimate
2.1 update mean
2.2 update covariance
until accurate enough
return mean and covariance
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Animation (1)
CCD Algorithm
repeat
1. learn local statistics
1.1 determine vicinity
1.2 compute local statistics
2. refine parameter estimate
2.1 update mean
2.2 update covariance
until accurate enough
return mean and covariance
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Animation (2)
CCD Algorithm
repeat
1. learn local statistics
1.1 determine vicinity
1.2 compute local statistics
2. refine parameter estimate
2.1 update mean
2.2 update covariance
until accurate enough
return mean and covariance
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Animation (3)
CCD Algorithm
repeat
1. learn local statistics
1.1 determine vicinity
1.2 compute local statistics
2. refine parameter estimate
2.1 update mean
2.2 update covariance
until accurate enough
return mean and covariance
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Animation (4)
CCD Algorithm
repeat
1. learn local statistics
1.1 determine vicinity
1.2 compute local statistics
2. refine parameter estimate
2.1 update mean
2.2 update covariance
until accurate enough
return mean and covariance
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Animation (5)
CCD Algorithm
repeat
1. learn local statistics
1.1 determine vicinity
1.2 compute local statistics
2. refine parameter estimate
2.1 update mean
2.2 update covariance
until accurate enough
return mean and covariance
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Animation (6)
CCD Algorithm
repeat
1. learn local statistics
1.1 determine vicinity
1.2 compute local statistics
2. refine parameter estimate
2.1 update mean
2.2 update covariance
until accurate enough
return mean and covariance
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Animation (7)
CCD Algorithm
repeat
1. learn local statistics
1.1 determine vicinity
1.2 compute local statistics
2. refine parameter estimate
2.1 update mean
2.2 update covariance
until accurate enough
return mean and covariance
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Animation (8)
CCD Algorithm
repeat
1. learn local statistics
1.1 determine vicinity
1.2 compute local statistics
2. refine parameter estimate
2.1 update mean
2.2 update covariance
until accurate enough
return mean and covariance
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Curve Fitting: Local Statistics
0
2
a.) Input image with superimposed curve
5
iteration:
b.) Expected image according to the local statistics
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
CCD Tracking: Results and Next Steps
Results:
•
CCD tracking
•
Condensation
tracking
Basics
Bayesian Filtering
Michael Beetz
Summer Term 2012
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
CCD Tracking: Results and Next Steps
Next Steps:
• CCD-based tracking of human motions
• Recognizing complex activities
• Application:
Results:
•
CCD tracking
•
Condensation
tracking
Basics
Bayesian Filtering
Michael Beetz
Summer Term 2012
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Opponent Tracking
algorithm Bayes Filter (Bel(xt−1 ), data)
1 switch (data)
2
case (data is a control
signal vector u) :
R
3
Bel(xt ) = p(xt |xt−1 , ut−1 ) Bel(xt−1 ) d xt−1
4
case (data is a sensor data vector z) :
5
Bel(xt ) = ν p(zt |xt ) Bel(xt−1 )
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Opponent Tracking
algorithm Bayes Filter (Bel(xt−1 ), data)
1 switch (data)
2
case (data is a control
signal vector u) :
R
3
Bel(xt ) = p(xt |xt−1 , ut−1 ) Bel(xt−1 ) d xt−1
4
case (data is a sensor data vector z) :
5
Bel(xt ) = ν p(zt |xt ) Bel(xt−1 )
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Opponent Tracking
algorithm Bayes Filter (Bel(xt−1 ), data)
1 switch (data)
2
case (data is a control
signal vector u) :
R
3
Bel(xt ) = p(xt |xt−1 , ut−1 ) Bel(xt−1 ) d xt−1
4
case (data is a sensor data vector z) :
5
Bel(xt ) = ν p(zt |xt ) Bel(xt−1 )
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Opponent Tracking
algorithm Bayes Filter (Bel(xt−1 ), data)
1 switch (data)
2
case (data is a control
signal vector u) :
R
3
Bel(xt ) = p(xt |xt−1 , ut−1 ) Bel(xt−1 ) d xt−1
4
case (data is a sensor data vector z) :
5
Bel(xt ) = ν p(zt |xt ) Bel(xt−1 )
•
represent Bel(xt ) as a set of weighted hypotheses:
S
Ht = ni=1 {hti , pti }
•
Belief is a P
mixture of Gaussians
n
i
i
Bel(xt ) =
i=1 pt ∗ N(xt ; h̄t , Σhi )
•
compute pti from P(hti |Zt...0 )
t
– the form of P(hti |Zt...0 ) is highly task specific with
parameters:
• termination likelihood λT = 2s
• false observations λF = 0.0002
• new tracks λN = 0.04
• detection probability PD = 0.9
• ...
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
The Multi Hypotheses Tracking Algorithm
for Opponent Localization
• Hypotheses are organized in trees
• Every node represents a hypothesis
– (mean + covariance + probability)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
The Multi Hypotheses Tracking Algorithm
for Opponent Localization
• Hypotheses are organized in trees
• Every node represents a hypothesis
– (mean + covariance + probability)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
The Multi Hypotheses Tracking Algorithm
for Opponent Localization
• Hypotheses are organized in trees
• Every node represents a hypothesis
– (mean + covariance + probability)
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
The Multi Hypotheses Tracking Algorithm
for Opponent Localization
algorithm MultipleHypothesisTracking()
1 for k ← 1 to ∞
2 do Z (k) ← InterpretSensorData();
e k ← ApplyMotionModel(H k−1 , M);
3
H
4
for i ← 1 to nk
5
do for j ← 1 to mk
6
do hijk ← Associate(e
hjk , zi (k));
k
7
Compute(P(hij |Z (k)));
8
for j ← 1 to nk
9
do H k ← H k ∪ {GenNewHyp(zj (k))};
10
PruneHypotheses(H k−N );
11
X k−N ← {h1k−N , . . . , hok−N
};
k−N
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Assessing Hypotheses
P(hik |Z k )
=
P(θ(k), hik−1 |Z (k), Z k−1 )
(13)
=
P(θ(k), hik−1 |Z (k), H k )
α ∗ p(Z (k)|θ(k), hjk−1 , Z k−1 )
P(θ(k)|hjk−1 , Z k−1 )P(hjk−1 |Z k−1 )
(14)
=
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
(15)
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Learning Perception Models
• Classes: use or discard
• Training examples from off-line
observation
MHT run with tree depth N =
20
• Learned rules:
• Feature Language:
F = { R,T,V,W,A,D,U }
R
T
=
=
V
W
A
D
U
=
=
=
=
=
Basics
Michael Beetz
Summer Term 2012
observing Robot
observation type
(unknown, opponent)
translational velocity
rotational velocity
observational angle
distance
area of uncertainty
Bayesian Filtering
Self Localization
– if W < 41
then use observation
– if W > 41 ∧ D > 2.4 then
use observation
– if W > 41 ∧ D < 2.4 then
discard observation
• other features irrelevant
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
More Informative Perception Models
• belief state:
S
S
Ht = ni=1 {hti , pti } = ni=1 {hh̄it , Σhit i, pti }
with hti ∼ N(h̄it , Σhit )
• perception model:
a hypothesis h is in Ht if
– h corresponds to a robot and has not
been pruned
– h resulted from a hallucination
estimate expected accuracy
• simulator
• conditional probability distributions in
estimation algorithm
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
State Estimation
What?
How?
Input
Output
Why?
• cooperative
Results
• vision-based
estimated game state
state estimation
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
State Estimation
algorithm SelfLocalization()
eΦ ),
bΦ ),
e C
b C
(Φ,
(Φl , ClΦ ),
projections,
corr
1 let (Φ,
2 do
3
for all data
eΦ ) ← ApplyMotionModel(Φl , Cl ); (Φ,
bΦ ) ← (Φ,
eΦ );
e C
b C
e C
4
do (Φ,
Φ
5
loop
6
switch (data)
7
case (data is an image) :
b
8
projections ← ProjectCurvePoints(Φ);
9
corr ← SearchCorresp(projections, data);
10
χ2i ← QualityOfFitImage(corr , Φ);
11
case (data is odometric data) :
12
χ2o ← QualityOfFitOdom(data, Φ);
13
case (data is ball observation from team-mate) :
14
χ2t ← QualityOfFitTeam(data, Φ);
e2 ;
15
χ2 ← χ2i + χ
b
16
Φ ← arg minΦ χ2 ;
% one Newton step.
b is small or data is not an image)
17
until (change of Φ
bΦ = 2/ Hessian of χ2 ;
18
C
bΦ );
b C
19
(Φl , ClΦ ) ← (Φ,
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
Detecting Opponents
b φ)
b C
algorithm InterpretSensorData(Φ,
1 I ← GetSensorData();
2 R ← ExtractBlackRegions(I );
3 R ← CheckConstraints(R);
4 R ← ExtractCascadedRobots(R);
5 for i ← 1 to |R|
6 do (row , col, width) ← ExtractFeatures(ri );
h
iT
b row , col, width ;
7
ω ← Φ,


bφ
C
0
0
0
 0 σrow
0
0 
;
8
Cω ← 
 0
0
σcol
0 
0
0
0
σwidth
9
zi (k) ← UnscentedTransform(ω, Cω , opp);
10 return (Z (k));
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems
Department of Informatics
Intelligent Autonomous Systems
Technische Universität München
MHT Algorithm
algorithm MultipleHypothesisTracking()
1 for k ← 1 to ∞
2 do Z (k) ← InterpretSensorData();
e k ← ApplyMotionModel(H k−1 , M);
3
H
4
for i ← 1 to nk
5
do for j ← 1 to mk
6
do hijk ← Associate(e
hjk , zi (k));
7
Compute(P(hijk |Z (k)));
8
for j ← 1 to nk
9
do H k ← H k ∪ {GenerateNewHyp(zj (k))};
10
PruneHypotheses(H k−N );
11
X k−N ← {h1k−N , . . . , hok−N
};
k−N
Basics
Michael Beetz
Summer Term 2012
Bayesian Filtering
Self Localization
State
Models
RoboCup
Technical Cognitive Systems