Mobile Tracker Technology - PowerPoint

							Sensor Fusion Systems

 Overview and Mathematics

         Bjoern Griesbach
        griesbac@in.tum.de
             Sensor Fusion Systems
                   [Content]
1.    Introduction
     1.   Motivation
     2.   Different tracking options
2.    Existing Multi Sensor Fusion Systems
     1.   Fusion of head mounted and fixed sensor data
     2.   Fusion of magnetic & optical sensor data
     3.   Fusion of gyroscope & optical sensor data
     4.   Open Tracker – an open source AR software
3.    Mathematics of Sensor Fusion
     1.   Kalman Filter
     2.   Particle Filter
                        Motivation
• Each sensor has its strengths and weaknesses
• One sensor is never sufficient for reliable
  tracking
• Optimal tracking = use multiple sensors
                       Precise Estimation


                       Data Fusion Component

               Noisy
                            Noisy              Noisy
               Data                            Data
                            Data

      Sensor                Sensor              Sensor
               Motivation

Multi Sensor Fusion used in various fields of
research:
• Augmented Reality
• Virtual Reality
• Mobile Robots
    Different Tracking Options
    Technology             Location

• Magnetic Tracking • Fixed Trackers
  – reliable           – no limit in size, weight
  – stable             – highly precise (e.g. stereo
  – fast                 vision) in tracking objects
• Optical Tracking     – bad for head orientation
  – precise          • Mobile Trackers (i.e.
  – time consuming     Head Mounted Tracker)
• Gyroscope            – good for head orientation
                       – limited in size & weight
  – precise
                       – less precise in tracking
  – drift error
                         objects
                             Content
1.    Introduction
     1.   Motivation
     2.   Different tracking options
2.    Existing Multi Sensor Fusion Systems
     1.   Fusion of head mounted and fixed sensor data
     2.   Fusion of magnetic & optical sensor data
     3.   Fusion of gyroscope & optical sensor data
     4.   Open Tracker – an open source AR software
3.    Mathematics of Sensor Fusion
     1.   Kalman Filter
     2.   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 =(x,y,z,α,β,γ);
• By transforming poses of two different sensors
  into the same coordinate system, one will get
                                    
  two (noisy) measurements z1 and z 2 for the same
  object
• Each measurement z i  is weighted differently 
  depends on the variance of the measurement
                        
• Each measurement z i has its own variance σi2
  represented by a matrix Pi
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:
                  P2         P 
           x          z1      1
                                   z2 ;
                P  P2
                 1          P  P2
                             1

                 P2
           P        P;
              P  P2
                      1
               1

• Already a simple form of the Kalman Filter - Remember!
Fusion of Data from Head-Mounted
        and Fixed Sensors
 Result: max. translational error was reduced by 90%




                                Experiment by W. Hoff,
                                First International Workshop on AR
                                San Francisco
                             Content
1.    Introduction
     1.   Motivation
     2.   Different tracking options
2.    Existing Multi Sensor Fusion Systems
     1.   Fusion of head mounted and fixed sensor data
     2.   Fusion of magnetic & optical sensor data
     3.   Fusion of gyroscope & optical sensor data
     4.   Open Tracker – an open source AR software
3.    Mathematics of Sensor Fusion
     1.   Kalman Filter
     2.   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.    Introduction
     1.   Motivation
     2.   Different tracking options
2.    Existing Multi Sensor Fusion Systems
     1.   Fusion of head mounted and fixed sensor data
     2.   Fusion of magnetic & optical sensor data
     3.   Fusion of gyroscope & optical sensor data
     4.   Open Tracker – an open source AR software
3.    Mathematics of Sensor Fusion
     1.   Kalman Filter
     2.   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.    Introduction
     1.   Motivation
     2.   Different tracking options
2.    Existing Multi Sensor Fusion Systems
     1.   Fusion of head mounted and fixed sensor data
     2.   Fusion of magnetic & optical sensor data
     3.   Fusion of gyroscope & optical sensor data
     4.   Open Tracker – an open source AR software
3.    Mathematics of Sensor Fusion
     1.   Kalman Filter
     2.   Particle Filter
  Open Tracker: An open Software
        Framework for AR
Most implementations of AR     Open Tracker
Systems were not portable      • Open source framework
solutions.                     • Configurable via XML
                               • Object oriented
Reason: data flow in AR        • Uses a directional graph
Systems is each time             to describe data flow
implemented specifically for
this solution

Need for a standard which        Eases setting up an AR
handles data flow in AR              environment
Systems                             (e.g. distributed, several
                                          trackers, etc.)
            Open Tracker: Example
                                   Multicast
                                   Information
               Console             to multiple
                                   users on a
                                   network


                         Fusion
                          Filter
Sink node

                                    Transformation         Noise
   Filter                               Filter             Filter
   node


Source node    Optical Tracker     Optical Tracker   Magnetic Tracker
                  [Mobile]             [Fixed]          [Mobile]
                             Content
1.    Introduction
     1.   Motivation
     2.   Different tracking options
2.    Existing Multi Sensor Fusion Systems
     1.   Fusion of head mounted and fixed sensor data
     2.   Fusion of magnetic & optical sensor data
     3.   Fusion of gyroscope & optical sensor data
     4.   Open Tracker – an open source AR software
3.    Mathematics of Sensor Fusion
     1.   Kalman Filter
     2.   Particle Filter
           Kalman Filter
Step by step introduction:

1.   Static KF
2.   Basic KF
3.   Extended KF
4.   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!
             Kalman Filter
Conventions:
 We observe a system with:
  x : state of the system
  z : measurement (approximates x)
  σ2 : variance of a measurement
  
  x : vector
  P : covariance matrix
  ˆ
 x : best estimate of state
   
 ˆ
 x : best estimate before measurement was taken
    Introduction Kalman Filter
Assumptions:
• Two scalar sensor measurements z1 and z2
• Gaussian noise, i.e. zi ~ N(0, σi2 )

 Optimal state estimate:

       x  z1  K  ( z2  z1 );
       ˆ
      2  (1  K )   12
                         12
     Kalman Gain    K 2       ;
                      1   2
                             2
       Introduction Kalman Filter
•   Let’s incorporate time!
•   Measurements z1 and z2 were taken sequentially 
    z(t1), z(t2)


 Optimal state estimate at time t2 :

    x(t2 )  z (t1 )  K  z (t2 )  z (t1 )
    ˆ
       2  1  K   z2(t )1


                             z2(t )
       Kalman Gain K  2              1
                                          ;
                       z (t )   z (t )
                                  1
                                     2
                                          2
      Introduction Kalman Filter
•   Let’s take more measurements (time continues)
•   Incorporate previous knowledge (last estimate)



 Optimal state estimate at time tk :

      x(tk )  x(tk 1 )  K  z (tk )  x(tk 1 )
      ˆ        ˆ                          ˆ
     (tk )  1  K   (tk 1 )
      2                         2



                             2 (tk 1 )
          Kalman Gain K  2                    ;
                          (tk 1 )   z (t )
                                         2
                                             k
     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, σw2 )

• Measurement Model:
  z(tk)=x(tk)+v                Noise: v ~ N(0, σz2 )
       Introduction Kalman Filter
• Process & Measurement Model

 z(tk-1)                        z(tk)    z(tk+1)   Measurements
                                                   (observed)

     Measurement Model                             States of the system
     (measurement equation)                        (cannot be observed)

 x(tk-1)                        x(tk)    x(tk+1)
           Process Model
           (state transition equation)
       Introduction Kalman Filter
• Process & Measurement Model

 z(tk-1)                        z(tk)    z(tk+1)   Measurements
                                                   (observed)

     Measurement Model                             States of the system
     (measurement equation)                        (cannot be observed)

 x(tk-1)                        x(tk)    x(tk+1)
           Process Model
           (state transition equation)

 Kalman Filter evolves two step algorithm:
 1. Predict: via process model
 2. Correct: via measurement model
          Introduction Kalman Filter
Kalman Filter Algorithm (simplified):


x  (tk )  x  (t k 1 )  u;
ˆ           ˆ                         x(tk )  x  (tk )  K  z (tk )  x  (tk )
                                      ˆ        ˆ                        ˆ                       
 x  (tk )   x (tk 1 )   w ;
  2             2              2
                                       x (tk )  1  K   x  (tk )
                                        2                     2


                                 tk := tk+1
                                                                            x2 (tk )
Start with init                                   Kalman Gain       K  2_                   ;
values                                                                  x (tk )   z (tk )
                                                                                       2




     1. Predict: (superminus!)                      2. Correct:
     with Process Model                             with measurement
     Measurement not yet taken!
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 =(x,y,z,α,β,γ)
                                                      
   •   Process Model: x (t k )  A  x (t k 1 )  B  u  w
                            
   •   Covariance Matrix: wi (t )  N (0, Qi (t ))
2. Matrices are time dependent e.g. A(t), B(t)
3. Using a non linear process model
    Extended Kalman Filter (EKF)
                   Basic Kalman Filter
                                                                     
• Process Model:               x (t k )  A  x (t k 1 )  Bu (t k )  w(t k 1 )        w(t )  N (0, Q(t ))

                                                                
• Measurement Model:                    z (t k )  H  x (t k )  v (t k )                v(t )  N (0, R(t ))




• Algorithm: (vector arrows omitted!)

     x  (tk )  Ax(tk 1 )  Bu(tk )
     ˆ                                                                           
                                                   x(tk )  x  (tk )  K  z (tk )  H  x  (tk )
                                                   ˆ        ˆ                             ˆ                
    P  (tk )  AP(tk 1 ) AT  Q(tk )            P(tk )  I  KH P  (tk )

      1. Predict                                                             2. Correct

                                                                                    P  (tk ) H T
                                                    Kalman Gain              K
                                                                                HP (tk ) H T  R(tk )
               Basic Kalman Filter [abstract]
• Process Model:               x(t k )  f ( x(t k 1 ),)

• Measurement Model:           z (t k )  h( x(t k ), v)     v  N (0, R)




• Algorithm:

        1. Predict                                  2. Correct
           via process model                           via measurement model




                  Idea of the Kalman Filter
   How to use a Kalman Filter
1. Find a state representation
2. Find a process model
3. 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 z 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
       Kalman Filter: Sensor Fusion
                  [with Basic Kalman Filter]
• Process Model:              x(t k )  f ( x(t k 1 ),)

• Measurement Model:          z (t k )  H  x(t k )  v    v  N (0, R)


• Measurement Vector incorporates all measurements:
                   
           State : x  nx (e.g. pose : nx  6)
                                    
           Sensor Si serves data : zi  
                                           n zi


                                           
           Combined measuremen vector: z   zi
                                                  n
                                 t
           Measurement relation matrix : H   zi x
                                               n n


• Covariance matrix R    n     zi  n x
                                             reflects variances of
  different sensors!
 Then use “normal” Basic KF algorithm
  Kalman Filter: Sensor Fusion
      [Advanced approach Welch/Bishop]
Process model:
                                    
        x (t )  A(t ) x (t  t )  w(t )
State Representation:
   
                                        
   x (t )  ( x, y, z,  ,  ,  , x, y, z,  ,  ,  )
 State transition via A:
  A relates for example:

    y (t )  y (t  t )  y (t )t ;
                           
    y (t )  y (t  t );
            
                                            
 System noise: w with covariance matrix Q   wi (t )  N (0, Qi (t ))
  Kalman Filter: Sensor Fusion
      [Advanced approach Welch/Bishop]
Individual measurement model for Sensor i:

                                             
          zi (t )  hi ( x(t ),b (t ), c (t ))  vi (t )
Measurement Function: hi(●) with corresponding Jacobian Hi:

                                                      
  H i ( x (t ), b(t ), c(t ))[ k , l ]   hi ( x (t ), b (t ), c (t ))[ k ]
        ˆ
                                        x[l ]

Measurement noise: v with covariance matrix R
   
   vi (t )  N (0, Ri (t ))
    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 2

        Sensor 3
                    Kalman Fusion
                        Filter



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



x  (t )  A(t ) x(t  t )
ˆ                                                              x(t )  x  (t )  K  zi (t )  z (t )
                                                               ˆ       ˆ                         ˆ
P (t )  A(t ) P(t  t ) A (t )  Q(t )
                              T                              P(t )  I  KH  P  (t )

      1. Predict                                                               2. Correct

                                            P  (t ) H T
                                     K                                     Kalman Gain
                                        HP  (t ) H T  Ri (t )
                                   z (t )  hi ( x  (t ), b(t ), c(t ))
                                   ˆ             ˆ                          Predicted measurement i
                                     H  H i ( x  (t ), b(t ), c(t ))
                                               ˆ                            Corresponding Jacobian
                             Content
1.    Introduction
     1.   Motivation
     2.   Different tracking options
2.    Existing Multi Sensor Fusion Systems
     1.   Fusion of head mounted and fixed sensor data
     2.   Fusion of magnetic & optical sensor data
     3.   Fusion of gyroscope & optical sensor data
     4.   Open Tracker – an open source AR software
3.    Mathematics of Sensor Fusion
     1.   Kalman Filter
     2.   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
               Conclusion
             Multi 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




   ?