Sensor Fusion for Aerial Robots

Shaojie ShenAssistant Professor

1

Why Sensor Fusion?

• Vision/GPS-only state estimation is too noisy, slow, and delayed for feedback control of agile aerial robots

• To improve robustness with multiple sensors and handle sensor failures• To estimate quantities that are unobservable using single sensors

2

Red: Vision+IMU FusionBlue: Vision-only

Design Considerations…

3

• Accuracy• Frequency• Latency• Sensor synchronization & timestamp accuracy• Delayed and out-of-order measurements• Estimator initialization• Sensor calibration• Different measurement models with uncertainties• Robustness to outliers• Computational efficiency

• IMU centric fusion– High frequency– Low latency– (Almost) always available– (Usually) large drift

• Absolute measurements

• Relative measurements

What to Fuse?

4

IMU

GPS Velocity

GPS PositionPressure Altimeter

Cameras

Laser Scanner

LaserAltimeter

Multi-Sensor Fusion

(A Few) Sensor Fusion Methods

5

• Kalman filtering– Loosely-coupled [1]

• Fuse processed information from individual sensors

– Tightly-coupled [2]• Fuse raw measurements

directly• Optimization-based methods

– Mostly tightly-coupled [3,4]

IMU

Visual Odometry

EKF/UKF-Based Multi-

Sensor Fusion

[1] D. Scaramuzza, et. al. Vision-controlled micro flying robots: from system design to autonomous navigation and mapping in GPS denied environments. IEEE Robot. Autom. Mag., 21(3), 2014.[2] A. I. Mourikis and S. I. Roumeliotis. A multi-state constraint Kalman filter for vision-aided inertial navigation. In Proc. of the IEEE Intl. Conf. on Robot. and Autom., pages 3565–3572, Roma, Italy, April 2007.[3] S. Leutenegger, et al. Keyframe-based visual-inertial SLAM using nonlinear optimization. In Proc. of Robot.: Sci. and Syst., Berlin, Germany, June 2013.[4] S. Shen, et. al. Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom., Seattle, WA, May 2014.

Outline

6

• Loosely-Coupled, Extended Kalman Filtering-Based Multi-Sensor Fusion (“Tutorial”)

• Tightly-Coupled, Optimization-Based, Monocular Visual-Inertial Fusion, with Online Initialization and Camera-IMU Extrinsic Calibration (Brief Intro)

IMU

Visual Odometry

EKF-Based Multi-Sensor

Fusion

EKF - Assumption & Model

• The prior state of the robot is represented by a Gaussian distribution– 𝑝𝑝 𝑥𝑥0 ~ 𝑁𝑁(𝜇𝜇0, Σ0)

• The continuous time process model is:– �̇�𝑥 = 𝑓𝑓(𝑥𝑥,𝑢𝑢,𝑛𝑛)– 𝑛𝑛𝑡𝑡 ~ 𝑁𝑁 0,𝑄𝑄𝑡𝑡 is Gaussian white noise– Can be linearized using one-step Euler integration:

• 𝑥𝑥 ̅𝑡𝑡 ≈ 𝑥𝑥𝑡𝑡−1 + 𝑓𝑓(𝑥𝑥𝑡𝑡−1,𝑢𝑢𝑡𝑡 ,𝑛𝑛𝑡𝑡) 𝛿𝛿𝛿𝛿• The measurement model is:

– 𝑧𝑧 = ℎ(𝑥𝑥, 𝑣𝑣)– 𝑣𝑣𝑡𝑡 ~ 𝑁𝑁 0,𝑅𝑅𝑡𝑡 is Gaussian white noise

• The process and measurement models are all time-stamped

7

EKF - Process Model Linearization

• Linearize the process model about 𝑥𝑥 = 𝜇𝜇𝑡𝑡−1, 𝑢𝑢 = 𝑢𝑢𝑡𝑡 , 𝑛𝑛 = 0

– �̇�𝑥 ≈ 𝑓𝑓 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡 , 0 + �𝜕𝜕𝜕𝜕𝜕𝜕𝜕𝜕 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡,0

𝑥𝑥 − 𝜇𝜇𝑡𝑡−1 + �𝜕𝜕𝜕𝜕𝜕𝜕𝑢𝑢 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡,0

𝑢𝑢 − 𝑢𝑢𝑡𝑡

+ �𝜕𝜕𝜕𝜕𝜕𝜕𝑛𝑛 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡,0

(𝑛𝑛 − 0)

• Let:

– 𝐴𝐴𝑡𝑡 = �𝜕𝜕𝜕𝜕𝜕𝜕𝜕𝜕 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡,0

– 𝐵𝐵𝑡𝑡 = �𝜕𝜕𝜕𝜕𝜕𝜕𝑢𝑢 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡,0

– 𝑈𝑈𝑡𝑡 = �𝜕𝜕𝜕𝜕𝜕𝜕𝑛𝑛 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡,0

• Linear process model:– �̇�𝑥 ≈ 𝑓𝑓 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡 , 0 + 𝐴𝐴𝑡𝑡(𝑥𝑥 − 𝜇𝜇𝑡𝑡−1) + 𝐵𝐵𝑡𝑡(𝑢𝑢 − 𝑢𝑢𝑡𝑡) + 𝑈𝑈𝑡𝑡(𝑛𝑛 − 0)

8

EKF – Measurement Model Linearization

• Linearize the measurement model about 𝑥𝑥 = �̅�𝜇𝑡𝑡, 𝑣𝑣 = 0

– ℎ 𝑥𝑥, 𝑣𝑣 ≈ ℎ �̅�𝜇𝑡𝑡 , 0 + �𝜕𝜕ℎ𝜕𝜕𝜕𝜕 �𝜇𝜇𝑡𝑡,0

𝑥𝑥 − �̅�𝜇𝑡𝑡 + �𝜕𝜕ℎ𝜕𝜕𝑣𝑣 �𝜇𝜇𝑡𝑡,0

(𝑣𝑣 − 0)

• Let:

– 𝐶𝐶𝑡𝑡 = �𝜕𝜕ℎ𝜕𝜕𝜕𝜕 �𝜇𝜇𝑡𝑡,0

– 𝑊𝑊𝑡𝑡 = �𝜕𝜕ℎ𝜕𝜕𝑣𝑣 �𝜇𝜇𝑡𝑡,0

• Linear measurement model:– 𝑧𝑧𝑡𝑡 = ℎ 𝑥𝑥𝑡𝑡 , 𝑣𝑣𝑡𝑡 ≈ ℎ �̅�𝜇𝑡𝑡, 0 + 𝐶𝐶𝑡𝑡 𝑥𝑥𝑡𝑡 − �̅�𝜇𝑡𝑡 + 𝑊𝑊𝑡𝑡 𝑣𝑣𝑡𝑡

9

EKF - Summary

• Process Update:– �̅�𝜇𝑡𝑡 = 𝜇𝜇𝑡𝑡−1 + 𝛿𝛿𝛿𝛿 𝑓𝑓 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡, 0– �Σ𝑡𝑡 = 𝐹𝐹𝑡𝑡 Σ𝑡𝑡−1 𝐹𝐹𝑡𝑡𝑇𝑇 + 𝑉𝑉𝑡𝑡 𝑄𝑄𝑡𝑡 𝑉𝑉𝑡𝑡𝑇𝑇

– �̇�𝑥 = 𝑓𝑓 𝑥𝑥,𝑢𝑢,𝑛𝑛– 𝑛𝑛𝑡𝑡 ~ 𝑁𝑁 0,𝑄𝑄𝑡𝑡

– 𝐴𝐴𝑡𝑡 = �𝜕𝜕𝜕𝜕𝜕𝜕𝜕𝜕 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡,0

– 𝑈𝑈𝑡𝑡 = �𝜕𝜕𝜕𝜕𝜕𝜕𝑛𝑛 𝜇𝜇𝑡𝑡−1,𝑢𝑢𝑡𝑡,0

– 𝐹𝐹𝑡𝑡 = 𝐼𝐼 + 𝛿𝛿𝛿𝛿 𝐴𝐴𝑡𝑡– 𝑉𝑉𝑡𝑡 = 𝛿𝛿𝛿𝛿 𝑈𝑈𝑡𝑡

• Measurement Update:– 𝜇𝜇𝑡𝑡 = �̅�𝜇𝑡𝑡 + 𝐾𝐾𝑡𝑡 (𝑧𝑧𝑡𝑡 − ℎ(�̅�𝜇𝑡𝑡, 0))– Σ𝑡𝑡 = �Σ𝑡𝑡 − 𝐾𝐾𝑡𝑡 𝐶𝐶𝑡𝑡 �Σ𝑡𝑡– 𝐾𝐾𝑡𝑡 = �Σ𝑡𝑡 𝐶𝐶𝑡𝑡𝑇𝑇 (𝐶𝐶𝑡𝑡 �Σ𝑡𝑡 𝐶𝐶𝑡𝑡𝑇𝑇 +

10

Assumptions

Linearization

Discretization

Assumptions

Linearization

Notes:• We may have multiple heterogeneous measurement models as long as

they are fused sequentially

Example 1

• Quadrotor with IMU and:– Absolute Pose Sensors (GPS + Pressure Altimeter + Magnetometer)– Absolute Velocity Sensor (Optical Flow / GPS using Doppler effect)

11

State

• The IMU provides noisy and biased measurements of linear acceleration and angular velocity

• 𝐱𝐱 =

𝐩𝐩𝐪𝐪�̇�𝐩𝐛𝐛𝑔𝑔𝐛𝐛𝒂𝒂

=

positionorientation

linear velocitygyroscope bias

accelerometer bias

∈ 𝐑𝐑15

• (For now) use Z-X-Y Euler angle parameterization of 𝑆𝑆𝑆𝑆(3) for orientation– 𝐪𝐪 = 𝜙𝜙, 𝜃𝜃,𝜓𝜓 𝑇𝑇 = roll, pitch, yaw 𝑇𝑇

– 𝑅𝑅 =𝑐𝑐𝜓𝜓𝑐𝑐𝜃𝜃 − 𝑠𝑠𝜙𝜙𝑠𝑠𝜓𝜓𝑠𝑠𝜃𝜃 −𝑐𝑐𝜙𝜙𝑠𝑠𝜓𝜓 𝑐𝑐𝜓𝜓𝑠𝑠𝜃𝜃 + 𝑐𝑐𝜃𝜃𝑠𝑠𝜙𝜙𝑠𝑠𝜓𝜓𝑐𝑐𝜃𝜃𝑠𝑠𝜓𝜓 + 𝑐𝑐𝜓𝜓𝑠𝑠𝜙𝜙𝑠𝑠𝜃𝜃 𝑐𝑐𝜙𝜙𝑐𝑐𝜓𝜓 𝑠𝑠𝜓𝜓𝑠𝑠𝜃𝜃 − 𝑐𝑐𝜓𝜓𝑐𝑐𝜃𝜃𝑠𝑠𝜙𝜙

−𝑐𝑐𝜙𝜙𝑠𝑠𝜃𝜃 𝑠𝑠𝜙𝜙 𝑐𝑐𝜙𝜙𝑐𝑐𝜃𝜃– We may use quaternions and error-state EKF to avoid singularities (not

covered here) 12

Process Model

• The gyroscope gives a noisy and biased estimate of the angular velocity– 𝜔𝜔𝑚𝑚 = 𝜔𝜔 + 𝐛𝐛𝑔𝑔 + 𝐧𝐧𝑔𝑔

• The drift in the gyroscope bias is a Gaussian, white noise process

– �̇�𝐛𝑔𝑔 = 𝐧𝐧𝑏𝑏𝑔𝑔– 𝐧𝐧𝑏𝑏𝑔𝑔 ~ 𝑁𝑁(0,𝑄𝑄𝑔𝑔)

• The accelerometer gives a noisy and baised estimate of the linear acceleration– 𝐚𝐚𝑚𝑚 = 𝑅𝑅 𝐪𝐪 𝑇𝑇(�̈�𝐩 − 𝐠𝐠) + 𝐛𝐛𝑎𝑎 + 𝐧𝐧𝑎𝑎

• The drift in the accelerometer bias is a Gaussian, white noise process– �̇�𝐛𝑎𝑎 = 𝐧𝐧𝑏𝑏𝑎𝑎– 𝐧𝐧𝑏𝑏𝑎𝑎 ~ 𝑁𝑁(0,𝑄𝑄𝑎𝑎)

13

Process Model

• 𝜔𝜔𝑚𝑚 is in the body frame, 𝐪𝐪 is in the world frame• The angular velocity in the body frame is given by

– 𝜔𝜔 =𝑝𝑝𝑞𝑞𝑟𝑟

=𝑐𝑐𝜃𝜃 0 −𝑐𝑐𝜙𝜙𝑠𝑠𝜃𝜃0 1 𝑠𝑠𝜙𝜙𝑠𝑠𝜃𝜃 0 𝑐𝑐𝜙𝜙𝑐𝑐𝜃𝜃

�̇�𝜙�̇�𝜃�̇�𝜓

= 𝐺𝐺 𝐪𝐪 �̇�𝐪

• Process model:

– �̇�𝐱 =

�̇�𝐩𝐺𝐺 𝐪𝐪 −1 𝜔𝜔𝑚𝑚 − 𝐛𝐛𝑔𝑔 − 𝐧𝐧𝑔𝑔𝐠𝐠 + 𝑅𝑅(𝐪𝐪)(𝐚𝐚𝑚𝑚 − 𝐛𝐛𝒂𝒂 − 𝐧𝐧𝑎𝑎)

𝐧𝐧𝑏𝑏𝑔𝑔𝐧𝐧𝑏𝑏𝑎𝑎

14

Absolute Measurement Model

• Position: GPS + Pressure Altimeter• Orientation: Magnetometer + Accelerometer (not exactly correct…)• Velocity: Optical flow / GPS with Doppler effect

• 𝐳𝐳 =𝐩𝐩𝐪𝐪�̇�𝐩

+ 𝐯𝐯

=𝐼𝐼 0 0 0 00 𝐼𝐼 0 0 00 0 𝐼𝐼 0 0

𝐩𝐩𝐪𝐪�̇�𝐩𝐛𝐛𝑔𝑔𝐛𝐛𝒂𝒂

+ 𝐯𝐯

= 𝐶𝐶 𝐱𝐱 + 𝐯𝐯

15

Example 2

• Quadrotor with IMU and:– 6-DOF Relative Pose Sensor (Stereo Visual Odometry)– 3-DOF Relative Pose Sensor (Laser Scan Matching)– 1-DOF Relative Pose Sensor (Laser/ Sonar Altimeter)

• These are all measurements with respect to a certain keyframe

16

State Augmentation

• Kalman filter requires all measurements to be related to the current state only

• State augmentation at keyframe changes (Roumeliotis and Burdick, 2002)– May augment arbitrary copies of states depending on the availability

of relative sensors

17

𝐱𝐱1 𝐱𝐱2 𝐱𝐱3

𝐱𝐱3 𝐱𝐱3𝐱𝐱3

State Augmentation

𝐏𝐏33 𝐏𝐏33𝐏𝐏33 𝐏𝐏33

𝐏𝐏33

�𝐱𝐱5𝐱𝐱3

𝐱𝐱1 𝐱𝐱2 𝐱𝐱3 𝐱𝐱4 𝐱𝐱5�𝐏𝐏55 �𝐏𝐏35�𝐏𝐏53 𝐏𝐏33

𝐱𝐱5𝐱𝐱3

𝐳𝐳5

𝐏𝐏55 𝐏𝐏35𝐏𝐏53 𝐏𝐏33

MainAugmented

Relative Measurement Models

• 6-DOF relative pose measurement from stereo visual odometry

• Both 𝐱𝐱𝑡𝑡 and 𝐱𝐱𝑡𝑡−𝑘𝑘 are augmented into a joint state 𝐲𝐲𝑡𝑡 =𝐱𝐱𝑡𝑡𝐱𝐱𝑡𝑡−𝑘𝑘

• 𝐳𝐳 =Δ𝐩𝐩𝑡𝑡|𝑡𝑡−𝑘𝑘Δ𝐪𝐪𝑡𝑡|𝑡𝑡−𝑘𝑘

+ 𝐯𝐯t

=𝑅𝑅(𝐪𝐪𝑡𝑡−𝑘𝑘)(𝐩𝐩𝑡𝑡 − 𝐩𝐩𝑡𝑡−𝑘𝑘)

𝛿𝛿𝑡𝑡𝑡𝑡𝑢𝑢𝑡𝑡𝑡𝑡𝑟𝑟(𝑅𝑅 𝐪𝐪𝑡𝑡−𝑘𝑘 𝑇𝑇𝑅𝑅 𝐪𝐪𝑡𝑡 ) + 𝐯𝐯t= ℎ 𝐱𝐱𝑡𝑡, 𝐱𝐱𝑡𝑡−𝑘𝑘 + 𝐯𝐯t= ℎ 𝐲𝐲𝑡𝑡 + 𝐯𝐯t

Notes:• One state augmentation per keyframe change• Only states that are affected by the relative measurement (pose in the

example) need to be augmented

18

Sensor synchronization & timestamps

• Best: Sensors perfectly synchronized

• OK: Sensors have the same clock

• Bad: Sensors with different clock or inaccurate timestamps

19

IMU

Vision

IMU

Vision

IMU

Vision

Delayed and Out-of-Sequence Measurements

• Handles delayed and out-of-sequence measurements using fixed-lag priority queue– Redo process process updates (state only) after each (delayed)

measurement update– Covariance update is done only up to the latest measurement

20

Multi-Sensor Quadrotor

• UKF-based multi-sensor fusion

21

Stereo CamerasGPS and Magnetometer

Laser Scanner with Mirror Housing

IMU and Pressure AltimeterIntel Core i3

Computer

22

23

Example 3

• Quadrotor with IMU and:– Up-to-Scale Relative Pose Sensor (Monocular Visual Odometry)– Very relevant for micro aerial robots!

24

Loosely-Coupled Monocular Visual-Inertial Fusion

• State is modified to explicitly estimate the visual scale:

– 𝐱𝐱 =

𝐩𝐩𝐪𝐪�̇�𝐩𝐛𝐛𝑔𝑔𝐛𝐛𝒂𝒂𝜆𝜆

=

positionorientation

linear velocitygyroscope bias

accelerometer biasvisual scale

∈ 𝐑𝐑1𝟔𝟔

• Relative measurement model (Δ�𝐩𝐩: up-to-scale relative translation):

• 𝐳𝐳 =Δ�𝐩𝐩𝑡𝑡|𝑡𝑡−𝑘𝑘Δ𝐪𝐪𝑡𝑡|𝑡𝑡−𝑘𝑘

+ 𝐯𝐯t

=𝜆𝜆 ⋅ 𝑅𝑅(𝐪𝐪𝑡𝑡−𝑘𝑘)(𝐩𝐩𝑡𝑡 − 𝐩𝐩𝑡𝑡−𝑘𝑘)𝛿𝛿𝑡𝑡𝑡𝑡𝑢𝑢𝑡𝑡𝑡𝑡𝑟𝑟(𝑅𝑅 𝐪𝐪𝑡𝑡−𝑘𝑘 𝑇𝑇𝑅𝑅 𝐪𝐪𝑡𝑡 ) + 𝐯𝐯t

= ℎ 𝐱𝐱𝑡𝑡, 𝐱𝐱𝑡𝑡−𝑘𝑘 + 𝐯𝐯t25

May suffer from convergence issues if scale initialization is poor

Summary of Loosely-Coupled EKF Fusion

26

• Accuracy • Frequency • Latency • Sensor synchronization & timestamp accuracy

– Can be done using offline calibration if sensors have the same clock

• Delayed and out-of-order measurements • Estimator initialization • Sensor calibration

– Can be done using offline calibration and online refinement

• Different measurement models with uncertainties • Robustness to outliers • Computational efficiency

Open Source Packages

27

• Kalibr - Offline calibration toolbox (ETH Zurich ASL)– Multi-camera calibration– Camera-IMU calibration– Temporal alignment: handles sensor synchronization & timestamp

accuracy issues– https://github.com/ethz-asl/kalibr

• MSF - Modular framework for multi sensor fusion based on an EKF (ETH Zurich ASL)– https://github.com/ethz-asl/ethzasl_msf

Outline

28

• Loosely-Coupled, Extended Kalman Filtering-Based Multi-Sensor Fusion (“Tutorial”)

• Tightly-Coupled, Optimization-Based, Monocular Visual-Inertial Fusion, with Online Initialization and Camera-IMU Extrinsic Calibration (Brief Intro)

IMU

Visual Odometry

EKF-Based Multi-Sensor

Fusion

Challenges

• Scale ambiguity

• Up-to-scale motion estimation and 3D reconstruction (Structure from Motion)

29

λ =?

Challenges

• With IMU, scale is observable, but…– Requires initial velocity and attitude (gravity)– Requires knowledge about camera-IMU calibration– Highly nonlinear system – requires initial values to converge

30

v0 =?g0 =?Rcb =?

pcb =?

Short term integration of IMU

Can we operate without initialization or calibration?

Plug-and-play monocular visual-inertial systems!

System Pipeline

• Linear camera-IMU rotation calibration (Rcb)

• Linear initialization and camera-IMU translation calibration (v0, g0, pcb)

• Tightly-coupled nonlinear optimization and calibration refinement• Calibration convergence identification methods

31

Linear Camera-IMU Rotation Calibration

• Incremental IMU Rotations:• Incremental Camera Rotations:

32

Rb1b0 ,Rb2

b1 ,Rb3b2 …

, Rc2c1 , Rc3

c2 …Rc1c0

Incremental compute Rcb to

align two rotation sequences

IMU Pre-Integration

• IMU integration in the body frame of the first pose– Nonlinearity from relative rotation only– Linear update equations for position, velocity, and gravity– IMU Integration without initialization– Uncertainty propagation on manifold

33

𝑩𝑩𝟎𝟎

𝑩𝑩𝟏𝟏

• IMU pre-integration for using IMU without initialization• Estimates position, velocity, gravity, camera-IMU translation,

and feature depth using graph-based optimization• Linear formulation enables recovery of initial conditions

34

Initial Condition

Linear Sliding Window Initialization and Camera-IMU Translation Calibration

𝐩𝐩0𝐯𝐯0𝐠𝐠0

𝐩𝐩1𝐯𝐯1𝐠𝐠1

𝐩𝐩2𝐯𝐯2𝐠𝐠2

𝐩𝐩3𝐯𝐯3𝐠𝐠3

𝐩𝐩4𝐯𝐯4𝐠𝐠4

𝝀𝝀0

𝝀𝝀1

𝐩𝐩𝑐𝑐𝑏𝑏

35

On-the-Fly Initialization

Camera-IMU Calibration

36

Camera-IMU Rotation

Camera-IMU Translation

Linear Initialization and Camera-IMU Calibration

37

System Pipeline

• Tightly-coupled nonlinear optimization and calibration refinement

38

Tightly-Coupled Nonlinear Sliding Window Optimization with Calibration Refinement

• Nonlinear graph-based optimization using linear initialization– Optimize position, velocity, rotation, inverse feature depth, and camera-IMU

transformation simultaneously:

– Iteratively minimize residuals from all sensors

39

Linearization

IMU residual Reprojection errorPrior

Autonomous Quadrotor Flight

40

Self-Calibrating Multi-Sensor Fusion

41

Summary of Tightly-Coupled Fusion

42

• Accuracy • Frequency • Latency • Sensor synchronization & timestamp accuracy • Delayed and out-of-order measurements • Estimator initialization • Sensor calibration • Different measurement models with uncertainties • Robustness to outliers • Computational efficiency

Conclusion

43

• Why sensor fusion?• What methods are available?

– Filtering-based– Optimization-based– Loosely-coupled– Tightly-coupled

• What are the special design considerations?– Multiple measurement models– Delayed and out-of-order measurements– Sensor calibration– Estimator initialization– Computational efficiency