Introduction to Kalman Filtering - Institute For Systems...
Transcript of Introduction to Kalman Filtering - Institute For Systems...
Introduction to Kalman FilteringA set of two lectures
Maria Isabel RibeiroAssociate Professor
Instituto Superior Técnico / Instituto de Sistemas e Robótica
June 2000
All rights reserved
2
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
INTRODUCTION TO KALMAN FILTERING
• What is a Kalman Filter ?– Introduction to the Concept– Which is the best estimate ?– Basic Assumptions
• Discrete Kalman Filter– Problem Formulation– From the Assumptions to the Problem Solution– Towards the Solution– Filter dynamics
• Prediction cycle• Filtering cycle• Summary
– Properties of the Discrete KF– A simple example
• The meaning of the error covariance matrix• The Extended Kalman Filter
3
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
WHAT IS A KALMAN FILTER?
• Optimal Recursive Data Processing Algorithm
Measuring devices
System
Kalman filter
System state (desired but not know)
Measurement error sources
Controls
System error sources
Observed measurements
Optimal estimate of system state
• Typical Kalman filter application
4
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
WHAT IS A KALMAN FILTER?Introduction to the Concept
•• OptimalOptimal Recursive Data Processing Algorithm
– Dependent upon the criteriacriteria chosen to evaluate performance
– Under certain assumptionsassumptions, KF is optimaloptimal with respect to virtually any criteria that makes sense.
– KF incorporates all available informationall available information• knowledge of the system and measurement device dynamics• statistical description of the system noises, measurement errors, and
uncertainty in the dynamics models• any available information about initial conditions of the variables of interest
5
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
WHAT IS A KALMAN FILTER?Introduction to the concept
•• OptimalOptimal Recursive Data Processing Algorithm
))1k(v),1k(x(h)1k(z))k(w),k(u),k(x(f)1k(x
++=+=+
• x - state• f - system dynamics• h - measurement function• u - controls• w - system error sources• v - measurement error sources• z - observed measurements
•• GivenGiven– f, h, noise characterization, initial conditons– z(0), z(1), z(2), …, z(k)
•• ObtainObtain– the “best”“best” estimate of x(k)
6
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
WHAT IS A KALMAN FILTER?Introduction to the concept
• Optimal RecursiveRecursive Data Processing Algorithm
– the KF does not require all previous data to be kept in storage and reprocessed every time a new measurement is taken.
z(k) ..... z(2) z(1) )0(z
)k(x
KFKF
1)z(k z(k) ..... z(2) z(1) )0(z +
)1k(x +
KFKF
)k(x
KFKFTo evaluate the
KF only requires
and z(k+1)
)1k(x +)k(x
7
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
WHAT IS A KALMAN FILTER?Introduction to the concept
• Optimal Recursive Data ProcessingData Processing Algorithm
– The KF is a data processing algorithm– The KF is a computer program runing in a central processor
8
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
WHAT IS THE KALMAN FILTER ?Which is the best estimate?
• Any type of filter tries to obtain an optimal estimate of desired quantities from data provided by a noisy environment.
• Best = minimizing errors in some respect.
• Bayesian viewpoint - the filter propagates the conditional probability density of the desired quantities, conditioned on the knowledge of the actual data coming from measuring devices
• Why base the state estimation on the conditional probability density function ?
9
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
WHAT IS A KALMAN FILTER?Which is the best estimate?
Example• x(i) one dimensional position of a vehicle at time instant i• z(j) two dimensional vector describing the measurements of position
at time j by two separate radars
• If z(1)=z1, z(2)=z2, …., z(j)=zj
– represents all the information we have on x(i) based (conditioned) on the measurements acquired up to time i
– given the value of all measurements taken up time i, this conditional pdf indicates what the probability would be of x(i) assuming any particular value or range of values.
)z,....,z,z|x(p i21)i(z),...,2(z),1(z|)i(x
10
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
WHAT IS A KALMAN FILTER?Which is the best estimate?
• The shape of conveys the
amount of certainty we have in the knowledge of the value x.
)z,....,z,z|x(p i21
x
)z,....,z,z|x(p i21)i(z),...,2(z),1(z|)i(x
• Based on this conditional pdf, the estimate can be:– the mean - the center of probability mass (MMSE)– the mode - the value of x that has the highest probability (MAP)– the median - the value of x such that half the probability weight lies to
the left and half to the right of it.
11
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
WHAT IS THE KALMAN FILTER ?Basic Assumptions
• The Kalman Filter performs the conditional probability density propagation– for systems that can be described through a LINEARLINEAR model– in which system and measurement noises are WHITEWHITE and GAUSSIANGAUSSIAN
• Under these assumptions,– the conditional pdf is Gaussian– mean=mode=median– there is a unique best estimate of the state– the KF is the best filter among all the possible filter types
• What happens if these assumptions are relaxed?• Is the KF still an optimal filter? In which class of filters?
12
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProblem Formulation
kkkk
kkkkkkkvxCz
k ,wGuBxAx+=
≥++=+ 01
MOTIVATION• Given a discrete-time, linear, time-varying plant
– with random initial state– driven by white plant noise
• Given noisy measurements of linear combinations of the plant state variables
• Determine the best estimate of the system state variable
STATE DYNAMICS AND MEASUREMENT EQUATION
13
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTER Problem Formulation
VARIABLE DEFINITIONS
process) white-nonc (stochasti vector state Rx nk ∈
sequence inputtic determinis Ru mk ∈
mean) zero with(assumed noise system Gaussian whiteRw n
k ∈
mean) zero with(assumed noise tmeasuremen Gaussian whiteRv r
k ∈
sequence) white-nonc (stochasti vector tmeasuremen Rz rk ∈
14
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTER Problem Formulation
INITIAL CONDITIONS
• x0 is a Gaussian random vector, with
– mean
– covariance matrix
STATE AND MEASUREMENT NOISE• zero mean E[wk]=E[vk]=0• {wk}, {vk} - white Gaussian sequences
00 x]x[E =
0PP])xx)(xx[(E T00
T0000 ≥==−−
=
kkT
kTkk
kR
Qvw
vw
E0
0
x(0), wk and vj are independent for all k and j
15
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTER Problem Formulation
DEFINITION OF FILTERING PROBLEM
• Let k denote present value of time
• Given the sequence of past inputs
• Given the sequence of past measurements
• Evaluate the best estimate of the state x(k)
}u,...u,u{U 1k101k
0 −− =
}z,...z,z{Z k21k1 =
16
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProblem Formulation
• Given x0
– “Nature” apply w0
– We apply u0
– The system moves to state x1
– We make a measurement z1
Question: which is the best estimate of x1?
– “Nature” apply w1
– We apply u1
– The system moves to state x2
– We make a measurement z2
Question: which is the best estimate of x2?
.
.
.
)U,Z|x(p 00
111Answer: obtained from
)U,Z|x(p 10
212Answer: obtained from
kkkk
kkkkkkkvxCz
k ,wGuBxAx+=
≥++=+ 01
17
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProblem Formulation
.
.
.
Question: which is the best estimate of xk-1?
– “Nature” apply wk-1
– We apply uk-1
– The system moves to state xk
– We make a measurement zk
Question: which is the best estimate of xk?
.
.
.
)U,Z|x(p 2k0
1k11k
−−−
)U,Z|x(p kkk
101−
Answer: obtained from
Answer: obtained from
18
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERTowards the Solution
• The filter has to propagate the conditional probability density functions
)U,Z|x(p 00
111
)U,Z|x(p 10
212
)U,Z|x(p kkk
20
111
−−−
)U,Z|x(p kkk
101−
)x(p 0
...
)1|1(x
)2|2(x
)1k|1k(x −−
)k|k(x
...
......
19
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERFrom the Assumptions to the Problem Solution
• The LINEARITYLINEARITY of– the system state equation– the system observation
equation• The GAUSSIANGAUSSIAN nature of
– the initial state, x0
– the system whitewhite noise, wk
– the measurement whitewhite noise, vk
)U,Z|x(p kkk
101−
is Gaussian
Uniquely characterized by• the conditional mean• the conditional covariance
]U,Z|x[E)k|k(x kkk
101−=
]U,Z|x;xcov[)k|k(P kkkk
111−=
))k|k(P),k|k(x( ~ )U,Z|x(p 1k0
k0k Ν−
20
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERTowards the Solution
• As the conditional probability density functions are Gaussian, the Kalman filter only propagates the first two momentsonly propagates the first two moments
)U,Z|x(p 00
111
)U,Z|x(p 10
212
)U,Z|x(p kkk
20
111
−−−
)U,Z|x(p kkk
101−
)x(p 0
...
)|(x]U,Z|x[E 1100
111 =
...
......
)x(p 0
)|(x]U,Z|x[E 2210
212 =
)k|k(x]U,Z|x[E kkk 112
01
11 −−=−−−
)k|k(x]U,Z|x[E kkk =−1
01
)1|1(P
)2|2(P
)1k|1k(P −−
)k|k(P
...
...
21
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERTowards the Solution
• We stated that the state estimate equals the conditional mean
]U,Z|x[E)k|k(x kkk
101−=
• Why?• Why not the mode of ? • Why not the median of ?
)U,Z|x(p kkk
101−
)U,Z|x(p kkk
101−
• As is Gaussian– mean = mode = median
)U,Z|x(p kkk
101−
22
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERFilter dynamics
• KF dynamics is recursive
)U,Z|x(p kkk
101− )U,Z|x(p kk
k 01
11+
+
)U,Z|x(p kkk 011+
}u,...,u,u{U
}z,...,z,z{Z
kk
kk
1101
0
211
−− =
=
}u,U{U
}z,...,z,z{Z
kkk
kk
100
211−=
=
}u,U{U
}z,Z{Z
kkk
kkk
100
111
1−
++
=
=
Prediction cyclePrediction cycle Filtering cycleFiltering cycle
What can you say about xk+1 before we make the measurement zk+1
How can we improve our information on xk+1after we make the measurement zk+1
23
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERFilter dynamics
)x(p 0
)U|x(p 001
prediction
)U,Z|x(p 10
112
prediction
)U,Z|x(p 00
111
filtering
)U,Z|x(p 10
212
filtering
)U,Z|x(p kkk
101−
)U,Z|x(p kkk 011+
)U,Z|x(p kkk 0
111+
+ )U,Z|x(p kkk
10
212
+++
)U,Z|x(p kkk
1012+
+
predictionprediction
filtering filtering
24
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERFilter dynamics - Prediction cycle
• Prediction cycle
)U,Z|x(p kkk
101−
))k|k(P),k|k(x( ~ )U,Z|x(p kkk Ν−1
01
assumed known
)U,Z|x(p kkk 011+
?• Is Gaussian
? ]U,Z|x;cov[xk)|1P(k
? )U,Z|x(E)k|k(xkk
k1k
kkk
011
0111
++
+
=+
=+
]U,Z|w[EG]U,Z|u[EB]U,Z|x[EA]U,Z|x[E
wGuBxAxkk
kkkk
kkkk
kkkk
k
kkkkkkk
010101011
1
++=
++=
+
+
kkk uB)k|k(xA)k|k(x +=+1
25
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERFilter dynamics - Prediction cycle
• Prediction cycle
]U,Z|x;cov[xk)|1P(k kkk1k 011++=+
])yy)(yy[(E]y;ycov[ T−−=
)k|k(xx)k|k(x~ k 11 1 +−=+ +
]U,Z|)k|k(x~)k|k(x~[Ek)|1P(k kkT0111 ++=+
prediction error
)uB)k|k(xA(wGuBxA)k|k(x)k(x kkkkkkkkk +−++=+−+ 11
kkk wG)k|k(x~A)k|k(x~ +=+1
Tkkk
Tkk GQGk)A|P(k Ak)|1P(k +=+
26
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERFilter dynamics - Filtering cycle
• Filtering cycle
+ 1+kz )U,Z|x(p kkk 0
111+
+ ?
1º Passo Measurement prediction What can you say about zk+1 before we make the measurement zk+1
)U,Z|x(p kkk 011+
))k|k(P),k|k(x( 11 ++Ν
)U,Z|z(p kkk 011+
)U,Z|vxC(p kkkkk 01111 +++ +
)k|k(xC)k|k(z]U,Z|z[E kkk
k 11 1011 +=+= ++
1110111 11 +++++ ++=+= kTkkz
kkkk RC)k|k(PC)k|k(P]U,Z|z;zcov[
27
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERFilter dynamics - Filtering cycle
• Filtering cycle
2º Passo )U,Z|x(p kkk 011+
]U,z,Z|x[E]U,Z|x[E kk
kk
kkk 01110
111 +++
+ =
xm]z|x[E]y|x[E]z,y|x[E −+=
If x, y and z are jointly Gaussian and y and z are statistically independent
Required result
]U),k|k(z~,Z|x[E]U,Z|x[E kkk
kkk 0110
111 1+= ++
+
)}k|k(z~,Z{ e Z kk 111
1 ++ São equivalentes do ponto de vista de infirmação contida
28
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERFilter dynamics - Filtering cycle
• Filtering cycle
))k|k(xCz(RC)k|k(PCC)k|k(P)k|k(x)k|k(x kkkTkk
Tk 111111 11
11111 +−
+++++=++ ++
−++++
)k|k(z 1+
)k(K 1+Kalman Gain
))k|k(xCz)(k(K)k|k(x)k|k(x kk 11111 11 +−+++=++ ++
)k|k(PCRC)k|k(PCC)k|k(P)k|k(P)k|k(P kkTkk
Tk 111111 1
11111 +
+++−+=++ +
−++++
measurement prediction
)k|k(z~ 1+
29
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERDynamics
• Linear System
• Discrete Kalman Filter
kkkk
kkkkkkkvxCz
k ,wGuBxAx+=
≥++=+ 01
kkk uB)k|k(xA)k|k(x +=+1Tkkk
Tkk GQGk)A|P(k Ak)|1P(k +=+
))k|k(xCz)(k(K)k|k(x)k|k(x kk 11111 11 +−+++=++ ++
)k|k(PCRC)k|k(PCC)k|k(P)k|k(P)k|k(P kkTkk
Tk 111111 1
11111 +
+++−+=++ +
−++++
11111 111
−++++
+++=+ k
Tkk
Tk RC)k|k(PCC)k|k(P)k(K
prediction
filtering
Initial conditions 000 x)|(x = 000 P)|(P =
30
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProperties
• The Discrete KF is a time-varying linear system
– even when the system is time-invariant and has stationary noise
• the Kalman gain is not constant
• Does the Kalman gain matrix converges to a constant matrix? In which conditions?
kkkkk|kkkkk|k uBzKxA)CKI(x ++−= ++++++ 111111
kkkk|kkk|k BuzKxA)CKI(x ++−= +++++ 11111
31
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProperties
• The state estimate is a linear function of the measurements
kkkkk|kkkkk|k uBzKxA)CKI(x ++−= ++++++ 111111
KF dyamics in terms of the filtering estimate
Assuming null inputs for the sake of simplicity
kΦ
3322211120001233
22111000122
1100011
000
zKzKzKxx
zKzKxx
zKxx
xx
||
||
||
|
+Φ+ΦΦ+ΦΦΦ=
+Φ+ΦΦ=
+Φ=
=
32
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProperties
• Innovation process
– z(k+1) carries information on x(k+1) that was not available on– this new information is represented by r(k+1) - innovation process
• Properties of the innovation process– the innovations r(k) are orthogonal to z(i)
– the innovations are uncorrelated/white noise
• this test can be used to acess if the filter is operating correctly
)k|k(xCzr kkk 1111 +−= +++
? )U,Z|x(E)k|k(x kkk 0111 +=+
kZ1
1210 −== k,...,,i ,)]i(z)k(r[E T
ki ,)]i(r)k(r[E T ≠= 0
33
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProperties
• Covariance matrix of the innovation process
111 11 +++ ++=+ kTkk RC)K|K(PC)k(S
11111 111
−++++
+++=+ k
Tkk
Tk RC)k|k(PCC)k|k(P)k(K
11111 −+++=+ k
Tk SC)k|k(P)k(K
34
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProperties
• The Discrete KF provides an unbiased estimate of the state
– is an unbiased estimate of the state x(k+1), providing that the
initial conditions are
– Is this still true if the filter initial conditions are not the specified ?
11 ++ k|kx
000 x)|(x = 000 P)|(P =
35
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERSteady state Kalman Filter
• Time invariant system and stationay white system and observationnoise
• Filter dynamics
kkk
kkkvCxz
k ,GwAxx+=
≥+=+ 01
R]vv[E
Q]ww[ETkk
Tkk
=
=
TTTTT GQGA)k|k(CP]RC)k|k(CP[C)k|k(AP1)A-k|P(kAk)|1P(k +−+−−−=+ − 111 1
))k|k(xCz)(k(K)k|k(xA)k|k(x k 11111 1 +−+++=++ +
Discrete Riccati Equation
)k(K
36
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERSteady state Kalman Filter
• If Q is positive definite, is controllable, and (A,C) is
observable, then
– the steady state Kalman filter exists
– the limit exists
– is the unique, finite positive-semidefinite solution to the algebraic equation
– is independent of provided that
– the steady-state Kalman filter is assymptotically unbiased
)QG,A(
−∞∞→ =+ P)k|k(Plimk 1
−∞P
TT-T-T-T-- GQGAPC]RCPC[CPAAPAP ++−= ∞−
∞∞∞∞1
0P−∞P 00 ≥P
1−∞∞∞ += ]RCPC[CPK T-T-
37
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
MEANING OF THE COVARIANCE MATRIXGenerals on Gaussian pdf
• Let z be a Gaussian random vector of dimension n
• P - covariance matrix - symetric, positive defined• Probability density function
[ ] ( )( )[ ] PmzmzE,mzE T =−−=
( )( )
−−−
π= − )mz(Pmzexp
P det)z(p T
n1
21
2
1
n=1 n=2
38
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
MEANING OF THE COVARIANCE MATRIXGenerals on Gaussian pdf
• Locus of points where the fdp is greater or equal than a given threshold
n=1 line segment n=2 ellipse and inner pointsn=3 3D ellipsoid and inner points n>3 hiperellipsoid and inner points
• If
– the ellipsoid axis are aligned with the axis of the referencial where the vector z is defined
– length of the ellipse semi-axis =
) , , ,( diagP n22
221 σσσ= !
K)mz(P)mz( 1T ≤−− −
Kiσ
1K
)mz(K)mz(P)mz(n
1i 2i
2ii1T ≤∑
σ−⇔≤−−
=
−
39
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
MEANING OF THE COVARIANCE MATRIXGenerals on Gaussian pdf - Error elipsoid
Example n=2
σσσσ= 2
212
1221P
σσ= 2
2
210
0P
40
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
MEANING OF THE COVARIANCE MATRIXGenerals on Gaussian pdf -Error ellipsoid and axis orientation
• Error ellipsoid
• P=PT - to distinct eigenvalues correspond orthogonal eigenvectors
• Assuming that P is diagonalizable
• Error ellipoid (after coordinate transformation)
• At the new coordinate system, the ellipsoid axis are aligned with the axis of the new referencial
K )mz(P)mz( z1T
z ≤−− −
with1TDTP −= ),,,(diagD n21 λλλ= !
ITTT =
K)mw(D)mw(
K)mz(TTD)mz(
w1T
w
zT1T
z
≤−−
≤−−−
−
zTw T=
41
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
MEANING OF THE COVARIANCE MATRIXGenerals on Gaussian pdf -Error elipsis and referencial axis
• n=2[ ] K my
mx mymx
yx
y
xyx ≤
−−
σσ
−−−1
2
2
00
12
2
2
2
K
)my(
K)mx(
y
y
x
x ≤σ
−+
σ
−
xm
ym
x K σ
y K σ
ellipse
=
yx
z
42
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
MEANING OF THE COVARIANCE MATRIXGenerals on Gaussian pdf -Error ellipse and referencial axis
• n=2
σσρσ
σρσσ= 2
yyx
yx2xP [ ] K
yx
yxyyx
yxx ≤
σσρσ
σρσσ−1
2
2
12
22
1
21
Kw
Kw
≤λ
+λ
[ ][ ]2
y2x
222y
2x
2y
2x2
2y
2x
222y
2x
2y
2x1
4)(21
4)(21
σσρ+σ−σ−σ+σ=λ
σσρ+σ−σ+σ+σ=λ
1 K λλλλ2 K λλλλ
x
y
1w2w
α
2y
2x
2y
2x
yx1
,44
,2
tan 21
σ≠σπ≤α≤π−
σ−σ
σρσ=α −
=
yx
z
43
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProbabilistic interpretation of the error ellipsoid
• Given and it is possible to define the locus where, with a given probability, the values of the random vector x(k) ly.
))k|k(P),k|k(x( ~ )U,Z|x(p 1k0
k0k Ν−
)k|k(x )k|k(P
Hiperellipsoid with center in and with semi-axis proportional to the eigenvalues of
)k|k(x
)k|k(P
44
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProbabilistic interpretation of the error ellipsoid
))k|k(P),k|k(x( ~ )U,Z|x(p 1k0
k0k Ν−
• Example for n=2
)k|k(xkx
}K)]k|k(xx[)k|k(P)]k|k(xx[ :x{M kT
kk ≤−−= −1
}MxPr{ k ∈• is a function of K
• a pre-specified values of this probability can be obtained by an apropriate choice of K
45
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERProbabilistic interpretation of the error ellipsoid
• How to chose K for a desired probability?– Just consult a Chi square distribution table
))k|k(P),k|k(x( ~ )U,Z|x(p 1k0
k0k Ν−
nk Rx ∈
K)]k|k(xx[)k|k(P)]k|k(xx[ kT
k ≤−− −1
(Scalar) random variable with a distribution with n degrees of reedom
2χ
Probability = 90%n=1 K=2.71n=2 K=4.61
Probability = 95%n=1 K=3.84n=2 K=5.99
46
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERThe error ellipsoid and the filter dynamics
• Prediction cycle
kkk uB)k|k(xA)k|k(x +=+1Tkkk
Tkk GQGk)A|P(k Ak)|1P(k +=+
)k|k(P)k|k(P 1+
kQ
)k|k(x
)k|k(x 1+kx
ku
0
kw kkkkkkk wGuBxAx ++=+1
1+kx
47
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
DISCRETE KALMAN FILTERThe error ellipsoid and the filter dynamics
• Filtering cycle
kR
)k|k(P 1+
)k|k(x 1+
0
1111 ++++ += kkkk vxCz
1+kx
)k|k(xCk 11 ++
1+kz
1+kv
1+kr1+kr
)k(S 1+
)k|k(x 11 ++
1+kx
)k|k(P 11 ++
)k|k(xCzr kkk 1111 +−= +++
111 11 +++ ++=+ kTkk RC)K|K(PC)k(S
)k(r)k(K)k|k(x)k|k(x 11111 ++++=++
)k|k(PC)k(K)k|k(P)k|k(P k 11111 1 ++−+=++ +
48
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
Extended Kalman Filter
•• Non linearNon linear dynamics•• White GaussianWhite Gaussian system and observation noise
• QUESTION: Which is the MMSE (minimum mean-square error) estimate of x(k+1)?– Conditional mean– Due to the non-linearity of the system,
kkkk
kkkk1kv)x(hz
w)u,x(fx+=
+=+ )P,x(N~x 000
kjkTjk
kjkTjk
R]vv[E
Q]ww[E
δ=
δ=
? )U,Z|x(E)k|k(x kkk 0111 +=+
)U,Z|x(p kkk
101−
)U,Z|x(p kkk 011+
are non Gaussian
49
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
Extended Kalman Filter
•• (Optimal)(Optimal) ANSWER: The MMSE estimate is given by a non-linear filter, that propagates the conditonal pdf.
• The EKF EKF gives an approximation of the optimal estimate– The non-linearities are approximated by a linearized version of the non-linear
model around the last state estimate.– For this approximation to be valid, this linearization should be a good
approximation of the non-linear model in all the unceratinty domain associated with the state estimate.
50
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
Extended Kalman Filter
)U,Z|x(p kkk
101−
)k|k(x
)U,Z|x(p kkk 011+
kkkkk w)u,x(fx +=+1linearize
around
)k|k(x
)k|k(x 1+
)U,Z|x(p kkk 0
111+
+
Apply KF to the linear dynamics
linearize 1111 ++++ += kkkk v)x(hz
around )k|k(x 1+Apply KF to the linear dynamics
)k|k(x 11 ++
51
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
Extended Kalman Filter
)k|k(xkkkkk w)u,x(fx +=+1linearize
around
....)xx(f)u,x(f)u,x(f k|kkkkk|kkkkk +−∇+≅
)xf)u,x(f(wxfx k|kkkk|kkkkkk ∇−++∇≅+1
known inputPrediction cycle of KF
)xf)u,x(f(xfx k|kkkk|kkk|kkk|k ∇−+∇=+1
kT
kk Qf)k|k(Pf)k|k(P +∇∇=+1
52
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
Extended Kalman Filter
)k|k(x 1+linearize
around
....)xx(h)x(h)x(h k|kkkk|kkkk +−∇+≅ +++++++ 1111111
)xh)x(h(vxhz k|kkk|kkkkkk 1111111 +++++++ ∇−++∇≅
known inputUpdate cycle of KF
)]x(hz[)Rh)k|k(Ph(h)k|k(Pxx k|kkkkTkk
Tkk|kk|k 111
11111111 11 +++−
+++++++ −+∇+∇∇++=
)k|k(Ph]Rh)k|k(Ph[h)k|k(P)k|k(P)k|k(P kkTkk
Tk 111111 1
11111 +∇+∇+∇∇+−+=++ +−
++++
1111 ++++ += kkkk v)x(hz
53
M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto Superior Técnico
References
• Anderson, Moore, “Optimal Filtering”, Prentice-Hall, 1979.
• M. Athans, “Dynamic Stochastic Estimation, Prediction and Smoothing,” Series of Lectures, Spring 1999.
• E. W. Kamen, J. K. Su, “Introduction to Optimal Estimation”, Springer, 1999.
• Peter S. Maybeck, “The Kalman Filter: an Introduction to Concepts”
• Jerry M. Mendel, “Lessons in Digital Estimation Theory”, Prentice-Hall, 1987.
• M.Isabel Ribeiro, “Notas Dispersas sobre Filtragem de Kalman” CAPS, IST, June 1989 (http://www.isr.ist.utl.pt/~mir)