-
Simultaneous Localization and
Mapping (SLAM)
Lecture
01
Introduction
SLAM Objective
?
Place a robot
in an unknown location in an unknown environment
and
have the
robot incrementally build a map of this
environment while
simultaneously using
this map to compute vehicle location
?
SLAM began with
seminal paper by R. Smith, M. Self, and P.
Cheeseman in
1990
?
A solution to SLAM has been seen as the
“Holy
Grail”
–
Would enable
robots to operate in an environment without
a priori
knowledge of obstacle
locations
?
Research over the last decade has shown
that a solution is possible!!
The
Localization Problem
Defined
?
A map
m
of landmark locations is
known a priori
?
Take measurements of landmark location
z
k
(i.e. distance
and bearing)
?
Determine vehicle location
x
k
based on
z
k
–
Need filter if sensor is
noisy!
?
?
?
?
x
k
: location of
vehicle at time k
u
k
: a control
vector applied at k-1 to
drive the
vehicle from x
k-1
to
x
k
z
k
: observation
of a landmark taken
at time
k
X
k
:
history of states {x
1
, x
2
,
x
3
,
…,
x
k
}
?
U
k
: history of
control inputs {u
1
, u
2
,
u
3
,
…,
u
k
}
?
m: set of all
landmarks
The Mapping Problem
Defined
?
The vehicle
locations
X
k
are
provided
?
Take measurement of landmark location
z
k
(i.e. distance
and bearing)
?
Build map
m
based
on on z
k
–
Need filter if
sensor is noisy!
?
?
?
?
X
k
: history of
states {x
1
,
x
2
,
x
3
,
…,
x
k
}
z
k
: observation
of a landmark taken at
time
k
m
i
:
true location of the i
th
landmark
m: set of all
landmarks
Simultaneous Localization and
Mapping
Defined
?
From knowledge
of observations
Z
k
k
–
Determine vehicle locations
X
–
Build map
m
of
landmark locations
?
?
?
?
?
x
k
: location of
vehicle at time k
u
k
: a control
vector applied at k-1 to
drive the
vehicle from x
k-1
to
x
k
m
i
: true location
of i
th
landmark
z
k
: observation
of a landmark taken
at time
k
X
k
:
history of states {x
1
, x
2
,
x
3
,
…,
x
k
}
?
U
k
: history of
control inputs {u
1
, u
2
,
u
3
,
…,
u
k
}
?
m: set of all
landmarks
?
Z
k
: history of
all observations {z
1
,
z
2
,
…,
z
k
}
H. Durrant-Whyte, D. Rye,
E. Nebot,
“Localisatio
n of
Automatic Guided
Vehicles”,
ISRR
1995
Simultaneous Localization and
Mapping
Characteristics
?
Localization
and mapping are coupled problems
–
Two quantities
are to be inferred from a single
measurement
?
A solution can only be obtained if the
localization and mapping
processes are considered
together
H. Durrant-Whyte,
D. Rye, E. Nebot,
“Localisation
of Automatic
Guided
Vehicles”,
Robotics Research: The
7
th
International Symposium
(ISRR 1995)
SLAM Fundamentals
Setting
?
A vehicle with
a known kinematic model moving through an
?
environment containing a
population of landmarks
(process
model)
The vehicle is
equipped with a sensor that can take measurements
of the relative location between any
individual landmark and the
vehicle
itself
(observation
model)
SLAM Fundamentals
Process Model
?
For better
understanding, a linear model of the vehicle is
assumed
?
If the state of the vehicle is given as
x
v
(k) then the vehicle model
is
x
(
k
F
k
x
k
u
k
v
1)
v
(
)
(
v
)
(
v
1)
where
–
F
v
(k) is the
state transition matrix
–
u
v
(k)
is a vector of control inputs
–
w
v
(k) is a vector
of uncorrelated process noise errors with zero
mean and
covariance
Q
v
(k)
?
The state
transition equation for the i
th
landmark is
p
(
1)
(
)
i
k
p
k
p
w
(
v
k
1)
i
i
SLAM considers all landmarks
stationary!
SLAM Fundamentals
Process Model
?
The augmented
state vector containing both the state of the
vehicle
and the state of
all landmark locations is
x
x
(
k
)
T
T
v
(
k
)
p
1
...
p
?
The
state transition model
for the complete system is
now
T
T
N
v
x
(
k
1)
F
(
k
)
0
0
(
k
)
u
k
(
w
k
1)
1)
(
p
1
p
v
0
I
p
0
0
0
1
0
0
I
x
v
p
1
p
v
0
p
1
0
v
0
p
1
0
N
where
–
I
pi
is the
dim(p
i
) x
dim(p
i
) identity matrix
–
null
vector
p
N
N
p
p
N
N
0
pi
is the
dim(p
i
)
SLAM
Fundamentals
Observation
Model
?
Assuming the observation to be linear,
the
observation model
for
the
i
th
landmark is
given as
z
(
k
)
H
x
(
k
)
v
(
k
i
)
i
where
–
–
v
i
(k) is a vector
of uncorrelated observation errors with zero mean
and
variance
R
i
(k)
H
i
is the
observation matrix
that relates the
sensor output z
i
(k) to the
state
vector x(k) when observing the
i
th
landmark and is written
as
H
H
,0
0,
H
,0
0
i
v
–
Re-expressing the observation
model
z
(
k
)
H
p
i
p
H
x
(
k
)
p
i
v
v
v
(
k
i
)
Estimation
Process
Objective
?
The state of
our discrete-time process x
k
needs to be estimated based
on our measurement
z
k
?
This is the exact definition of the
Kalman filter!!
Kalman
Filter
?
Recursively computes estimates of state
x(k) which is evolving
according to the
process and
observation models
?
The filter
proceeds in three stages
–
Prediction
–
Observation
–
Update
Estimation
Process
Prediction
?
(
k
)
x
?
After initializing the filter (i.e.
setting values for
prediction is
generated for
–
The
a priori
state estimate
and P(k)), a
x
?(
k
1|
k
)
F
(
k
)
x
?
(
k
|
k
)
u
(
k
)
–
The
a priori
observation
relative to the i
th
landmark
i
?
(
k
1|
k
)
H
(
k
)
x
z
?
(
k
1|
k
–
The
a priori
state covariance
(e.g. a measure of how uncertain the states
i
)
computed by the process model
are)
P
(
k
1|
k
)
F
(
k
)
P
(
k
|
k
)
F
T
(
k
)
Q
(
k
)
Estimation Process
Observation
?
Following the
prediction, an observation
z
i
(k+1) of the
i
th
landmark is
made using the
observation
model
?
An innovation and innovation covariance
matrix are calculated
–
Innovation is
the discrepancy between the actual measurement
z
?
z
k
and the
predicted
measurement
k
i
v
(
k
1)
z
(
k
i
1)
z
?
(
k
i
1|
k
S
i
(
k
H
k
P
k
k
H
)
k
R
k
1)
(
)
(
1|
)
i
(
T
)
(
i
i
1)
Estimation
Process
Update
?
The state
estimate and corresponding state estimate
covariance are
then updated
according to
?
k
k
x
k
k
W
x
k
(
1|
1)
?(
1|
)
(
i
1)
P
(
k
k
P
k
k
W
k
1|
1)
(
1|
)
(
1)
(
i
where the gain matrix
W
i
(k+1) is given
by
k
(
i
S
k
(
w
1)
W
T
k
1)
i
1)
W
(
k
i
1)
P
k
k
H
k
S
k
(
1|
)
(
)
(
1)
1
T
i
i
Kalman
Filter
A Closer
Look…
Kalman
Filter
Background
?
Developed by
Rudolph E. Kalman in 1960
?
A set of
mathematical equations that provides an
efficient computational
(recursive) means to
estimate the state of a
process
?
It supports estimations of
–
Past
states
–
Present states
–
Future
states
a
nd can do so when the
nature of the modeled
system is
unknown!
Discrete Kalman
Filter
Process
Model
?
Assumes true state at time k evolves
from state (k-1) according to
x
(
k
)
Fx
(
k
1)
Gu
(
k
1)
w
(<
/p>
k
)
where
–
–
–
F
is the state transition model (A
matrix)
G is the control
input matrix (B matrix)
w(k)
is the process noise which is assumed to be white
and have a normal
probability
distribution
p
(
w
)
~
N
(0,
Q
)
covariance
Discrete Kalman
Filter
Observation
Model
?
At time k, a measurement z(k) of the
true state x(k) is made according to
z
(
k
)
Hx
(
k
)
v
(
k
)
where
–
–
H
is the observation matrix and relates the
measurement z(k) to the state
vector
x(k)
v(k) is the observation
noise which is assumed to be white and have a
normal
probability
distribution
p
(
v
)
~
N
(0,
R
)
covariance
Discrete Kalman
Filter
Algorithm
?
It’s
recursive!
–
Only the estimated state from the
previous time step and the current
measurement are needed to compute the
estimate for the current state
?
The state of
the filter is represented by two
variables
–
x(k): estimate of the state at time
k
–
P(k|k): error covariance matrix (a
measure of the estimated accuracy of the
state estimate)
?
The filter has
two distinct stages
–
Predict (and
observe)
–
Update
Discrete Kalman Filter
(Notation 1)
Prediction
Predicted state
?
Predicted
covariance
Observation
?
Innovation
?
Innovation covariance
Update
?
Optimal Kalman
gain
?
Updated state
F
(
k
x
?
k
k
B
k
u
k
)
(
1|
1)
(
)
(
1)
?
F
T
(
k
)
P
(
k
1
|
k
1)
F
(
k
)
Q
(
k
)
~
y
(
k
)
z
k
H
k
S
(
k
)
(
)
H
(
k
)
P
(
(
k
)
|
?
k
(
x
k
1)
|
k
1)
H
(
k
)
T
R
(
k
)
Not the same
variable!!
K
T
(
k
P
k
k
H
k
S
k
)
(
|
1)
(
)
(
)
1
~
?
Updated
covariance
Not the same
variable!!
x
?
(
k
(
I
k
K
k
(
)
K
(
k
)
y
(
k
H
k
P
k
k
(
))
(
|
)
1)
|
1)
Discrete Kalman Filter
(Notation 2)
Prediction
?
Predicted state
?
Predicted
estimate covariance
Observation
?
Innovation
~
y
(
k
?
Innovation covariance
?
k
x
(
)
)
z
k
(
(
)
S
(
F
k
x
k
(
)
?(
1)
P
(
)
k
H
x
k
)
?
k
)
HP
k
(
Bu
k
(
1)
(
FP
k
)
H
T
1)
T
F
R
Q
Update
?
Optimal Kalman
gain
K
(
k
P
k
HS
k
)
(
)
(
)
?
Updated state
estimate
?
Updated estimate covariance
x
?
(
k
)
x
?
(
k
)
P
(
k
)
(
1
~
K
(
k
)
y
(
k
)
I
K
k
H
P
(
(
)
)
k
)
Discrete Kalman
Filter
Prediction
(1) Project the state ahead
?
k
x
(
)
F
k
x
k
(
)
?(
1)
Bu
k
(
1)
(2) Project
the error covariance ahead
P
(
)
k
(
FP
k
1)
F
T
Q
Observation and
Update
(1) Compute the Kalman gain
K
(
k
P
k
H
T
HP
k
H
T
)
(
)
(
(
)
R
1
)
(2) Update estimate with measurement
z(k)
?
k
x
(
)
x
k
?(
)
K
k
z
k
(
)[
(
)
Hx
k
?(
)
]
(3)
Update error covariance
P
(
k
)
I
K
(
k
H
P
(
)
k
)
(
)
A Kalman Filter
in Action
An
Example…
Kalman
Filter Example
Process
Model
?
Estimate a scalar random constant (e.g.
voltage )
–
Measurements are corrupted by 0.1 volt
RMS white noise
Kalman Filter
Example
Process
Model
?
Governed by the linear difference
equation
x
(
k
)
Fx
(
k
1)
Gu
(
k
1)
x
(
k
)
x
(
k
1)
w
(
k
)
with a
measurement
z
(
k
)
Hx
(
k
)
v
(
k
)
w
(
k
)
State
doesn’t
change (F=0)
No control
input (u=0)
z
)
(
k
)
x
(
k
)
v
(
k
Measurement is of
state
directly
(H=1)
Output
Kalman Filter
Example
Simultaneous
Localization and
Mapping (SLAM)
Lecture 02
Discrete Kalman
Filter
Recall
Prediction
?
Predicted state
?
Predicted
estimate covariance
Observation
?
Innovation
~
y
(
k
?
Innovation covariance
?
k
x
(
)
)
z
k
(
(
)
S
(
F
k
x
k
(
)
?(
1)
P
(
)
k
H
x
k
)
?
k
)
HP
k
(
Bu
k
(
1)
(
FP
k
)
H
T
1)
T
F
R
Q
Update
?
Optimal Kalman
gain
K
(
k
P
k
HS
k
)
(
)
(
)
?
Updated state
estimate
?
Updated estimate covariance
x
?
(
k
)
x
?
(
k
)
P
(
k
)
(
1
~
K
(
k
)
y
(
k
)
I
K
k
H
P
(
(
)
)
k
)
Discrete Kalman
Filter
Recall
Prediction
(1) Project the
state ahead
?
k
x
(
)
F
k
x
k
(
)
?(
1)
Bu
k
(
1)
(2) Project
the error covariance ahead
P
(
)
k
(
FP
k
1)
F
T
Q
Observation and
Update
(1) Compute the Kalman gain
K
(
k
P
k
H
T
HP
k
H
T
)
(
)
(
(
)
R
1
)
(2) Update estimate with measurement
z(k)
?
k
x
(
)
x
k
?(
)
K
k
z
k
(
)[
(
)
Hx
k
?(
)
]
(3)
Update error covariance
P
(
k
)
I
K
(
k
H
P
(
)
k
)
(
)
Another
Example
t=0
y
.
0
= 125
y
0
= 0
t=1
t=2
Kinematic
Equations
1
2
y
y
0
y
t
a
(
t
)
0
2
y
t
y
0
a
Position (from model)
t=3
Process Model
Process Model
y
(
k
1)
y
(<
/p>
k
)
y
(
k
)
y
k
(
1)
y
k
where
y
(
k
1
)
y
(
k
1)
1
t
a
(
2
t
(
a
)
t
x
(
k
1)
)
2
y
(
k
)
and
y
(
k
)
x
(
k
)
1
0
t
x
k
(
)
2
2
t
a
so
x
(
k
1)
1
Observation Model
Observation Model
z
(
k
)
Hx
(
k
)
v
(
k
)
where
because z is a
H
1
0
measurement of the height
directly
Kalman Filter
Initial Estimates
?
x
(
k
y
(
1)
k
y
(
k
Prediction
1
1
1
x
(
k
)
?(
1)
*
9.81
x
k
1)
100
1)
P
(
k
0
P
1
1
1)
(
k
)
P
(
k
1)
1
1
1
1
1
0
1
R
t
1
1
0
0
0
0
1