Sensor Fusion Systems Overview and Mathematics Bjoern Griesbach [email protected]

Sensor Fusion Systems

Overview and Mathematics

Bjoern [email protected]

Sensor Fusion Systems[Content]

1. Introduction1. Motivation2. Different tracking options

2. Existing Multi Sensor Fusion Systems1. Fusion of head mounted and fixed sensor data2. Fusion of magnetic & optical sensor data3. Fusion of gyroscope & optical sensor data4. Open Tracker – an open source AR software

3. Mathematics of Sensor Fusion1. Kalman Filter2. Particle Filter

Motivation

• Each sensor has its strengths and weaknesses• One sensor is never sufficient for reliable

tracking• Optimal tracking = use multiple sensors

Sensor

Data Fusion Component

Precise Estimation

Sensor Sensor

NoisyData

NoisyData

NoisyData

Motivation

• Augmented Reality

• Virtual Reality

• Mobile Robots

Multi Sensor Fusion used in various fields of research:

Different Tracking Options

• Magnetic Tracking– reliable– stable– fast

• Optical Tracking– precise– time consuming

• Gyroscope– precise– drift error

• Fixed Trackers– no limit in size, weight– highly precise (e.g. stereo

vision) in tracking objects– bad for head orientation

• Mobile Trackers (i.e. Head Mounted Tracker)– good for head orientation– limited in size & weight– less precise in tracking

objects

Technology Location

Content




Fusion of Data from Head Mounted and Fixed Sensors

• Two optical trackers:– Mobile (Head Mounted)– Fixed

• Wanted: Fusing data of fixed and mobile tracker: Hybrid inside-out & outside-in approach in order to

• Estimate pose of a certain object (for example a head’s pose)

How to realize?

Fusion of Data from Head-Mounted and Fixed Sensors

• Pose of an object is represented as a vector =(x,y,z,α,β,γ);

• By transforming poses of two different sensors into the same coordinate system, one will get two (noisy) measurements and for the same object

• Each measurement is weighted differently depends on the variance of the measurement

• Each measurement has its own variance σi2

represented by a matrix Pi

x

1z

2z

iz

iz


• Given: pose measurements: z1 z2 with covariance matrices P1 P2 from two different sensors

• Wanted: optimal weights to get optimal estimate

• Solution: Optimal estimate x with minimal combined covariance matrix P:

;

;

121

2

221

11

21

2

PPP

PP

zPP

Pz

PP

Px

• Already a simple form of the Kalman Filter - Remember!


Experiment by W. Hoff, First International Workshop on ARSan Francisco

Result: max. translational error was reduced by 90%

Content




Fusion of Magnetic and Optical Trackers

• E.g. Studierstube in Vienna:– HMD with magnetic tracker & stereo camera system– estimating pose by Landmark Tracking– pose estimation from magnetic tracker is used to

predict feature locations in the image– optical tracking system can thus work with small

search areas

• Result: Features of the entire system– more precise than an magnetic tracker– faster and more reliable than an optical tracker

Fusion of Magnetic and Optical Trackers

• Landmark Predictor:– keeps track of potentially

detectable landmarks and sorts them

– improves head pose after each newly found landmark

– tells IA where to search for landmarks

• Image Analyzer:– inspects search area

defined by LP

Content




Fusion of Gyroscope and Optical Tracker data

• Situation:– Gyroscope (head mounted)– Optical tracker (head mounted)

• Problem to solve: Gyroscope serves highly precise head orientation data but with a drift error

• Solution:Vision based drift compensation algorithm

Content




Open Tracker: An open Software Framework for AR

Most implementations of AR Systems were not portable solutions.

Reason: data flow in AR Systems is each time implemented specifically for this solution

Need for a standard which handles data flow in AR Systems

Open Tracker• Open source framework• Configurable via XML• Object oriented• Uses a directional graph

to describe data flow

Eases setting up an AR environment

(e.g. distributed, several trackers, etc.)

Open Tracker: Example

Optical Tracker[Mobile]

Optical Tracker[Fixed]

FusionFilter

Multicast Information to multiple users on a network

Magnetic Tracker[Mobile]

TransformationFilter

Sink node

Filter node

Source node

Console

NoiseFilter

Content




Kalman Filter

Step by step introduction:

1. Static KF2. Basic KF3. Extended KF4. Sensor Fusion with the KF

Kalman Filter

Optimal data processing algorithm• Major use: filter out noise of measurement data (but

can also be applied to other fields, e.g. Sensor Fusion)• Result: Computes an optimal estimation of the state of

an observed system based on measurements

• Recursive• Optimal: incorporates all information (i.e. measurement

data) that can be provided to it• Does not need to keep all previous measurement data in

storage!

Conventions:

We observe a system with:x : state of the system

z : measurement (approximates x)

σ2 : variance of a measurement

: vector

P : covariance matrix

: best estimate of state

: best estimate before measurement was taken

Kalman Filter

x

x̂x̂

Introduction Kalman Filter

Assumptions: • Two scalar sensor measurements z1 and z2

• Gaussian noise, i.e. zi ~ N(0, σi2 )

Optimal state estimate:

21

2

121

)1(

);(ˆ

K

zzKzx

;22

21

21

KKalman Gain


• Let’s incorporate time!• Measurements z1 and z2 were taken sequentially

z(t1), z(t2)

Optimal state estimate at time t2 :

2

)(2

1212

11

)()()()(ˆ

tzK

tztzKtztx

;2

)(2

)(

2)(

21

1

tztz

tzK

Kalman Gain


• Let’s take more measurements (time continues) • Incorporate previous knowledge (last estimate)

Optimal state estimate at time tk :

)(1)(

)(ˆ)()(ˆ)(ˆ

122

11

kk

kkkk

tKt

txtzKtxtx

;)(

)(2

)(12

12

ktzk

k

t

tK

Kalman Gain


Static Kalman Filter:


• Previous slides: static state• Now: Dynamic state x (stochastic process)• Idea: Use knowledge about process x in

addition to measurements to obtain best estimate:


• Previous slides: static state• Now: Dynamic state x (stochastic process)• Idea: Use knowledge about process x in

addition to measurements to obtain best estimate:

• Process Model: (example)x(tk)=x(tk-1)+u+w Noise: w ~ N(0, σw

2 )

• Measurement Model:z(tk)=x(tk)+v Noise: v ~ N(0, σz

2 )


• Process & Measurement Model

x(tk)x(tk-1) x(tk+1)

Measurements(observed)

States of the system(cannot be observed)

z(tk-1) z(tk) z(tk+1)

Process Model(state transition equation)

Measurement Model(measurement equation)


• Process & Measurement Model

x(tk)x(tk-1) x(tk+1)

Measurements(observed)

States of the system(cannot be observed)

z(tk-1) z(tk) z(tk+1)

Process Model(state transition equation)

Measurement Model(measurement equation)

Kalman Filter evolves two step algorithm: 1. Predict: via process model2. Correct: via measurement model


;)()(

;)(ˆ)(ˆ2

122

1

wkxkx

kk

tt

utxtx

)(1)(

)(ˆ)()(ˆ)(ˆ22

kxkx

kkkk

tKt

txtzKtxtx

1. Predict: (superminus!)with Process ModelMeasurement not yet taken!

2. Correct: with measurement

Kalman Filter Algorithm (simplified):

Start with init values

;)(

)(2

)(_2

2

ktzkx

kx

t

tK

Kalman Gaintk := tk+1

Kalman Filter: Possible Extensions

1. Extending to Vector World:Previous scalar state x becomes vector which contains all relevant information of a state in a certain system. For example:

• State vector: =(x,y,z,α,β,γ)

• Process Model:

• Covariance Matrix:

2. Matrices are time dependent e.g. A(t), B(t) 3. Using a non linear process model

Extended Kalman Filter (EKF)

x

wuBtxAtx kk

)()( 1

))(,0()( tQNtw ii

Basic Kalman Filter

• Algorithm: (vector arrows omitted!)

)()()(

)()()(ˆ

1

1

kT

kk

kkk

tQAtAPtP

tButAxtx

)()(

)(ˆ)()(ˆ)(ˆ

kk

kkkk

tPKHItP

txHtzKtxtx

1. Predict 2. Correct

)()(

)(

kT

k

Tk

tRHtHP

HtPK

Kalman Gain

• Measurement Model:

• Process Model: )()()()( 11 kkkk twtuBtxAtx

)()()( kkk tvtxHtz

))(,0()( tQNtw

))(,0()( tRNtv

Basic Kalman Filter [abstract]

• Algorithm:

1. Predict via process model

2. Correct via measurement model


• Process Model: )),(()( 1 kk txftx

)),(()( vtxhtz kk ),0( RNv

Idea of the Kalman Filter

How to use a Kalman Filter

1. Find a state representation2. Find a process model3. Find a measurement model

Many ways to apply a Kalman Filter, i.e. depends on the chosen models!

How to apply KF for Sensor Fusion?

Kalman Filter: Sensor Fusion

Examples:1. Static KF: As seen before (not a real KF!)

2. Basic KF: • Measurement vector incorporates data of

all sensors.• Covariance Matrix R weights data of different

sensors according to their strength

3. “Advanced” KF of G. Welch / G. Bishop: • Asynchronous algorithm• Uses multiple measurement models

z

Kalman Filter: Sensor Fusion[with Basic Kalman Filter]


• Process Model:

vtxHtz kk )()( ),0( RNv

• Measurement Vector incorporates all measurements:

)),(()( 1 kk txftx

xiz

iz

iz

x

nn

n

n

i

xn

H

z

z

nx

:matrix relation t Measuremen

:t vectormeasuremen Combined

:data serves SSensor

6) :pose (e.g. :State

i

• Covariance matrix R reflects variances of different sensors!

Then use “normal” Basic KF algorithm

xiznn

Kalman Filter: Sensor Fusion[Advanced approach Welch/Bishop]

)()()()( twttxtAtx

Process model:

State Representation:

),,,,,,,,,,,()( zyxzyxtx

State transition via A: A relates for example:

);()(

;)()()(

ttyty

ttyttyty

System noise: w with covariance matrix Q ))(,0()( tQNtw ii


Individual measurement model for Sensor i:

Measurement Function: hi(●) with corresponding Jacobian Hi:

Measurement noise: v with covariance matrix R

)())(),(),(()( tvtctbtxhtz iii

))(,0()( tRNtv ii

]))[(),(),((][

],))[(),(),(ˆ( ktctbtxhlx

lktctbtxH ii


• Asynchronous algorithm

• Each time a new measurement z becomes available, a new estimate x will be computed

Sensor 3

Sensor 1

Sensor 2

Kalman FusionFilter


Algorithm:

)()()()()(

)()()(ˆ

tQtAttPtAtP

ttxtAtxT

)()(

)(ˆ)()(ˆ)(ˆ

tPKHItP

tztzKtxtx i

1. Predict 2. Correct

))(),(),(ˆ(

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

)()(

)(

tctbtxHH

tctbtxhtz

tRHtHP

HtPK

i

i

iT

T

Kalman Gain

Corresponding Jacobian

Predicted measurement i

Content




Particle Filters

• To handle non linear processes• To handle non Gaussian Noise• Process and measurement models but different

algorithm slower• Refer to: ”Particle Filters: an overview”, M.

Muehlich• Extensions:

– e.g. Decentralized Sensor Fusion with Distributed Particle Filters

ConclusionMulti Sensor Fusion

• Sensor Fusion is of increasing interest due to higher tracking demands in AR

• Sensor Fusion can be complex and therefore has greater computational requirements

• Future work: standardizing AR fusion systems

Questions

?

Documents

Sensor Fusion Systems Overview and Mathematics Bjoern Griesbach [email protected]