Sensor FusionThe Kalman Filter and its

Extensions

JASS 2005Katharina Pentenrieder

Introduction

TaskFusion of different data sets

ApproachesSimple

Stochastic approach No perfect model

Disturbances

Imperfect or incomplete data

Introduction

Kalman FilterOptimal linear recursive estimator

Incorporates all available information Knowledge about system and measurement device

Statistical description of noise and error

Initial conditions

“prediction-correction-cycle” Time update

Measurement update

Simple, robust and popular

Overview

Stochastic Basics

Discrete Kalman Filter

Extended Kalman Filter

Sensor FusionDKF and EKF

SCAAT

FKF

Conclusion

Stochastic Basics

Probability and Random Variables Probability

Random Variable X: Sample Space → Numbers

Cumm. distribution function

Probability density function

Mean and Covariance Mean

(discrete, continuous)

Variance

Std. deviation

outcomes possible of number total

A favoringoutcomes possibleAp =)(

xFx

xfXX

)()(!

!=

],()( xpxFX !"=

!=b

a

XX dxxfbap )(],[

)(XVarX=!

222 ][][]])[[()( XEXEXEXEXVar !=!=

!"#

#$=

== dxxxfXE xpXE X

n

i

ii )(][][1

Stochastic Basics

Gaussian distribution Popular for modelling random systems

Normally distributed

Probability density function

White noise Autocorrelation

White noise is uncorrelated, independent

2

2

2

)(

22

1)( !

µ

"!

##

=

x

Xexf

),(~ !µNX

)]()([)( !! += tXtXERX

!"# =

=se , otherwi

a , if ôRX

0

0)($

The Discrete Kalman Filter

Process and Measurement Models Models

Noise

Origins of the Filter state estimates, errors and covariances

Computational origin

Probabilistic origin

kkk

kkkk

vHxz

wBuAxx

+=

++= !! 11

),0(~)(

),0(~)(

RNvp

QNwp

][][ˆˆˆˆ T

kk

-T

k

-

kkkkkkk

-

kkk keeEP , e e EP xxe , xxe x , x ==!=!=

!!!

)ˆ(ˆˆ !!!+=

kkkkxHzKxx

),ˆ(~)|( kkkk PxNzxp

The Discrete Kalman Filter

Discrete Kalman Filter Cycle Time update

Measurement update

Influence of Q and R Process noise covariance Q:

large → close track of changes in data

Measurement noise covariance R:large → measurements are considered not very accurate

QAAPP

BuxAx

T

kk

kkk

+=

+=

!

!

!

!

1

1ˆˆ

!

!!

!!!

!=

!+=

+=

kkk

kkkkk

T

k

T

kk

PHKIP

xHzKxx

RHHPHPK

)(

)ˆ(ˆˆ

)( 1

The Discrete Kalman Filter

Influence of Q and R

Q small, R large Q large, R small

The Discrete Kalman Filter

AssumptionsAll underlying models are linear

Often adequate

More complete theory

Gaussian probability distribution “natural”

Completely determined by μ and σWhite (independent) noise

Identical to wideband noise in bandpass

Mathematics are vastly simplified

The Discrete Kalman Filter

Optimality Filter minimizes the estimated error covariance

Based on computation of Kalman gain K

diagonal of P contains mean squared errors → minimize trace

])ˆ)(ˆ[(][ T

kkkk

T

kkkxxxxEeeEP !!==

T

kkk

T

kkk

T

k

T

kkk

T

k

T

kkk

T

kkkkkkkkkkk

kkkkkkkkkk

KRKHKIPHKI

KvvEKHKIeeEHKI

vKxxHKIvKxxHKIEP

xHvHxKxxHzKxx

+!!=

+!!=

!!!!!!="

!++=!+=

!

!!

!!

!!!!

)()(

][)]([)(

]))ˆ)()(()ˆ)([((

)ˆ)((ˆ)ˆ(ˆˆ

RHHP

HPK RHHPKHP

K

]T[P

T

k

T

kT

kk

T

k

k

k

+=!=++"=

#

#"

"

"" 0)(2)(2

The Discrete Kalman Filter

Examples 1D voltage measurement

Models

Noise covariances

Measurementsmean m →

Resultsred=measurementsgreen=predicted states

kkkkxz xx == !1

251010

!!== R Q

)1.0,(~ mNz

The Discrete Kalman Filter

Examples 3D position measurement

State vector

Models

Filter cycle Compute Δt since previous estimate

Compute state transition matrix A(Δt)

Do the prediction and correction steps

Determination of Q and R

T)z,y,x,z,y,xz,y,(x,x &&&&&&&&&=

vtHxttz

z

z

z

wtxtAttx

z

y

x

+=!+=

"""

#

$

%%%

&

'

+!=!+

)()(

)()()(

!!!!!

"

#

$$$$$

%

&

'(

'(

'(

=

I

ItI

It

ItI

A

00

0

2

2

!!!

"

#

$$$

%

&

=

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

0

1

0

0

0

1

0

0

0

1

H

The Extended Kalman Filter

Non-Linearity Assumptions of the DKF do not always hold

EKF linearizes about the current mean and covariance

EKF Models Non-linear equations

Noise values unknown

Linearization

Jacobians

),(),,( 111 kkkkkkkvxhz wuxfx == !!!

)0,~(~)0,,ˆ(~11 kkkkk

xhz uxfx == !!

kkkkkkkkkkVvxxHzz WwxxAxx +!+"+!+" !! )~(~)ˆ(~

11

)0,~()0,~(

)0,,ˆ()0,,ˆ(

kkkk

kkkkkk

xv

hV x

x

hH

uxw

fW ux

x

fA

!

!=

!

!=

!

!=

!

!=

The Extended Kalman Filter

Extended Kalman Filter Cycle Time update

Measurement update

T

kkk

T

kkkk

kkk

WQWAPAP

uxfx

11

1 )0,,ˆ(ˆ

!!

!

!

!

+=

=

!

!!

!!!

!=

!+=

+=

kkk

kkkkk

T

kkk

T

kkk

T

kkk

PHKIP

xhzKxx

VRVHPHHPK

)(

))0,ˆ((ˆˆ

)( 1

The Extended Kalman Filter

Example 3D position and orientation tracking with quaternions

State vector

Models

Filter cycle: equations as presentedJacobians need to be computed

TTTTTTT

T

wzyx

rpppx

),r,r,r,r,z,y,x,z,y,xz,y,(x,x

),,,,,(

,,,,, 321321

!!

!!!!!!

&&&&

&&&&&&&&&&&&

=

=

!!"

#$$%

&==

!!!

"

#

$$$

%

&

'''''

(

)

*****

+

,

-.

-.

-.

=

!!!

"

#

$$$

%

&

/

/

/

)()(

00

02

1

1

1

2

rnormalize

pxhz

p

p

p

I

ItI

It

ItI

p

p

p

k

k

k

k

k

k

k

&&

&

&&

& erdrrkk tt

kkkk

1

2

12

1

111

!! "#+"#

!!! $=$=%% &

Kalman Filter Discussion

Kalman Filterstable, robust and popular optimal estimator

DKF(+) optimal linear estimator

applicable to many system processes

(-) three assumptions

EKF(+) faces non-linearity problem

(-) unreliable for non Gaussian distributions

Sensor Fusion with KFs

Discrete and Extended Kalman FilterOne Filter

Multiple sensors summed up in a single filter

Updates when enough information is gathered

→ UNC hybrid landmark-magnetic tracker

Separate Filters Separate filters for each sensor

Optimal adjusting

→ Azuma: head location prediction

Sensor Fusion with KFs

Single Constraint at a Time (SCAAT) Introduction

Multiple seq. measurements for a single update

Problems Simultaneity assumption

System depends on sufficient data sets

SCAAT idea Single-constraint-at-a-time

Each measurement provides some information aboutthe current state

Incremental improvement of previous estimates

Sensor Fusion with KFs

Single Constraint at a Time (SCAAT) State vector and models

State vector

Process model

Measurement model

for each sensors σ a corresponding measurement vectorb and c are tracker source and sensor parameters

Ideal SCAAT applicationonly a single source and sensor pair for each update

])[,),(()(

],)[,),((

)(),),(()(

icbtxhjx

jicbtxH

tvcbtxhtz

tttt

tt

!!

!!!

"

"=

+=

)()(

)()()(:)(

),,,,,,,,,,,(

txttx

ttxtxttx tA

zyxzyxx T

&&

&

&&&&&&

=!+

!"+=!+!

= #$%#$%

1|| =!z

Sensor Fusion with KFs

Single Constraint at a Time (SCAAT) Algorithm

Compute Δt since previous estimate

Predict state and error covariance

Predict measurement and compute Jacobian

Compute Kalman gain

Correct state estimate and error covariance

Discussion SCAAT integrates individual (incomplete) measurements

Faster, more accurate, no simultaneity assumption

),,ˆ(),,ˆ(ˆtttt

cbxHH cbxhz!!

== ""

1))(( !!!+= tRHHPHPK

TT

"

!!!=!+= PKHItP ztzKxtx )()()ˆ)((ˆ)(ˆ "

Sensor Fusion with KFs

The Federated Kalman Filter Introduction

Computational load problems inmultisensor systems

Decentralization and reducedrate at master filter

FKF idea Decentr. approach with local filter

and a master filter

Local data compression throughpre-filtering

Optimal or suboptimal accuracyvia selectable master filter rate

Sensor Fusion with KFs

The Federated Kalman Filter Filter Structure

Models

Composite global filter

Global cost index

Globally optimal solution if local estimates are uncorrelated

Elimination of cross-correlations through upper bounds forcovariances Q and P: as bounding variable

iiikkvxHz GwAxx +=+= !

ˆ1

!!!

"

#

$$$

%

&

==

NNN

N

T

N

P P

P P

P xxx

...

...

...

]...[

1

111

1

!=

""=N

i

Piiii

xx

1

21||)ˆ(||#

111

11

1

1

1

11

]...[

]ˆ...ˆ[ˆ

!!!

!!

++=

++=

NNmm

NNNmmm

PPP

xPxPPx

i!

Sensor Fusion with KFs

The Federated Kalman Filter Algorithm

Set initial local covariances to x common system value

Local filters process own measurements via locally optimal KF

Master filter combines local filter solutions after each cycleupdate via the equations

Master filter resets local filter states to master value and localcovariances to x master value

Discussion Highly fault tolerant, rate-reduced, decentralized filtering

approach

i!

i!

111

11

1

1

1

11

]...[

]ˆ...ˆ[ˆ

!!!

!!

++=

++=

NNmm

NNNmmm

PPP

xPxPPx

Conclusion

Kalman Filter DKF: optimal linear estimator with three assumptions EKF: faces non-linear models, linearizes about μ and σ

Sensor Fusion KF - Direct fusion: easy and common KF - Separate filters: faces complexity, ignores

possible correlations SCAAT: integrates single measurements, more

accurate and faster FKF: decentralized system with pre-filtering, high fault

tolerance and globally optimal/suboptimal estimation

References

G. Bishop, G. Welch: “An Introduction to the Kalman Filter”,SIGGRAPH 2001 Course Notes

P. S. Maybeck: “Stochastic Models, Estimation and Control”,Academic Press NY 1979

G. Bishop, G. Welch: “SCAAT: Incremental Tracking withIncomplete Information”, SIGGRAPH 1997

N. A. Carlson: “Federated Square Root Filter for DecentralizedParallel Processing”, IEEE Transactions on Aerospace andElectronic Systems 1990

D. Pustka: “Handling Errors is Ubiquitous Tracking Setups”, DiplomaThesis TU Munich 2003