Introduction to Kalman Filtering - Institute For Systems ...users.isr.ist.utl.pt/~mir/pub/Kalman-Filter.pdf · 6 M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto

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


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


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


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



• 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



• 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


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


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


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


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


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


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



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



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



• 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



.

.

.

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


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


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



• 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



• 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


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


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


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


DISCRETE KALMAN FILTERFilter dynamics - 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


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



• 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



• 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


DISCRETE KALMAN FILTERDynamics

• Linear System

• Discrete Kalman Filter

kkkk

kkkkkkkvxCz

k ,wGuBxAx+=

≥++=+ 01

kkk uB)k|k(xA)k|k(x +=+1Tkkk


))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


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



• 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



• 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



• 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



• 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


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


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


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


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


MEANING OF THE COVARIANCE MATRIXGenerals on Gaussian pdf - Error elipsoid

Example n=2

σσσσ= 2

212

1221P

σσ= 2

2

210

0P

40


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


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


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


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



))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



• 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


DISCRETE KALMAN FILTERThe error ellipsoid and the filter dynamics


kkk uB)k|k(xA)k|k(x +=+1Tkkk


)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


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


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



•• (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



)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



)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



)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


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)

Documents

Introduction to Kalman Filtering - Institute For Systems ...users.isr.ist.utl.pt/~mir/pub/Kalman-Filter.pdf · 6 M.Isabel Ribeiro - June.2000 Instituto de Sistemas e Robótica/Instituto