15.sensor_fusion.md

title	Sensor Fusion | EKF
textbook	#sensor-fusion

Sensor Fusion

Design of Autonomous Systems

csci 6907/4907-Section 86

Prof. Sibin Mohan

---

remember this?

Note:

• sensors and state estimation, by its very probabilistic and noisy nature, introduces errors.

---

question is → "which sensor do we believe?"

---

this might be the wrong question!

---

instead of picking one vs. the other

---

instead of picking one vs. the other

use both?

---

sensor fusion

---

sensor fusion

process of combining sensory data from multiple sources to obtain more accurate information than would be possible using individual sensors alone

---

individual sensors have inherent limitations:

---

individual sensors have inherent limitations:

• limited accuracy → measurement errors

---

individual sensors have inherent limitations:

• limited accuracy → measurement errors
• limited range/coverage → may only work in certain conditions

---

individual sensors have inherent limitations:

• limited accuracy → measurement errors
• limited range/coverage → may only work in certain conditions
• limited sampling rates → cannot always update fast enough

---

individual sensors have inherent limitations:

• limited accuracy → measurement errors
• limited range/coverage → may only work in certain conditions
• limited sampling rates → cannot always update fast enough
• sensor-specific weaknesses: GPS fails indoors, cameras in darkness

---

sensor fusion

• combines complementary data from multiple sensors

---

sensor fusion

• combines complementary data from multiple sensors
• each with different strengths/weaknesses

---

sensor fusion

• combines complementary data from multiple sensors
• each with different strengths/weaknesses

goal → produce combination that's more accurate, complete, robust

---

so, how do we fuse sensors?

---

can we use the kalman filter for sensor fusion?

---

can we use the kalman filter for sensor fusion?

yes!

---

example: consider the following

• state, $\mathbf{x}=\left[\begin{array}{l}x \newline \dot{x}\end{array}\right]$

---

example: consider the following

• state, $\mathbf{x}=\left[\begin{array}{l}x \newline \dot{x}\end{array}\right]$
• two sensors, $\mathbf{z}=\left[\begin{array}{c}z_{G P S} \newline z_{\text {odom }}\end{array}\right]$

Note:

• (GPS → metres, odometry → feet)

---

using parameters for Kalman Filter,

$$ \begin{aligned} \mathbf{H} =\left[\begin{array}{cc} 1 & 0 \newline 1 / 0.3048 & 0 \end{array}\right] \quad \quad \mathbf{R} =\left[\begin{array}{cc} \sigma_{\text {GPS }}^{2} & 0 \newline 0 & \sigma_{\text {Odom }}^{2} \end{array}\right] \end{aligned} $$

---

substituting this into the Kalman Filter equations,

$$ \begin{aligned} \mathbf{y} = & \mathbf{z}-\mathbf{H} \overline{\mathbf{x}} \newline \end{aligned} $$

substituting this into the Kalman Filter equations,

$$ \begin{aligned} \mathbf{y} = & \mathbf{z}-\mathbf{H} \overline{\mathbf{x}} \newline \mathbf{y} = & {\left[\begin{array}{c} z_{G P S} \newline z_{\text {Odom }} \end{array}\right]-\left[\begin{array}{cc} 1 & 0 \newline 1 / 0.3048 & 0 \end{array}\right]\left[\begin{array}{l} \bar{x} \newline \bar{\dot{x}} \end{array}\right] } \newline \end{aligned} $$

---

substituting this into the Kalman Filter equations,

$$ \begin{aligned} \mathbf{y} = & \mathbf{z}-\mathbf{H} \overline{\mathbf{x}} \newline \mathbf{y} = & {\left[\begin{array}{c} z_{G P S} \newline z_{\text {Odom }} \end{array}\right]-\left[\begin{array}{cc} 1 & 0 \newline 1 / 0.3048 & 0 \end{array}\right]\left[\begin{array}{l} \bar{x} \newline \bar{\dot{x}} \end{array}\right] } \newline \mathbf{x} = & \overline{\mathbf{x}}+{\mathbf{K} \mathbf{y}} \newline \end{aligned} $$

---

substituting this into the Kalman Filter equations,

$$ \begin{aligned} \mathbf{y} = & \mathbf{z}-\mathbf{H} \overline{\mathbf{x}} \newline \mathbf{y} = & {\left[\begin{array}{c} z_{G P S} \newline z_{\text {Odom }} \end{array}\right]-\left[\begin{array}{cc} 1 & 0 \newline 1 / 0.3048 & 0 \end{array}\right]\left[\begin{array}{l} \bar{x} \newline \bar{\dot{x}} \end{array}\right] } \newline \mathbf{x} = & \overline{\mathbf{x}}+{\mathbf{K} \mathbf{y}} \newline \end{aligned} $$

$\mathbf{K}$ and $\mathbf{y}$ are $2 \times 2$ matrices → it mixes the GPS and odometry!

---

but Kalman filter doesn't work for non-linear systems

---

but Kalman filter doesn't work for non-linear systems

what exactly is the problem with nonlinear systems?

---

Kalman Filter (and any Bayes' Filter) requires → Gaussians

---

Kalman Filter (and any Bayes' Filter) requires → Gaussians

breaks down in the face of non-linearities

---

Gaussians and non-linear functions

---

Gaussians and non-linear functions

mixing of	result
two Gaussians	Gaussian

---

Gaussians and non-linear functions

mixing of	result
two Gaussians	Gaussian
Gaussian and linear function	Gaussian

---

Gaussians and non-linear functions

mixing of	result
two Gaussians	Gaussian
Gaussian and linear function	Gaussian
Gaussian and non-linear function	non-Gaussian

---

Gaussians and non-linear functions

mixing of	result
two Gaussians	Gaussian
Gaussian and linear function	Gaussian
Gaussian and non-linear function	non-Gaussian
linear and non-linear function	non-Gaussian

---

Gaussians and non-linear functions

mixing of	result
two Gaussians	Gaussian
Gaussian and linear function	Gaussian
Gaussian and non-linear function	non-Gaussian
linear and non-linear function	non-Gaussian
two non-linear function	non-Gaussian

---

example: Gaussian being transformed using another function, $y=g(x)$

---

case 1: $g(x)$ is linear, e.g.,

$$ g = 0.5*x + 1 $$

---

case 1: $g(x)$ is linear, e.g.,

Note:

• we can see the result and it is a nice Gaussian

---

case 2: $g(x)$ is non-linear, e.g.,

$$ g = \cos(3 * (\frac{x}{2} + 0.7)) * \sin(1.3 * x)—1.6 * x $$

---

case 2: $g(x)$ is non-linear, e.g.,

---

case 2: $g(x)$ is non-linear, e.g.,

output is no longer a Gaussian

---

output is no longer a Gaussian

violates unimodality assumption of Kalman Filter → requires single peak

---

extended kalman filter

---

extended kalman filter (ekf)

• linearization → approximate a non-linear function, $g(.)$

---

extended kalman filter (ekf)

• linearization → approximate a non-linear function, $g(.)$
• by a linear function that is tangent to $g(.)$

---

extended kalman filter (ekf)

• linearization → approximate a non-linear function, $g(.)$
• by a linear function that is tangent to $g(.)$
• at the point of interest

---

EKF → uses Taylor Series Expansion

---

a brief detour...

---

Taylor Series

---

Taylor Series

approximates function → around specific point using polynomial terms

---

Taylor Series

approximates function → around specific point using polynomial terms

infinite sum of polynomials → function’s derivatives at single point, $a$

---

Taylor Series

$$ f(x) = f(a) + f'(a)(x-a) + \frac{f''(a)}{2!}(x-a)^2 + \frac{f'''(a)}{3!}(x-a)^3 + \ldots $$

---

Taylor Series

$$ f(x) = f(a) + f'(a)(x-a) + \frac{f''(a)}{2!}(x-a)^2 + \frac{f'''(a)}{3!}(x-a)^3 + \ldots $$

term	description
$f(a)$	the function value at point $a$

---

Taylor Series

$$ f(x) = f(a) + f'(a)(x-a) + \frac{f''(a)}{2!}(x-a)^2 + \frac{f'''(a)}{3!}(x-a)^3 + \ldots $$

term	description
$f(a)$	the function value at point $a$
$f'(a)$, $f''(a)$, etc.	the derivatives of $f$ evaluated at point $a$

---

Taylor Series

$$ f(x) = f(a) + f'(a)(x-a) + \frac{f''(a)}{2!}(x-a)^2 + \frac{f'''(a)}{3!}(x-a)^3 + \ldots $$

term	description
$f(a)$	the function value at point $a$
$f'(a)$, $f''(a)$, etc.	the derivatives of $f$ evaluated at point $a$
$(x-a)$	represents the deviation from the expansion point

---

Taylor Series expansions

---

Taylor Series expansions

• higher-order Taylor Expansions → closer approximation of original function

---

Taylor Series expansions

• higher-order Taylor Expansions → closer approximation of original function
• computation → intractable!

---

example 1: $f(x) = x^2$ → expanded around $a = 1$

Note:

• f(x) is non-linear here

---

example 1: $f(x) = x^2$ → expanded around $a = 1$

1. $f(1) = 1$

---

example 1: $f(x) = x^2$ → expanded around $a = 1$

1. $f(1) = 1$
2. $f'(x) = 2x$, so $f'(1) = 2$

---

example 1: $f(x) = x^2$ → expanded around $a = 1$

1. $f(1) = 1$
2. $f'(x) = 2x$, so $f'(1) = 2$
3. first-order Taylor approximation: $f(x) \approx 1 + 2(x-1)$

---

example 1: $f(x) = x^2$ → expanded around $a = 1$

1. $f(1) = 1$
2. $f'(x) = 2x$, so $f'(1) = 2$
3. first-order Taylor approximation: $f(x) \approx 1 + 2(x-1)$
4. this gives us a linear approximation: $f(x) \approx 2x - 1$

---

linear approximation: $f(x) \approx 2x - 1$

---

example 2: non-linear function from before:

$$ g = \cos(3 * (\frac{x}{2} + 0.7)) * \sin(1.3 * x) — 1.6 * x $$

---

$$ g = \cos(3 * (\frac{x}{2} + 0.7)) * \sin(1.3 * x) — 1.6 * x $$

first order Taylor approximation (in red)

---

first order Taylor approximation (in red)

not a very good approximation!

---

what do we care about?

---

what do we care about?

• only concerned with approximation at → posterior

---

what do we care about?

• only concerned with approximation at → posterior
• recompute posteriors → very short time period

---

what do we care about?

• only concerned with approximation at → posterior
• recompute posteriors → very short time period
• approximation → quite good in close vicinity of point of interest

---

let's look at our approximation again

---

let's look at our approximation again

---

let's look at our approximation again

---

let's look at our approximation again

quite good!

---

EKF → first-order Taylor expansions

---

EKF → first-order Taylor expansions

• for nonlinear system and
• measurement functions

---

EKF → first-order Taylor expansions

• for nonlinear system and
• measurement functions

linearizing nonlinear functions → around current estimate

---

ekf | equations

---

ekf | equations

state equation: $x_k = f(x_{k-1}, u_k) + w_k$

---

ekf | equations

state equation: $x_k = f(x_{k-1}, u_k) + w_k$

measurement equation: $z_k = h(x_k) + v_k$

---

ekf | equations

state equation: $x_k = f(x_{k-1}, u_k) + w_k$

measurement equation: $z_k = h(x_k) + v_k$

$f(.)$ and $h(.)$ → non-linear

---

ekf | equations | linearization

---

ekf | equations | linearization

$$ f(x_{k-1}, u_k) \approx f(\hat x_{k-1|k-1}, u_k) + F_k(x_{k-1} - \hat{x}_{k-1|k-1}) $$

---

ekf | equations | linearization

$$ f(x_{k-1}, u_k) \approx f(\hat x_{k-1|k-1}, u_k) + F_k(x_{k-1} - \hat{x}_{k-1|k-1}) $$

$h(x_k) \approx h(\hat x_{k|k-1}) + H_k(x_k - \hat{x}_{k|k-1})$

---

ekf | equations | linearization

$$ f(x_{k-1}, u_k) \approx f(\hat x_{k-1|k-1}, u_k) + F_k(x_{k-1} - \hat{x}_{k-1|k-1}) $$

$h(x_k) \approx h(\hat x_{k|k-1}) + H_k(x_k - \hat{x}_{k|k-1})$

• $F_k$ → Jacobian of $f$ w.r.t. $x$
• $H_k$ → Jacobian of $h$ w.r.t. $x$

---

Jacobian matrices

---

Jacobian matrices

• contain all partial derivatives of nonlinear functions

---

Jacobian matrices

• contain all partial derivatives of nonlinear functions
• w.r.t each state variable

---

Jacobian matrices

• contain all partial derivatives of nonlinear functions
• w.r.t each state variable
• evaluated at the current estimate

---

Jacobian matrices

• contain all partial derivatives of nonlinear functions
• w.r.t each state variable
• evaluated at the current estimate

represent sensitivity of functions → to small changes in state

---

Jacobian matrices

state is a vector → need partial derivaties

---

ekf | steps

---

ekf | steps | prediction

$$\hat x_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_k)$$

---

ekf | steps | prediction

$$\hat x_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_k)$$

$$P_{k|k-1} = F_kP_{k-1|k-1}F_k^T + Q_k$$

Note:

• x is state
• P is uncertainty/covariance

---

ekf | steps | update

---

ekf | steps | update

$$K_k = P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T + R_k)^{-1}$$

Note:

• K is kalman gain

---

ekf | steps | update

$$K_k = P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T + R_k)^{-1}$$

$$\hat x_{k|k} = \hat x_{k|k-1} + K_k(z_k - h(\hat x_{k|k-1}))$$

---

ekf | steps | update

$$K_k = P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T + R_k)^{-1}$$

$$\hat x_{k|k} = \hat x_{k|k-1} + K_k(z_k - h(\hat x_{k|k-1}))$$

$$P_{k|k} = (I - K_kH_k)P_{k|k-1}$$

---

please read the textbook chapter on ekf for actual details!

---

EKF and Sensor Fusion

---

two main approaches → multi-sensor fusion with EKF

---

two main approaches → multi-sensor fusion with EKF

1. centralized fusion
2. decentralized fusion

---

Centralized Fusion

---

Centralized Fusion

• all sensor measurements → processed in single EKF

---

Centralized Fusion

• all sensor measurements are processed in a single EKF
• measurement vector combines all sensor readings $$z_k = \begin{bmatrix} z_k^1 \newline z_k^2 \newline \vdots \newline z_k^n \end{bmatrix}$$

---

Centralized Fusion

• all sensor measurements are processed in a single EKF
• measurement vector combines all sensor readings
• measurement function

$$h(x_k) = \begin{bmatrix} h^1(x_k) \newline h^2(x_k) \newline \vdots \newline h^n(x_k) \end{bmatrix}$$

---

Centralized Fusion

• all sensor measurements are processed in a single EKF
• measurement vector combines all sensor readings
• measurement function, noise covariance

$$h(x_k) = \begin{bmatrix} h^1(x_k) \newline h^2(x_k) \newline \vdots \newline h^n(x_k) \end{bmatrix}, \quad R_k = \begin{bmatrix} R_k^1 & 0 & \cdots & 0 \newline 0 & R_k^2 & \cdots & 0 \newline \vdots & \vdots & \ddots & \vdots \newline 0 & 0 & \cdots & R_k^n \end{bmatrix}$$

---

Decentralized Fusion

• each sensor → has its own local filter

---

Decentralized Fusion

• each sensor → has its own local filter
• results are combined → fusion center

---

Decentralized Fusion

• each sensor → has its own local filter
• results are combined → fusion center

more modular and fault-tolerant

---

Decentralized Fusion

• state estimates from individual filters → combined
• using covariance intersection:

$$P_{fused}^{-1} = \sum_{i=1}^n P_i^{-1}$$ $$P_{fused}^{-1}\hat x_{fused} = \sum_{i=1}^n P_i^{-1}\hat{x}_i$$

---

measurement noise covariance matrix, $R_k$

---

measurement noise covariance matrix, $R_k$

• how much weight → each sensor's measurements

---

measurement noise covariance matrix, $R_k$

• how much weight → each sensor's measurements
• sensors with lower measurement uncertainty (smaller values in $R_k$)

---

measurement noise covariance matrix, $R_k$

• how much weight → each sensor's measurements
• sensors with lower measurement uncertainty (smaller values in $R_k$)
  • more influence → on final state estimate

---

measurement noise covariance matrix, $R_k$

• how much weight → each sensor's measurements
• sensors with lower measurement uncertainty (smaller values in $R_k$)
  • more influence → on final state estimate
• lower uncertainty → higher confidence

---

adaptive methods → dynamically adjust covariances

---

adaptive methods → dynamically adjust covariances

• current operating conditions

---

adaptive methods → dynamically adjust covariances

• current operating conditions
• sensor health monitoring

---

adaptive methods → dynamically adjust covariances

• current operating conditions
• sensor health monitoring
• consistency checks between sensors

---

adaptive methods → dynamically adjust covariances

• current operating conditions
• sensor health monitoring
• consistency checks between sensors
• historical performance

---

example → GPS accuracy degrades in urban canyons

---

example → GPS accuracy degrades in urban canyons

GPS covariance → increase (lower confidence) in those environments

---

Example: IMU and GPS Fusion using EKF

---

Example: IMU and GPS Fusion using EKF

fusing IMU (accelerometer, gyroscope) + GPS → position tracking

---

State Vector

$$ x = [\text{position}_x, \text{position}_y, \text{velocity}_x, \text{velocity}_y, \text{heading}]^T $$

---

State Transition

---

State Transition

constant velocity model + heading changes from gyroscope

---

State Transition

constant velocity model + heading changes from gyroscope

$$f(x_{k-1}, u_k) = \begin{bmatrix} \text{position}_x + \text{velocity}_x \Delta t \newline \text{position}_y + \text{velocity}_y \Delta t \newline \text{velocity}_x + a_x \Delta t \newline \text{velocity}_y + a_y \Delta t \newline \text{heading} + \omega_z \Delta t \end{bmatrix}$$

---

State Transition

constant velocity model + heading changes from gyroscope

$$ \begin{bmatrix} \text{position}_x + \text{velocity}_x \Delta t \newline \text{position}_y + \text{velocity}_y \Delta t \newline \text{velocity}_x + a_x \Delta t \newline \text{velocity}_y + a_y \Delta t \newline \text{heading} + \omega_z \Delta t \end{bmatrix}$$

$a_x, a_y$	accelerometer readings

---

State Transition

constant velocity model + heading changes from gyroscope

$$ \begin{bmatrix} \text{position}_x + \text{velocity}_x \Delta t \newline \text{position}_y + \text{velocity}_y \Delta t \newline \text{velocity}_x + a_x \Delta t \newline \text{velocity}_y + a_y \Delta t \newline \text{heading} + \omega_z \Delta t \end{bmatrix}$$

$a_x, a_y$	accelerometer readings
$\omega_z$	gyroscope reading (yaw rate)

---

Measurement Models

• GPS → $h_{gps}(x_k) = [\text{position}_x, \text{position}_y]^T$

---

Measurement Models

• GPS → $h_{gps}(x_k) = [\text{position}_x, \text{position}_y]^T$
• IMU (for update) → $h_{imu}(x_k) = [\text{velocity}_x, \text{velocity}_y, \text{heading}]^T$

---

high-frequency updates	IMU updates at $100-1000$ Hz	smooth tracking

---

high-frequency updates	IMU updates at $100-1000$ Hz	smooth tracking
drift correction	GPS updates at $1-10$ Hz	corrects accumulated drift from IMU

---

high-frequency updates	IMU updates at $100-1000$ Hz	smooth tracking
drift correction	GPS updates at $1-10$ Hz	corrects accumulated drift from IMU

robustness to GPS outages → reasonable position estimates

---

comparison

---

data source	description
blue dots	raw gps data
orange line	dead reckoning using only imu data
green line	ekf fusion of gps and imu

---

EKF+Sensor Fusion | Best Practices

---

EKF+Sensor Fusion | Best Practices

best practices	description	notes
careful state selection	include only necessary states	avoid computational burden

---

EKF+Sensor Fusion | Best Practices

best practices	description	notes
careful state selection	include only necessary states	avoid computational burden
proper initialization	set initial covariance	reflect actual uncertainty

---

EKF+Sensor Fusion | Best Practices

best practices	description	notes
careful state selection	include only necessary states	avoid computational burden
proper initialization	set initial covariance	reflect actual uncertainty
tuning noise parameters	adjust $Q$ and $R$	based on empirical data

---

EKF+Sensor Fusion | Best Practices (contd.)

best practices	description	notes
consistency monitoring	check filter consistency	normalized innovation squared (NIS)

---

EKF+Sensor Fusion | Best Practices (contd.)

best practices	description	notes
consistency monitoring	check filter consistency	normalized innovation squared (NIS)
fault detection	implement mechanisms to detect sensor failures

---

EKF+Sensor Fusion | Best Practices (contd.)

best practices	description	notes
consistency monitoring	check filter consistency	normalized innovation squared (NIS)
fault detection	implement mechanisms to detect sensor failures
numerical stability	use square-root or UD factorization	improved numerical properties

---

Handling Non-Gaussian Noise

EKF → both process and measurement noise are Gaussian

---

systems with non-Gaussian noise

---

systems with non-Gaussian noise

method	description
particle filters	represent the probability distribution using samples

---

systems with non-Gaussian noise

method	description
particle filters	represent the probability distribution using samples
robust kalman filters	use heavy-tailed distributions to model outliers

---

systems with non-Gaussian noise

method	description
particle filters	represent the probability distribution using samples
robust kalman filters	use heavy-tailed distributions to model outliers
pre-filtering	apply outlier rejection before using EKF

---

EKF vs UKF

Note:

• UKF -- unscented kalman filter

1. selects a set of sigma points around the current state estimate
2. propagates these points through the nonlinear functions
3. computes a weighted mean and covariance from the transformed points

---

EKF vs UKF

	EKF error	UKF error	impr.
position	1.45m	0.95m	34.5%
velocity	0.32m/s	0.25m/s	21.9%
heading	2.1deg	1.7deg	19.0%

Note:

• avoids the need for explicit Jacobian calculations and can handle nonlinearities better.