Mobile Robot

Localization and Mapping

using the Kalman Filter

15-491 : CMRoboBits: Creating an

Intelligent AIBO Robot

Paul E. Rybski
Mobile Robot Localization
n Where am I?
n Given a map, determine the robot’s location
n Landmark locations are known, but the robot’s
position is not
n From sensor readings, the robot must be able to
infer its most likely position on the field
n Example : where are the AIBOs on the soccer
field?
Mobile Robot Mapping
n What does the world look like?
n Robot is unaware of its environment
n The robot must explore the world and
determine its structure
n Most often, this is combined with localization
n Robot must update its location wrt the landmarks
n Known in the literature as Simultaneous
Localization and Mapping, or Concurrent
Localization and Mapping : SLAM (CLM)
n Example : AIBOs are placed in an unknown
environment and must learn the locations of the
landmarks (An interesting project idea?)
Bayesian Filter
n Why should you care?
n Robot and environmental state estimation is a
fundamental problem!
n Nearly all algorithms that exist for spatial
reasoning make use of this approach
n If you’re working in mobile robotics, you’ll see it
over and over!
n Very important to understand and appreciate
n Efficient state estimator
n Recursively compute the robot’s current state
based on the previous state of the robot
What is the robot’s state?
n
Bayesian Filter
n Estimate state x from data d
n What is the probability of the robot being at x?
n x could be robot location, map information,
locations of targets, etc…
n d could be sensor readings such as range,
actions, odometry from encoders, etc…)
n This is a general formalism that does not
depend on the particular probability
representation
n Bayes filter recursively computes the
posterior distribution: Bel( xT ) = P( xT | ZT )
What is a Posterior
Distribution?
Derivation of the Bayesian
Filter (slightly different notation from before)
Estimation of the robot’s state given the data:

Bel( xt ) = p( xt | ZT )
The robot’s data, Z, is expanded into two types:
observations oi and actions ai

Bel( xt ) = p( xt | ot , at -1 , ot -1 , at -2 ,..., o0 )
Invoking the Bayesian theorem
p(ot | xt , at -1 ,..., o0 ) p( xt | at -1 ,..., o0 )
Bel ( xt ) =
p(ot | at -1 ,..., o0 )
 Derivation of the Bayesian
Filter
Denominator is constant relative to xt
h = p(ot | at -1 ,..., a0 )

Bel( xt ) = hp(ot | xt , at -1 ,..., o0 ) p( xt | at -1 ,..., o0 )

First-order Markov assumption shortens first term:

Bel( xt ) = hp(ot | xt ) p( xt | at -1 ,..., o0 )

Expanding the last term (theorem of total probability):

Bel( xt ) = hp(ot | xt )ò p( xt | xt -1 , at -1 ,..., o0 ) p( xt -1 | at -1 ,..., o0 )dxt -1

 Derivation of the Bayesian
Filter
First-order Markov assumption shortens middle term:
Bel( xt ) = hp(ot | xt )ò p( xt | xt -1 , at -1 ) p( xt -1 | at -1 ,..., o0 )dxt -1
Finally, substituting the definition of Bel(xt-1):

Bel( xt ) = hp(ot | xt )ò p( xt | xt -1 , at -1 )Bel( xt -1 )dxt -1

The above is the probability distribution that must

be estimated from the robot’s data
Iterating the Bayesian Filter
n Propagate the motion model:

Bel- ( xt ) = ò P( xt | at -1 , xt -1 ) Bel( xt -1 )dxt -1

Compute the current state estimate before taking a sensor reading
by integrating over all possible previous state estimates and
applying the motion model

n Update the sensor model:

Bel( xt ) = hP(ot | xt ) Bel- ( xt )
Compute the current state estimate by taking a sensor reading
and multiplying by the current estimate based on the most recent
motion history
 Localization
Initial state
detects nothing:

Moves and
detects landmark:

Moves and
detects nothing:

Moves and
detects landmark:
Bayesian Filter : Requirements
for Implementation
n Representation for the belief function
n Update equations
n Motion model
n Sensor model
n Initial belief state
 Representation of the Belief
Function
Sample-based Parametric
representations representations
e.g. Particle filters

( x1 , y1 ), ( x2 , y2 ), ( x3 , y3 ),...( xn , yn ) y = mx + b
 Example of a Parameterized
Bayesian Filter : Kalman Filter
Kalman filters (KF) represent posterior belief by a
Gaussian (normal) distribution

A 1-d Gaussian An n-d Gaussian

distribution is given by: distribution is given by:
-( x-µ )2 1
1 1 - ( x - µ )T S -1 ( x - µ )
P( x) = e 2s 2 P( x) = e 2

s 2p (2p ) | S |
n
Kalman Filter : a Bayesian
Filter
n Initial belief Bel(x0) is a Gaussian distribution
n What do we do for an unknown starting position?
n State at time t+1 is a linear function of state at time t:

xt +1 = Fxt + But + e t ( action )

n Observations are linear in the state:

ot = Hxt + e t ( observation )
n Error terms are zero-mean random variables which are normally
distributed
n These assumptions guarantee that the posterior belief is Gaussian
n The Kalman Filter is an efficient algorithm to compute the posterior
n Normally, an update of this nature would require a matrix inversion (similar
to a least squares estimator)
n The Kalman Filter avoids this computationally complex operation
The Kalman Filter
n Motion model is Gaussian…
n Sensor model is Gaussian…
n Each belief function is uniquely characterized
by its mean µ and covariance matrix S
n Computing the posterior means computing a
new mean µ and covariance S from old data
using actions and sensor readings
n What are the key limitations?
1) Unimodal distribution
2) Linear assumptions
What we know…
What we don’t know…
n We know what the control inputs of our process are
n We know what we’ve told the system to do and have a model for
what the expected output should be if everything works right
n We don’t know what the noise in the system truly is
n We can only estimate what the noise might be and try to put some
sort of upper bound on it
n When estimating the state of a system, we try to find
a set of values that comes as close to the truth as
possible
n There will always be some mismatch between our estimate of the
system and the true state of the system itself. We just try to figure
out how much mismatch there is and try to get the best estimate
possible
 Minimum Mean Square Error
Reminder: the expected value, or mean value, of a
Continuous random variable x is defined as:
¥
E[ x] = ò xp ( x)dx
-¥

Minimum Mean Square Error

What is the mean of this distribution? P( x | Z )
This is difficult to obtain exactly. With our approximations,
we can get the estimate x̂
…such that E[( x - xˆ ) 2 | Z t ] is minimized.
According to the Fundamental Theorem of Estimation Theory
this estimate is: ¥
MMSE
xˆ = E[ x | Z ] = ò xp ( x | Z )dx
-¥
Fundamental Theorem of
Estimation Theory
n The minimum mean square error estimator equals the expected
(mean) value of x conditioned on the observations Z
n The minimum mean square error term is quadratic:

E[( x - xˆ ) 2 | Z t ]
n Its minimum can be found by taking the derivative of the function w.r.t x
and setting that value to 0.

Ñ x ( E[( x - xˆ ) 2 | Z ]) = 0
n It is interesting to note that when they use the Gaussian assumption,
Maximum Posteriori estimators and MMSE estimators find the same
value for the parameters.
n This is because mean and the mode of a Gaussian distribution are the
same.
Kalman Filter Components
(also known as: Way Too Many Variables…)

Linear discrete time dynamic system (motion model)

State Control input Process noise

xt +1 = Ft xt + Bt ut + Gt wt

State transition Control input Noise input

function function function with covariance Q

Measurement equation (sensor model)

Sensor reading State Sensor noise with covariance R

zt +1 = H t +1 xt +1 + nt +1
Sensor function Note:Write these down!!!
Computing the MMSE Estimate
of the State and Covariance
Given a set of measurements: Z t +1 = {zi , i £ t + 1}
According to the Fundamental Theorem of Estimation, the state
and covariance will be: MMSE
xˆ = E[ x | Z t +1 ]
P MMSE = E[( x - xˆ ) 2 | Z t +1 ]

We will now use the following notation:

xˆt +1|t +1 = E[ xt +1 | Z t +1 ]
xˆt|t = E[ xt | Z t ]
xˆt +1|t = E[ xt +1 | Z t ]
 Computing the MMSE Estimate
of the State and Covariance
What is the minimum mean square error estimate
of the system state and covariance?
xˆt +1|t = Ft xˆt|t + Bt ut Estimate of the state variables

zˆt +1|t = H t +1 xˆt +1|t Estimate of the sensor reading

T T
Pt +1|t = Ft Pt|t Ft + Gt Qt Gt Covariance matrix for the state
T
St +1|t = H t +1Pt +1|t H t +1 + Rt +1 Covariance matrix for the sensors
 At last! The Kalman Filter…
Propagation (motion model):

xˆt +1/ t = Ft xˆt / t + Bt ut

T T
Pt +1/ t = Ft Pt / t Ft + Gt Qt Gt

Update (sensor model):

zˆt +1 = H t +1 xˆt +1/ t
rt +1 = zt +1 - zˆt +1
T
St +1 = H t +1 Pt +1/ t H t +1 + Rt +1
T -1
K t +1 = Pt +1/ t H t +1 St +1
xˆt +1/ t +1 = xˆt +1/ t + K t +1rt +1
T -1
Pt +1/ t +1 = Pt +1/ t - Pt +1/ t H t +1 St +1 H t +1 Pt +1/ t
 …but what does that mean in
English?!?
Propagation (motion model):

xˆt +1/ t = Ft xˆt / t + Bt ut - State estimate is updated from system dynamics

T T
Pt +1/ t = Ft Pt / t Ft + Gt Qt Gt - Uncertainty estimate GROWS

Update (sensor model):

zˆt +1 = H t +1 xˆt +1/ t - Compute expected value of sensor reading

rt +1 = zt +1 - zˆt +1 - Compute the difference between expected and “true”

T - Compute covariance of sensor reading
St +1 = H t +1 Pt +1/ t H t +1 + Rt +1
T -1
K t +1 = Pt +1/ t H t +1 St +1 - Compute the Kalman Gain (how much to correct est.)

xˆt +1/ t +1 = xˆt +1/ t + K t +1rt +1 - Multiply residual times gain to correct state estimate
T -1
Pt +1/ t +1 = Pt +1/ t - Pt +1/ t H t +1 St +1 H t +1 Pt +1/ t - Uncertainty estimate SHRINKS
Kalman Filter Block Diagram
 Example 1: Simple 1D Linear
System
Given: F=G=H=1, u=0
Initial state estimate = 0
Linear system: x = x + w Unknown noise
t +1 t t
parameters
zt +1 = xt +1 + nt +1
Propagation: Update:
xˆt +1/ t = xˆt / t zˆt +1 = xˆt +1/ t
Pt +1/ t = Pt / t + Qt rt +1 = zt +1 - xˆt +1/ t
S t +1 = Pt +1/ t + Rt +1
-1
K t +1 = Pt +1/ t S t +1
xˆt +1/ t +1 = xˆt +1/ t + K t +1rt +1
-1
Pt +1/ t +1 = Pt +1 - Pt +1/ t S t +1 Pt +1/ t
State Estimate
State Estimation Error vs 3s Region of Confidence
Sensor Residual vs 3s Region of Confidence
Kalman Gain and State Covariance
 Example 2: Simple 1D Linear
System with Erroneous Start
Given: F=G=H=1, u=cos(t/5)
Initial state estimate = 20
xt +1 = xt + cos(t / 5) + wt
Linear system: Unknown noise
zt +1 = xt +1 + nt +1 parameters
Propagation: Update: (no change)
xˆt +1/ t = xˆt / t + cos(t / 5) zˆt +1 = xˆt +1/ t
Pt +1/ t = Pt / t + Qt rt +1 = zt +1 - xˆt +1/ t
S t +1 = Pt +1/ t + Rt +1
-1
K t +1 = Pt +1/ t S t +1
xˆt +1/ t +1 = xˆt +1/ t + K t +1rt +1
-1
Pt +1/ t +1 = Pt +1 - Pt +1/ t S t +1 Pt +1/ t
State Estimate
State Estimation Error vs 3s Region of Confidence
Sensor Residual vs 3s Region of Confidence
Kalman Gain and State Covariance
Some observations
n The larger the error, the smaller the effect on the
final state estimate
n If process uncertainty is larger, sensor updates will dominate
state estimate
n If sensor uncertainty is larger, process propagation will
dominate state estimate
n Improper estimates of the state and/or sensor
covariance may result in a rapidly diverging estimator
n As a rule of thumb, the residuals must always be bounded
within a ±3s region of uncertainty
n This measures the “health” of the filter
n Many propagation cycles can happen between
updates
Using the Kalman Filter for
Mobile Robots
n Sensor modeling
n The odometry estimate is not a reflection of the robot’s
control system is rather treated as a sensor
n Instead of directly measuring the error in the state vector
(such as when doing tracking), the error in the state must
be estimated
n This is referred to as the Indirect Kalman Filter
n State vector for robot moving in 2D
n The state vector is 3x1: [x,y,q]
n The covariance matrix is 3x3
n Problem: Mobile robot dynamics are NOT linear
Problems with the
Linear Model Assumption
n Many systems of interest are highly non-
linear, such as mobile robots
n In order to model such systems, a linear
process model must be generated out of the
non-linear system dynamics
n The Extended Kalman filter is a method by
which the state propagation equations and
the sensor models can be linearized about the
current state estimate
n Linearization will increase the state error
residual because it is not the best estimate
Approximating Robot Motion
Uncertainty with a Gaussian
 Linearized Motion Model
for a Robot
x!t = Vt
Y From a robot-centric
y x y! t = 0
v perspective, the
velocities look like this: f! = w
t t

w
From the global x!t = Vt cos ft
perspective, the y! t = Vt sin ft
velocities look like this: f! = w
G X t t

The discrete time state xˆt +1 = xˆt + (Vt + wVt )dt cos fˆt Problem! We don’t know
linear and rotational
estimate (including noise)
yˆ t +1 = yˆ t + (Vt + wVt )dt sin fˆt velocity errors. The state
looks like this: estimate will rapidly diverge
fˆt +1 = fˆt + (wt + wwt )dt if this is the only source of
information!
Linearized Motion Model
for a Robot
Now, we have to compute the covariance matrix propagation
equations.

The indirect Kalman filter derives the pose equations from the
estimated error of the state: xt +1 - xˆt +1 = ~xt +1
yt +1 - yˆ t +1 = ~
yt +1
~
f - fˆ = f
t +1 t +1 t +1

In order to linearize the system, the following small-angle

assumptions are made: cos f~ @ 1
~ ~
sin f @ f
Linearized Motion Model
for a Robot
From the error-state propagation equation, we can obtain the
State propagation and noise input functions F and G :

é~xt +1 ù é1 0 - Vmdt sin fˆù é ~xt ù é- dt cos fR 0 ù

ê~ ú ê ˆ úê~ ú ê ú é wVt ù
ê y~t +1 ú = ê0 1 Vmdt cos f ú ê y~t ú + ê - dt sin fR 0 úê ú
wwt û
êëft +1 úû ê0 0 1 ú êf ú ê 0 - dt úû ë
ë ûë t û ë
~ ~
X t +1 = Ft X t + GtWt

From these values, we can easily compute the standard

covariance propagation equation:

T T
Pt +1/ t = Ft Pt / t Ft + Gt Qt Gt
 Sensor Model for a Robot with
a Perfect Map
é xLt+1 ù é nx ù
Y From the robot, the ê ú ê ú
measurement looks zt +1 = ê yLt+1 ú + ên y ú
L like this: êfL ú ênf ú
y x ë t +1 û ë û
z
From a global
perspective, the
measurement looks
like:
G X
écos ft +1 - sin ft +1 0ù é xLt +1 - xt +1 ù é nx ù
ê ú ê ú ê ú
zt +1 = ê sin ft +1 cos ft +1 0ú ê yLt +1 - yt +1 ú + ên y ú
êë 0 0 1úû êë fLt +1 - ft +1 úû êënf úû

The measurement equation is nonlinear and must also be linearized!

 Sensor Model for a Robot with
a Perfect Map
Now, we have to compute the linearized sensor function. Once
again, we make use of the indirect Kalman filter where the
error in the reading must be estimated.

In order to linearize the system, the following small-angle

assumptions are made: cos f~ @ 1
~ ~
sin f @ f

The final expression for the error in the sensor reading is:

é~
xLt +1 ù é- cos fˆt +1 - sin fˆt +1 - sin fˆt+1 ( xL - xˆt +1 ) + cos fˆt ( y L - yˆ t +1 ) ù é ~
xt +1 ù é nx ù
ê~ ú ê ˆ ˆ ˆ ( x - xˆ ) - sin fˆ ( y - yˆ )ú ê ~ ú + ên ú
y =
ê Lt +1 ú ê sin f t +1 - cos f t +1 - cos f t +1 L t +1 t L y
t +1 ú ê t +1 ú ê yú
~
êfL ú ê 0 ~
ú êf ú ê n ú
0 -1
ë t +1 û ë û ë t +1 û ë f û
Updating the State Vector

Propagation only Propagation and update

Simulated Results Pose
Uncertainty with no Updates
Simulated Results Pose
Uncertainty with Updates
Extended Kalman Filter for
SLAM
n State vector
n Expanded to contain entries for all
landmarks positions: X = [X RT X LT ! X LT ]T 1 n

n State vector can be grown as new

landmarks are discovered
n Covariance matrix is also expanded
é PRR PRL1 ! PRLN ù
êP PL1L1 ! PL1LN ú
ê L1R ú
ê " " # " ú
ê ú
êë PLN R PLN L1 ! PLN LN úû
Extended Kalman Filter for
SLAM
n Kinematic equations for landmark
propagation

xˆt +1 = xˆt + (Vt + wVt )dt cos fˆt

yˆ t +1 = yˆ t + (Vt + wVt )dt sin fˆt
fˆt +1 = fˆt + (wt + ww )dt
t

xˆ Lit +1 = xˆ Li t
yˆ Lit +1 = yˆ Lit
fˆL t +1 = fˆL t
i i
Extended Kalman Filter for
SLAM
n Sensor equations for update:
~
[
~T
X = XR
~T ~T
X L1 ! X Li
~T
! X Ln ]
H = [H R 0 ! 0 H Li 0 ! 0 ]
n Very powerful because covariance
update records shared information
between landmarks and robot positions
EKF for SLAM
 Enhancements to EKF
n Iterated Extended Kalman Filter
State and
covariance update
rt +1 = zt +1 - zˆt +1
St +1 = H t +1 Pk +1/ k H Tt +1 + Rt +1
K t +1 = Pk +1/ k H Tt +1 S t-+11
Xˆ k +1/ k +1 = Xˆ k +1/ k + K t +1rt +1
Pk +1/ k +1 = Pk +1/ k - K t +1St +1 K Tt +1

Iterate state update

equation until
convergence
Enhancements to the EKF
n Multiple hypothesis tracking
n Multiple Kalman filters are used to track the data
n Multi-Gaussian approach allows for representation
of arbitrary probability densities
n Consistent hypothesis are tracked while highly
inconsistent hypotheses are dropped
n Similar in spirit to particle filter, but orders of
magnitude fewer filters are tracked as compared
to the particle filter