# 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.

---

<br>

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?

<br>

## **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,

---

substituting this into the Kalman Filter equations,

$\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

---

**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](#taylor-series)

---

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

<div>

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

</div>
</div>

---

### Taylor Series expansions

<div>

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

</div>
</div>

---

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 <span style="color:red"><b>red</b></span>)

---

**first order** Taylor approximation (in <span style="color:red"><b>red</b></span>)

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})$

<br>

- $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 estimat

---

### 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](https://autonomy-course.github.io/textbook/autonomy-textbook.html#extended-kalman-filter-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](#centralized-fusion)
2. [**decentralized** fusion](#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}$$
</div>
<div>

|||
|-----|-----|
|$a_x, a_y$ | accelerometer readings |

</div>
</div>

---

## State Transition

**constant velocity model** + heading changes from gyroscope

<br>

|||
|-----|-----|
|$a_x, a_y$ | accelerometer readings | 
|$\omega_z$  | gyroscope reading <br> <img src="img/sensors/imu_yaw.gif" width="200"> <br> (yaw rate) |
||

</div>
</div>

---

### 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 |
||
---

**robustness to GPS outages** → reasonable position estimates

---

### comparison

---

| **data source** | **description** |
|:----------------|:----------------|
| **<span style="color:blue">blue dots</span>** | raw gps data |
| **<span style="color:orange">orange line</span>** | dead reckoning using only imu data |
| **<span style="color:green">green line</span>** | 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

---

### 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.)

---

### 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

---

### EKF vs UKF

---

### EKF vs UKF

<div>

<br>

</div>

<div>

<br>

| | 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% |
||

</div>
</div>