Scribd
Upload
63 views19 pages

Lecture 11: Kalman Filters: CS 344R: Robotics Benjamin Kuipers

This lecture discusses extending the Kalman filter to higher dimensions and nonlinear systems. It introduces concepts such as state vectors, covariance matrices, and the Jacobian matrix. The key steps of the discrete and extended Kalman filters are presented. These include the time update to predict the state and error covariance, and the measurement update to incorporate new measurements and correct the estimates.

Uploaded by

K.Ramachandran
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PPT, PDF, TXT or read online on Scribd
63 views19 pages

Lecture 11: Kalman Filters: CS 344R: Robotics Benjamin Kuipers

This lecture discusses extending the Kalman filter to higher dimensions and nonlinear systems. It introduces concepts such as state vectors, covariance matrices, and the Jacobian matrix. The key steps of the discrete and extended Kalman filters are presented. These include the time update to predict the state and error covariance, and the measurement update to incorporate new measurements and correct the estimates.

Uploaded by

K.Ramachandran
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PPT, PDF, TXT or read online on Scribd
You are on page 1/ 19

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.

You might also like