Kalman FilteringKalman Filtering

COS 323COS 323

On-Line EstimationOn-Line Estimation

• Have looked at “off-line” model estimation:Have looked at “off-line” model estimation:all data is availableall data is available

• For many applications, want best estimate For many applications, want best estimate immediately when each new datapoint arrivesimmediately when each new datapoint arrives– Take advantage of noise reductionTake advantage of noise reduction

– Predict (extrapolate) based on modelPredict (extrapolate) based on model

– Applications: controllers, tracking, …Applications: controllers, tracking, …

• How to do this without storing all data points?How to do this without storing all data points?

Kalman FilteringKalman Filtering

• Assume that results of experimentAssume that results of experimentare are noisynoisy measurements of measurements of“system state”“system state”

• Model of how system evolvesModel of how system evolves

• Optimal combinationOptimal combinationof system model and observationsof system model and observations

• Prediction / correction frameworkPrediction / correction frameworkRudolf Emil KalmanRudolf Emil Kalman

Acknowledgment: much of the following material is Acknowledgment: much of the following material is based on thebased on theSIGGRAPH 2001 course by Greg Welch and Gary SIGGRAPH 2001 course by Greg Welch and Gary Bishop (UNC)Bishop (UNC)

Simple ExampleSimple Example

• Measurement of a single point zMeasurement of a single point z11

• Variance Variance 1122 (uncertainty (uncertainty 11))

• Best estimate of true position Best estimate of true position

• Uncertainty in best estimateUncertainty in best estimate

11̂ zx 11̂ zx 22

1 1ˆ 22

1 1ˆ

Simple ExampleSimple Example

• Second measurement zSecond measurement z22, variance , variance 2222

• Best estimate of true position?Best estimate of true position?

zz11 zz22

Simple ExampleSimple Example

• Second measurement zSecond measurement z22, variance , variance 2222

• Best estimate of true position: weighted Best estimate of true position: weighted average average

• Uncertainty in best estimateUncertainty in best estimate

121

11

21

11

2

ˆˆ

ˆ

22

21

21

22

21

22

21

xzx

zzx

121

11

21

11

2

ˆˆ

ˆ

22

21

21

22

21

22

21

xzx

zzx

22

21

1ˆ1

22

1ˆ

22

21

1ˆ1

22

1ˆ

Online Weighted AverageOnline Weighted Average

• Combine successive measurements into Combine successive measurements into constantly-improving estimateconstantly-improving estimate

• Uncertainty usually decreases over timeUncertainty usually decreases over time

• Only need to keep current Only need to keep current measurement,measurement,last estimate of state and uncertaintylast estimate of state and uncertainty

TerminologyTerminology

• In this example, position is In this example, position is statestate(in general, any vector)(in general, any vector)

• State can be assumed to evolve over time State can be assumed to evolve over time according to a according to a system modelsystem model or or process process modelmodel(in this example, “nothing changes”)(in this example, “nothing changes”)

• Measurements (possibly incomplete, possibly Measurements (possibly incomplete, possibly noisy) according to a noisy) according to a measurement modelmeasurement model

• Best estimate of state with covariance Best estimate of state with covariance PPx̂x̂

Linear ModelsLinear Models

• For “standard” Kalman filtering, For “standard” Kalman filtering, everythingeverythingmust be linearmust be linear

• System model:System model:

• The matrix The matrix kk is is state transition matrixstate transition matrix

• The vector The vector kk represents represents additive noiseadditive noise,,

assumed to have covariance assumed to have covariance QQ

111 kkkk xx 111 kkkk xx

Linear ModelsLinear Models

• Measurement model:Measurement model:

• Matrix Matrix HH is is measurement matrixmeasurement matrix

• The vector The vector is is measurement noisemeasurement noise,,assumed to have covariance assumed to have covariance RR

kkkk xHz kkkk xHz

PV ModelPV Model

• Suppose we wish to incorporate velocitySuppose we wish to incorporate velocity

01

10

1

H

t

x

kk

dtdxkx

01

10

1

H

t

x

kk

dtdxkx

Prediction/CorrectionPrediction/Correction

• Multiple values around at each iteration: Multiple values around at each iteration: – is prediction of new state on the basis of is prediction of new state on the basis of

past datapast data

– is predicted observationis predicted observation

– is new observationis new observation

– is new estimate of stateis new estimate of statek

k

k

k

x

z

z

x

ˆ

k

k

k

k

x

z

z

x

ˆ

Prediction/CorrectionPrediction/Correction

• Predict new statePredict new state

• Correct to take new measurements into Correct to take new measurements into accountaccount

kkk

kkkkk

kkk

xHz

QPP

xx

1T

111

11 ˆ

kkk

kkkkk

kkk

xHz

QPP

xx

1T

111

11 ˆ

kkkk

kkkkkk

PHKIP

xHzKxx

ˆ kkkk

kkkkkk

PHKIP

xHzKxx

ˆ

Kalman GainKalman Gain

• Weighting of process model vs. Weighting of process model vs. measurementsmeasurements

• Compare to what we saw earlier:Compare to what we saw earlier:

1TT kkkkkkk RHPHHPK 1TT kkkkkkk RHPHHPK

22

21

21

2

221

21

Results: Position-Only ModelResults: Position-Only Model

MovingMoving StillStill

[Welch & Bishop][Welch & Bishop]

Results: Position-Velocity ModelResults: Position-Velocity Model

[Welch & Bishop][Welch & Bishop]

MovingMoving StillStill

Extension: Multiple ModelsExtension: Multiple Models

• Simultaneously run many KFs with Simultaneously run many KFs with different system modelsdifferent system models

• Estimate probability each KF is correctEstimate probability each KF is correct

• Final estimate: weighted averageFinal estimate: weighted average

Probability EstimationProbability Estimation

• Given some Kalman filter, the Given some Kalman filter, the probability of a measurement zprobability of a measurement zkk is just is just

nn-dimensional Gaussian-dimensional Gaussian

wherewhere

T1T

21

2||2

1 kkkkkkn

xHzCxHzeC

p

T1T21

2||2

1 kkkkkkn

xHzCxHzeC

p

RHPHC T RHPHC T

Results: Multiple ModelsResults: Multiple Models

[Welch & Bishop][Welch & Bishop]

Results: Multiple ModelsResults: Multiple Models

[Welch & Bishop][Welch & Bishop]

Results: Multiple ModelsResults: Multiple Models

[Welch & Bishop][Welch & Bishop]

Extension: SCAATExtension: SCAAT

• HH can be different at different time steps can be different at different time steps– Different sensors, types of measurementsDifferent sensors, types of measurements

– Sometimes measure only part of stateSometimes measure only part of state

• Single Constraint At A Time (SCAAT)Single Constraint At A Time (SCAAT)– Incorporate results from one sensor at onceIncorporate results from one sensor at once

– Alternative: wait until you have measurements Alternative: wait until you have measurements from enough sensors to know complete state from enough sensors to know complete state (MCAAT)(MCAAT)

– MCAAT equations often more complex, but MCAAT equations often more complex, but sometimes necessary for initializationsometimes necessary for initialization

UNC HiBallUNC HiBall

• 6 cameras, looking at LEDs on ceiling6 cameras, looking at LEDs on ceiling

• LEDs flash over timeLEDs flash over time

[Welch & Bishop][Welch & Bishop]

Extension: Nonlinearity (EKF)Extension: Nonlinearity (EKF)

• HiBall state model has nonlinear degrees of HiBall state model has nonlinear degrees of freedom (rotations)freedom (rotations)

• Extended Kalman Filter allows Extended Kalman Filter allows nonlinearities by:nonlinearities by:– Using general functions instead of matricesUsing general functions instead of matrices

– Linearizing functions to project forwardLinearizing functions to project forward

– Like 1Like 1stst order Taylor series expansion order Taylor series expansion

– Only have to evaluate Jacobians (partial Only have to evaluate Jacobians (partial derivatives), not invert process/measurement derivatives), not invert process/measurement functionsfunctions

Other ExtensionsOther Extensions

• On-line noise estimationOn-line noise estimation

• Using known system input (e.g. Using known system input (e.g. actuators)actuators)

• Using information from both past and Using information from both past and futurefuture

• Non-Gaussian noise and particle Non-Gaussian noise and particle filteringfiltering