Estimation and the Kalman Filter

David Johnson

The Mean of a Discrete Distribution

• “I have more legs than average”

Gaussian Definition

-

Univariate

Multivariate

2

2)(

2

1

2

2

1)(

:),(~)(

x

exp

Nxp

)()(2

1

2/12/

1

)2(

1)(

:)(~)(

μxΣμx

Σx

Σμx

t

ep

,Νp

d

Back to the non-evolving case

• Two different processes measure the same thing

• Want to combine into one better measurement

• Estimation

Estimation

What is meant by estimation?

Data + noise

Data + noise

Data + noise

Estimator Estimation

Hz ŷStochastic process estimate

A Least-Squares Approach

• We want to fuse these measurements to obtain a new estimate for the range

• Using a weighted least-squares approach, the resulting sum of squares error will be

• Minimizing this error with respect to yields

222

111

),0(

),0(

vrRNrz

vrRNrz

n

iii zrwe

1

2)ˆ(

r̂

r̂

0)ˆ(2)ˆ(ˆˆ 11

2

n

iii

n

iii zrwzrw

rr

e

A Least-Squares Approach• Rearranging we have

• If we choose the weight to be

we obtain

0ˆ11

n

iii

n

ii zwrw

n

ii

n

iii

w

zwr

1

1ˆ

iii Rw

112

221

11

21

2

21

2

2

1

1

11ˆ z

RR

Rz

RR

R

RR

Rz

Rz

r

• For merging Gaussian distributions, the update rule is

A Least-Squares Approach

22

21

22

212

322

21

22

21

22

21

23

111

Show for N(0,a) N(0,b)

What happens when you move?

),(~),(~ 22

2

abaNYbaXY

NX

derive

Moving

• As you move– Uncertainty grows– Need to make new measurements– Combine measurements using Kalman gain

The Kalman Filter

“an optimal recursive data processing algorithm”

OPTIMAL:-Linear dynamics-Measurements linear w/r to state-Errors in sensors and dynamics must be zero-mean (un-bias) white Gaussian

RECURSIVE:-Does not require all previous data-Incoming measurements ‘modify’ current estimate

DATA PROCESSING ALGORITHM:The Kalman filter is essentially a technique of estimation given a system model and concurrent measurements

(not a function of frequency)

The Discrete Kalman Filter

Estimate the state of a discrete-time controlled process that is governed by the linear stochastic difference equation:

with a measurement:

The random variables wk and vk represent the process and measurement noise (respectively). They are assumed to be independent (of each other), white, and with normal probability distributions

In practice, the process noise covariance and measurement noise covariancematrices might change with each time step or measurement.

(PDFs)

The Discrete Kalman Filter

First part – model forecast: prediction

“prior” estimate

Process noisecovariance

Statetransition

Stateprediction

Error covarianceprediction

Controlsignal

Prediction is based only the model of the system dynamics.

The Discrete Kalman Filter

Second part – measurement update: correction

“posterior” estimate

statecorrection

“prior” stateprediction

Kalmangain

actualmeasurement

predictedmeasurement

update error covariance matrix (posterior)

The Discrete Kalman Filter

The Kalman gain, K: “Do I trust my model or measurements?”

RHPH

HPK

Tk

Tk

k

variance of the predicted states= ------------------------------------------------------------

variance of the predicted + measured states

measurement sensitivity matrix

measurementnoise covariance

As measurement error covariance, R, approaches zero, the actual measurement, zk is “trusted” more and more. is trusted less and less

But, as the “prior” (predicted) estimate error covariance, P, approaches zero, the actual measurement is trusted less, and predicted measurement, is trusted more and more

Estimate a constant voltage

• Measurements have noise

• Update step is

• Measurement step is

Results

Variance

Parameter tuning

More tuning

• The Extended Kalman (EKF) is a sub-optimal extension of the original KF algorithm• The EKF allows for estimation of non-linear processes or measurement relationships• This is accomplished by linearizing the current mean and covariance estimates

(similar to a first order Taylor series approximation)• Suppose our process and measurement equations are the non-linear functions

The Extended Kalman Filter (EKF)

),(

),,(1

kkk

kkkk

vxhz

wuxfx

kkk

kkkk

vxHz

wuBxAx

1

Kalman Filter Extended Kalman Filter

Linearity Assumption Revisited

Non-linear Function

EKF Linearization (1)

EKF Linearization (2)

EKF Linearization (3)