Lecture11:

KalmanFilters
CS344R:Robotics
BenjaminKuipers

UpToHigherDimensions
OurpreviousKalmanFilterdiscussionwas
ofasimpleonedimensionalmodel.
Nowwegouptohigherdimensions:
n
Statevector: x
m
Sensevector: z
l
Motorvector: u
First,alittlestatistics.

Expectations
Letxbearandomvariable.
TheexpectedvalueE[x]isthemean:

E[x] =

1
x p(x) dx x= x i
N 1

Theprobabilityweightedmeanofallpossible
values.Thesamplemeanapproachesit.

Expectedvalueofavectorxisby

T
component.
E[x] =x =[x1,L xn ]

VarianceandCovariance
ThevarianceisE[(xE[x])2]
N
1
2 =E[(xx) 2] = (xi x) 2
N 1
CovariancematrixisE[(xE[x])(xE[x])T]
N
1
Cij = (xik xi )(xjk x j )
N k=1
DividebyN1tomakethesamplevariancean
unbiasedestimatorforthepopulationvariance.

BiasedandUnbiasedEstimators
Strictlyspeaking,thesamplevariance
N
1
2
2
2
=E[(xx) ] = (xi x)
N 1
isabiasedestimateofthepopulation
variance.Anunbiasedestimatoris:
N
1
2
2
s =
(x i x)

N 1 1
But:IfthedifferencebetweenNandN1
evermatterstoyou,thenyouareprobably
uptonogoodanyway[Press,etal]

CovarianceMatrix
Alongthediagonal,Ciiarevariances.
OffdiagonalCijareessentiallycorrelations.

C 1,1 = 12

C2,1

CN ,1

C1,2
2
C2,2 = 2

M
2
CN ,N = N
C1,N

O
L

IndependentVariation
xandyare
Gaussianrandom
variables(N=100)
Generatedwith
x=1y=3
Covariancematrix:
0.90

0.44
Cxy =

8.82
0.44

DependentVariation
canddarerandom
variables.
Generatedwith
c=x+yd=xy
Covariancematrix:
10.62

7.93
Ccd =

8.84
7.93

DiscreteKalmanFilter
n

x
Estimatethestateofalinear
stochasticdifferenceequation
xk =Axk1 +Buk +wk1
processnoisewisdrawnfromN(0,Q),with
covariancematrixQ.

withameasurement z

zk =Hxk +vk

measurementnoisevisdrawnfromN(0,R),with
covariancematrixR.

A,Qarenxn.Bisnxl.Rismxm.Hismxn.

EstimatesandErrors
n

xistheestimatedstateattimestepk.
k

xafterprediction,beforeobservation.
k

e
=x

x
Errors:
k
k
k
k
ek =xk x

Errorcovariancematrices:
T

k k

P =E[e e ]
T
k

Pk =E[ek e ]

x
KalmanFilterstaskistoupdate k Pk

TimeUpdate(Predictor)
Updateexpectedvalueofx

k1 +Buk
xk =Ax
UpdateerrorcovariancematrixP

T
Pk =APk1A +Q
Previousstatementsweresimplified
versionsofthesameidea:

3
2
3

(t ) =x
(t2) +u[t3 t2]
x
2

(t ) = (t2) + [t3t2 ]

MeasurementUpdate(Corrector)
Updateexpectedvalue

k =x
+K k(zk Hx
)
x

k
innovationis zk Hx

Updateerrorcovariancematrix

Pk =(IK kH)P

Comparewithpreviousform

(t3) =x
(t ) +K(t3)(z3 x
(t ))
x
2
2
(t3) =(1K(t3)) (t3 )

TheKalmanGain
TheoptimalKalmangainKkis
T
T
1
K k =Pk H (HPk H +R)
T
k
T
k

PH
=
HP H +R
Comparewithpreviousform
2

(t3 )
K (t3) = 2
2
(t3 ) + 3

ExtendedKalmanFilter
Supposethestateevolutionand
measurementequationsarenonlinear:
xk =f (xk1,uk ) +wk1

zk =h(xk) +vk
processnoisewisdrawnfromN(0,Q),with
covariancematrixQ.
measurementnoisevisdrawnfromN(0,R),
withcovariancematrixR.

TheJacobianMatrix
Forascalarfunctiony=f(x),
y = f (x)x
Foravectorfunctiony=f(x),
f1
y1 (x) L
x1
y =J x = M = M
f
yn n (x) L

x1

f1
(x)
xn x1
M M

fn
xn
(x)
xn

LinearizetheNonLinear
LetAbetheJacobianoffwithrespecttox.
fi
A ij =
(xk1,uk)
x j
LetHbetheJacobianofhwithrespecttox.

hi
Hij =
(xk)
x j

ThentheKalmanFilterequationsare
almostthesameasbefore!

EKFUpdateEquations
Predictorstep:

k1,uk )
xk =f (x

P =APk1A +Q

Kalmangain: K k =P H (HP H +R)

k =x
+K k(zk h(x
))
Correctorstep: x

Pk =(IK kH)P

CatchTheBallAssignment
Stateevolutionislinear(almost).
WhatisA?
B=0.

Sensorequationisnonlinear.
Whatisy=h(x)?
WhatistheJacobianH(x)ofhwithrespecttox?

Errorsaretreatedasadditive.IsthisOK?
WhatarethecovariancematricesQandR?

TTD
IntuitiveexplanationsforAPATandHPHT
intheupdateequations.