An Introduction To The Kalman Filter

By,Santhosh

Kumar

The Problem

System state cannot be measured directlyNeed to estimate “optimally” from measurements

Measuring Devices Estimato

r

MeasurementError Sources

System State (desired but not known)

External Controls

Observed Measurement

s

Optimal Estimate of

System State

SystemError Sources

System

Black Box

What is a Kalman Filter?

The Kalman Filter is essentially a set of mathematical equations that implement a predictor – corrector type estimator that is OPTIMAL – when some presumed conditions are met.

Optimal? For linear system and white Gaussian

errors, Kalman filter is “best” estimate based on all previous measurements

For non-linear system optimality is ‘qualified’

What’s so great about Kalman Filter?

noise smoothing (improve noisy measurements)

state estimation (for state feedback)recursive (computes next estimate using

only most recent measurement)

Discrete Kalman Filter

11 kkkk wBuAxx

kkk vHxz

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

with a measurement

Components of a Kalman Filter

1kw

Matrix (nxn) that relates the state at the previous time step k-1 to k without controls or noise.

A

Matrix (nxl) that describes how the control u changes the state from k-1 to k.B

Matrix (mxn) that describes how to map the state xk to a measurement zk.

H

1kv

Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance R and Q respectively.

Estimates and Errors

is the priori state estimate at step k.

is the posteriori state estimate at step k given measurement

Errors:

Error covariance matrices:

Kalman Filter’s task is to find

ˆ x k n

ˆ x k n

ek x k ˆ x k

ek x k ˆ x k

Pk E[ek

ek T

]

Pk E[ek ekT ]

kx̂

kz

Residual and Kalman Gain

Expected value

◦ innovation is

The optimal Kalman gain Kk is

kx̂

ˆ x k ˆ x k Kk(zk Hˆ x k

)

zk Hˆ x k

K k Pk HT (HPk

HT R) 1

Pk

HT

HPk HT R

Discrete Kalman Filter Algorithm

Prediction (Time Update)

(1) Project the state ahead

(2) Project the error covariance ahead

Correction (Measurement Update)

(1) Compute the Kalman Gain

(2) Update estimate with measurement zk

(3) Update Error Covariance

kkk BuA

1ˆˆ xx

QAAPP Tkk

1

1)( RHHPHPK Tk

Tkk

)ˆ(ˆˆ kkkkk HzK xxx

kkk PHKP )1(

Extended Kalman FilterSuppose the state-estimation and

measurement equations are non-linear:

◦ process noise w is drawn from N(0,Q), with covariance matrix Q.

◦ measurement noise v is drawn from N(0,R), with covariance matrix R.

),,( 11 kkkk f wuxx

),( kkk h vxz

Jacobian Matrix Recap

For a scalar function y=f(x),

For a vector function y=f(x),

y f (x)x

y Jx y1

yn

f1

x1

(x) f1

xn

(x)

fn

x1

(x) fn

xn

(x)

x1

xn

Linearize the Non-LinearThe equations that linearize a kalman estimate are

Where, and are actual state and measurement

vectors. and are approx. state and measurement

vectors. and are process and measurement noise.

(Cont.)

11

~

1

~

)( kkkkk WwxxAxx

kkkkk VvxxHzz )(~~

kx kz

kx~

kz~

kw kv

Linearize the Non-Linear(Cont.)

Let A be the Jacobian of f with respect to x.

Let W be the Jacobian of h with respect to w.

Let H be the Jacobian of h with respect to x.

Let V be the Jacobian of h with respect to v.

)0,,( 1

~

kk

j

iij x

fuxA

)0,(~

k

j

iij x

hxH

)0,(~

k

j

iij x

hV x

)0,,( 1

~

kk

j

iij w

fW ux

Extended Kalman Filter Algorithm

Prediction (Time Update)

(1) Project the state ahead

(2) Project the error covariance ahead

Correction (Measurement Update)

(1) Compute the Kalman Gain

(2) Update estimate with measurement zk

(3) Update Error Covariance

Tkkk

Tkkkk WQWAPAP 11

1)( Tkkk

Tkkk

Tkkk VRVHPHHPK

))0,ˆ((ˆˆ kkkkk hzK xxx

kkk PHKP )1(

)0,,( 1

^^

kkk fx ux

Quick Example – Constant Model

Measuring Devices Estimato

r

MeasurementError Sources

System State

External Controls

Observed Measurement

s

Optimal Estimate of

System State

SystemError Sources

System

Black Box

Quick Example – Constant Model

Time Update Equation

Measurement Update Equation

^

1

^

kk xx

QPP kk

1

1)( RPPK kkk

)(~^~^^

kkkkk xzKxx

kkk PKP )1(

Quick Example – Constant Model

Quick Example – Constant Model

Quick Example – Constant Model

QUERIES?????