Observers and Kalman Filters

CS 393R: Autonomous Robots

Slides Courtesy of
Benjamin Kuipers
 Good Afternoon Colleagues

Are there any questions?

 Stochastic Models of an
Uncertain World
x F(x,u) x F(x,u,1)

y G(x) y G(x,2 )

Actions are uncertain.

Observations are uncertain.
i ~ N(0,i) are random variables
 Observers
x F(x,u,1 )
y G(x,2 )
The state x is unobservable.
The sense vector y provides noisy information
about x.
An observer x Obs(y) is a process that uses
sensory history to estimate x.
Then a control law can be written
u Hi (x )
Kalman Filter: Optimal Observer
1 x F(x,u,1 )
u y G(x,2 )
x

y x
2
 Estimates and Uncertainty
Conditional probability density function
 Gaussian (Normal) Distribution
Completely described by N(, 2)
Mean
Standard deviation , variance 2

1 (x )2 / 2 2
2
e
 The Central Limit Theorem
The sum of many random variables
with the same mean, but
with arbitrary conditional density functions,
converges to a Gaussian density function.

If a model omits many small unmodeled

effects, then the resulting error should
converge to a Gaussian density function.
Illustrating the Central Limit Thm
Add 1, 2, 3, 4 variables from the same distribution.
 Detecting Modeling Error
Every model is incomplete.
If the omitted factors are all small, the
resulting errors should add up to a Gaussian.

If the error between a model and the data

is not Gaussian,
Then some omitted factor is not small.
One should find the dominant source of error
and add it to the model.
 Estimating a Value
Suppose there is a constant value x.
Distance to wall; angle to wall; etc.
At time t1, observe value z1 with variance
2
1

The optimal estimate is x (t1) z1 with

variance 1
2
 A Second Observation
At time t2, observe value z2 with variance
2
2
Merged Evidence
 Update Mean and Variance
Weighted average of estimates.
x (t 2) Az1 Bz2 A B 1
The weights come from the variances.
Smaller variance = more certainty
2 2
x (t 2 ) 2 2 2 z1 2 1 2 z2

1 2

1 2

1 1 1
2 2
(t 2) 1 2
2
 From Weighted Average
to Predictor-Corrector
Weighted average:
x (t 2) Az1 Bz2 (1 K)z1 Kz2

Predictor-corrector:
x (t 2) z1 K(z2 z1)
x (t1) K(z2 x (t1))
This version can be applied recursively.
 Predictor-Corrector
Update best estimate given new data
x (t 2) x(t1) K(t 2)(z2 x (t1))
12
K(t 2 ) 2
1 2
2

Update variance:
(t 2 ) (t1 ) K(t2 ) (t1 )
2 2 2

(1 K(t2 )) (t1 )
2
 Static to Dynamic
Now suppose x changes according to
x F(x,u,) u (N (0, ))
 Dynamic Prediction
At t2 we know x (t 2 ) (t 2 )
2

At t3 after the change, before an observation.

x (t ) x (t2 ) u[t3 t 2 ]
3

(t ) (t2 ) [t3 t2 ]
2
3
2 2

Next, we correct this prediction with the

observation at time t3.
 Dynamic Correction
At time t3 we observe z3 with variance
2
3

Combine prediction with observation.

x (t 3 ) x(t ) K(t3 )(z3 x (t ))
3 3

(t 3 ) (1 K(t 3 )) (t )
2 2
3

(t )2
K(t 3 ) 2 3
(t3 ) 3
2
 Qualitative Properties

x (t 3 ) x(t ) K(t3 )(z3 x (t ))
3 3

(t3 )
2
K(t 3 ) 2
(t3 ) 32

Suppose measurement noise

2
3 is large.
Then K(t3) approaches 0, and the measurement
will be mostly ignored.

Suppose prediction noise (t ) is large.
2
3

Then K(t3) approaches 1, and the measurement

will dominate the estimate.
 Kalman Filter
Takes a stream of observations, and a
dynamical model.
At each step, a weighted average between
prediction from the dynamical model
correction from the observation.
The Kalman gain K(t) is the weighting,
based on the variances (t) and
2 2

With time, K(t) and (t) tend to stabilize.

2
 Simplifications
We have only discussed a one-dimensional
system.
Most applications are higher dimensional.
We have assumed the state variable is
observable.
In general, sense data give indirect evidence.

x F(x,u,1) u 1
z G(x,2) x 2
We will discuss the more complex case next.
 Up To Higher Dimensions
Our previous Kalman Filter discussion was
of a simple one-dimensional model.
Now we go up to higher dimensions:
State vector: x n

Sense vector: z
m

Motor vector: u
l

First, a little statistics.

 Expectations
Let x be a random variable.
The expected value E[x] is the mean:
N
E[x] x p(x) dx x x i
1
N 1
The probability-weighted mean of all possible
values. The sample mean approaches it.
Expected value of a vector x is by component.

E[x] x [x1, x n ]T
 Variance and Covariance
The variance is E[ (x-E[x])2 ]
N
E[(x x ) ] (x i x )
2 2 1 2

N 1
Covariance matrix is E[ (x-E[x])(x-E[x])T ]
N
Cij
1
N
(x ik x i )(x jk x j )
k 1

Divide by N1 to make the sample variance an

unbiased estimator for the population variance.
 Covariance Matrix
Along the diagonal, Cii are variances.
Off-diagonal Cij are essentially correlations.

C1,1 12 C1,2 C1,N

C2,1 C2,2 2
2


2
CN ,1 CN ,N N
 Independent Variation
x and y are
Gaussian random
variables (N=100)
Generated with
x=1 y=3
Covariance matrix:
0.90 0.44
Cxy
0.44 8.82
 Dependent Variation
c and d are random
variables.
Generated with
c=x+y d=x-y
Covariance matrix:

10.62 7.93
Ccd
7.93 8.84
 Discrete Kalman Filter
Estimate the state x n of a linear
stochastic difference equation
x k Ax k1 Buk1 wk1
process noise w is drawn from N(0,Q), with
covariance matrix Q.
with a measurement z m
zk Hx k vk
measurement noise v is drawn from N(0,R), with
covariance matrix R.
A, Q are nn. B is nl. R is mm. H is mn.
 Estimates and Errors
x k is the estimated state at time-step k.
n

x k after prediction, before observation.
n

Errors: e
k x k x
k

e k x k x k
Error covariance matrices:
T
P E[e e ]
k k k

Pk E[e k eTk ]
Kalman Filters task is to update x k Pk
 Time Update (Predictor)
Update expected value of x
x k Ax k1 Buk1
Update error covariance matrix P

Pk APk 1A QT

Previous statements were simplified

versions of the same idea:

x (t ) x (t2 ) u[t3 t 2 ]
3

(t ) (t2 ) [t3 t2 ]
2
3
2 2
Measurement Update (Corrector)
Update expected value

x k x K k (z k Hx )
k k

innovation is z k Hx k
Update error covariance matrix

Pk (I K kH) P k

Compare with previous form

x (t 3 ) x(t ) K(t3 )(z3 x (t ))
3 3
2
(t 3 ) (1 K(t 3 )) (t3 )
2
 The Kalman Gain
The optimal Kalman gain Kk is
T T 1
K k Pk H (HPk H R)
T
PH
T
k
HPk H R

Compare with previous form

(t )
2
K(t 3 ) 2 3
(t3 ) 3
2
 Extended Kalman Filter
Suppose the state-evolution and
measurement equations are non-linear:
x k f (x k1,uk1) wk1
zk h(x k ) vk
process noise w is drawn from N(0,Q), with
covariance matrix Q.
measurement noise v is drawn from N(0,R),
with covariance matrix R.
 The Jacobian Matrix
For a scalar function y=f(x),
y f (x)x
For a vector function y=f(x),
f1 f1
y1 (x) (x)
x1 x n x1
y Jx
f f n
y n n (x) (x) x n

x1 x n
 Linearize the Non-Linear
Let A be the Jacobian of f with respect to x.
f i
A ij (x k1,uk1 )
x j
Let H be the Jacobian of h with respect to x.
hi
H ij (x k )
x j
Then the Kalman Filter equations are
almost the same as before!
 EKF Update Equations
Predictor step: x k f ( x k1,uk1 )

P APk 1A Q
k
T

1
gain: K k P H (HP H R)
T T
Kalman k k

Corrector step: x k x k K k (z k h(x k ))

Pk (I K kH) P k