ee 495 modern navigation systems kalman filtering – part i friday, march 28 ee 495 modern...

EE 495 Modern Navigation Systems

Kalman Filtering – Part I

Friday, March 28 EE 495 Modern Navigation Systems Slide 1 of 11

http://www.erau.edu/

Kalman Filtering – Part IBasic Estimation – Estimating a Fixed Constant

Friday, March 28 EE 495 Modern Navigation Systems

• CASE 1: A Fixed Constant Estimate an unknown constant

(x) given that we measure the truth + (white) noise

Simplest solution: o An non-finite memory averaging

filter

( ) ( )x k x w k

( ) ( 1) (1)ˆ( )

x k x k xx k

k

1 1ˆ( 1) ( )

kx k x k

k k

A recursive filter

0 1 2 3 4 5 6 7 8 9 10-20

-10

0

10

20

30

40

Time (sec)V

olta

ge (

V)

An example of an Averaging Filter

Measurement

TruthEstimate

10WhiteNoise V

( ) 12 ( )x k w k

50sF Hz

Slide 2 of 11


Kalman Filtering – Part IBasic Estimation – Estimating a Fixed Constant


• Conduct 50 Monte Carlo Runs: Theoretical improvement should be 1/ where N is the

number of samples in the average

0 1 2 3 4 5 6 7 8 9 10

-5

0

5

10

15

20

25

30

An Example of an Averaging Filter (50 runs)

x hat (

V)

Time (sec)0 1 2 3 4 5 6 7 8 9 10

0

1

2

3

4

5

6

7

8

9

10Standard Deviation of the Estimate vs Time

xh

at (

deg)

Time (sec)

Computed

Idealx̂

ˆ( , )x tˆ /

10 /

x WhiteNoise N

N

( , ) 12 ( , )x k w k

Slide 3 of 11


Kalman Filtering – Part IBasic Estimation – Estimating a Time Varying Quantity


• CASE 2: A Slowly Time Varying Quantity Estimate a time varying quantity (x) given that we measure

the truth + (white) noise

Simplest solution: o A fading memory filter

(i.e., ~ fixed memory length)

o where

( )( ) ( )kx k x w k

ˆ ˆ( ) ( 1) (1 ) ( )x k x k x k A recursive filter1MemLen

MemLen

x is no longer a constant!!

Slide 4 of 11


Kalman Filtering – Part IBasic Estimation – Estimating a Time Varying Quantity


• CASE 2: A Slowly Time Varying Quantity A simulation example

( ) ( ) ( )x k x k w k

0 1 2 3 4 5 6 7 8 9 10-40

-30

-20

-10

0

10

20

30

40

Time (sec)

Vol

tage

(V

)

An example of an Fading Memory Filter

Meas

TruthFading

Ave

0.3( ) 20 (2 0.2 )tx k e Cos k

9 1ˆ ˆ( ) ( ) ( )

10 10x k x k x k

100sF Hz

( ) [0,10]w k N

Essentially a low-pass filter

Slide 5 of 11

Mem Len = 10 1MemLen

MemLen

ˆ ˆ( ) ( ) (1 ) ( )x k x k x k


Kalman Filtering – Part IBeyond Basic Estimation


• Can we do better?

• What if we know something about the noise levels in the measurement? e.g., the standard deviation of the noise in the measurement

o Maybe more => A Gauss-Markov model with correlation time?

• What if we know something about how the quantity we are estimating evolves over time? e.g., dynamic model of a object being tracked

A Kalman Filter can use all of this type of information (and more)!!

Slide 6 of 11




• Estimate a quantity which is not typically constant and furthermore the noise levels of each measurement may vary? Let the measurements be described as

• When we have only the first measurements () our “best” estimate is:

k k kz x w

1 1x̂ z

Slide 7 of 11




• When the second measurement arrives we now have two measurements ( & ) A weighted least-square estimate (=N[0,] & =N[0,] )

1 22 2 2 2 2

1 2 1 2

1 1ˆ

z zx

2 2 2 22 1 1 2 1 2z z

2 2 21 1 1 2 2 1[ ]z z z

2 1 2 2 1ˆ ˆ ˆ[ ]x x K z x

2 2 21 1 1 2 2 1ˆ ˆ[ ]x z x

A recursive filter

Slide 8 of 11


Kalman Filtering – Part IBeyond Basic Estimation - The Kalman Filter


• The measurement model We measure a subset of the vector which we want to estimate

where the white additive measurement noiseo is the measurement noise covariance matrix

• The state model The quantity that we wish to estimate () evolves over time as

where the white additive state noiseo is the state noise covariance matrix

k k kz H x v

[0, ]k kw N Q

1k k kx A x w

[0, ]k kv N R

Slide 9 of 11


Kalman Filtering – Part IBeyond Basic Estimation - The Kalman Filter


• The state error-covariance matrix We will produce an estimate of the state-vector with an

associated error

The covariance matrix of this error is defined as

ˆk k ke x x

ˆ ˆ

T

k k k

T

k k k k

P E e e

E x x x x

Slide 10 of 11


Kalman Filtering – Part IBeyond Basic Estimation – The Kalman Filter Algorithm


Step 1: Prediction

1ˆ ˆk kx A x

1T

k k kP AP A Q

Step 2: Gain Calculation1( )T T

k k k kK P H H P H R

Step 3: Updateˆ ˆ ˆ( )k k k k kx x K z H x

( )k k kP I K H P

Step 0: Initialize

0 0ˆ ,x P

Slide 11 of 11


ee 495 modern navigation systems kalman filtering – part i friday, march 28 ee 495 modern...

Documents