Sensor Fusion Systems Overview and Mathematics Bjoern Griesbach [email protected].
-
Upload
emery-nash -
Category
Documents
-
view
230 -
download
0
Transcript of Sensor Fusion Systems Overview and Mathematics Bjoern Griesbach [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
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
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
Fusion of Data from Head-Mounted and Fixed Sensors
• 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!
Fusion of Data from Head-Mounted and Fixed Sensors
Experiment by W. Hoff, First International Workshop on ARSan Francisco
Result: max. translational error was reduced by 90%
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
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
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
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
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
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
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
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
Introduction Kalman Filter
• 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
Introduction Kalman Filter
• 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
Introduction Kalman Filter
Static Kalman Filter:
Introduction 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:
Introduction 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:
• 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 )
Introduction Kalman Filter
• 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)
Introduction Kalman Filter
• 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
Introduction Kalman Filter
;)()(
;)(ˆ)(ˆ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
• 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]
• Measurement Model:
• 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
Kalman Filter: Sensor Fusion[Advanced approach Welch/Bishop]
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
Kalman Filter: Sensor Fusion[Advanced approach Welch/Bishop]
• 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
Kalman Filter: Sensor Fusion[Advanced approach Welch/Bishop]
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
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
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
?