---
title: Particle Filter
tags: wiki
disqus: hackmd
---
Bayesian Particle Filter
===
**Author:** Teng-Hu Cheng
**email:** [e-mail]
**Date:** Nov. 2024
**Website:** https://ncrl.lab.nycu.edu.tw/member/

# 目錄
[toc]

# Preliminary
https://tomohiroliu22.medium.com/機率與貝葉氏機器學習-學習筆記系列-01-貝氏推斷-bayesian-inference-d81b01dc5c89

---
# Assumptions
The following assumtion are made in this article:
- Posterior $P(x_k | y_k)$ under Gaussian distribution (in Section 3).
- The state transition matrix is designed based on the linear motion model (in Section 2).
---
# **Scenario**
Here’s an example for **robot localization**:
- A robot is navigating a 2D environment.
- It has a laser rangefinder that measures the distance to walls.
- The environment is known (map available).
- The robot is uncertain about its position and orientation.

---
# Architecture
Below depicts the architecture of the algorithm

# **1. Initial Setup**
- **Particles**: Each particle represents a possible robot pose, $x$, $y$, $\theta$.
- **Map**: The map provides the positions of walls.
- **Sensor Model**: The laser sensor provides distance measurements to the nearest obstacle.

---
# 2. **Prediction (State Transition Probability)**
Below are two commonly used state transition models:
## 2.1 General Form of a Linear Motion Model

The linear state transition model can be expressed as:
$$\mathbf{x}_k = \mathbf{F} \mathbf{x}_{k-1} + \mathbf{B} \mathbf{u}_{k-1} + \mathbf{w}_{k-1}$$
where:
- $\mathbf{x}_{k-1}$: The state vector at the previous time step.
- $\mathbf{x}_k$: The state vector at the current time step.
- $\mathbf{F}$: The **state transition matrix**, which defines how the state evolves over time.
- $\mathbf{B}$: The **control input matrix**, which defines how the control input affects the state.
- $\mathbf{u}_{k-1}$: The control input at the previous time step.
- $\mathbf{w}_{k-1}$: Process noise, typically modeled as Gaussian noise: $\mathbf{w}_{k-1} \sim \mathcal{N}(0, \mathbf{Q}$), where $\mathbf{Q}$ is the process noise covariance matrix.

## 2.2 2D Constant Velocity Model
For a 2D motion system with position $(x, y)$ and velocity $(v_x, v_y)$:
$$\mathbf{x}_k =
\begin{bmatrix}
x_k \\
y_k \\
v_{x_k} \\
v_{y_k}
\end{bmatrix}
$$
The updated state becomes:
$$\mathbf{x}_k = \mathbf{F} \mathbf{x}_{k-1} + \mathbf{B} \mathbf{u}_{k-1} + \mathbf{w}_{k-1}$$
where the state transition matrix $\mathbf{F}$ and control input matrix $\mathbf{B}$ are defined as
$$
\mathbf{F} =
\begin{bmatrix}
1 & 0 & \Delta t & 0 \\
0 & 1 & 0 & \Delta t \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix},
\quad
\mathbf{B} =
\begin{bmatrix}
0.5 \Delta t^2 & 0 \\
0 & 0.5 \Delta t^2 \\
\Delta t & 0 \\
0 & \Delta t
\end{bmatrix}$$

---
# 3. Update
Now assume that the robot's laser rangefinder reports a distance of ==3 meters== to the nearest wall.

---
## **3.1 Compute Weights**
## 3.2 Compute Gaussian Likelihood
Use a Gaussian likelihood function to compare the predicted sensor reading with the actual reading ($\mathbf{y}_k$=3 from Step 3.) and the state $\mathbf{x}_k$ (i.e., from the state transition in Step 2.):
$$p(\mathbf{y}_k \mid \mathbf{x}_k) = (2\pi)^{-\frac{1}{2}} \det(\mathbf{R})^{-\frac{1}{2}} e^{-\frac{1}{2} (\mathbf{y}_k - \mathbf{C} \mathbf{x}_k)^\top \mathbf{R}^{-1} (\mathbf{y}_k - \mathbf{C} \mathbf{x}_k)}$$
or
$$
P(y_k | x_k) = \frac{1}{\sqrt{2\pi\sigma^2}} \exp \left( -\frac{(z - z_{\text{pred}})^2}{2\sigma^2} \right)$$
where:
- $y$: Actual sensor reading (3.0 meters).
- $y_{\text{pred}}=\mathbf{C} \mathbf{x}_k$: Predicted sensor reading for the particle.
- $\sigma$: Sensor noise standard deviation (e.g., 0.2 meters).
- $\mathbf{R}$: The covariance of the observation noise.

**Note: [You can check out "normpdf" function in Matlab for details.](https://www.mathworks.com/help/stats/normpdf.html#d126e857153)**
### ==Example Calculations:==
- **Particle 1**: $y_{\text{pred}} = 3.2$, likelihood = $\exp(- (3.0 - 3.2)^2 / (2 \times 0.2^2)) = 0.8$.
- **Particle 2**: $y_{\text{pred}} = 4.0$, likelihood = $\exp(- (3.0 - 4.0)^2 / (2 \times 0.2^2)) = 0.01$.
- **Particle 3**: $y_{\text{pred}} = 2.8$, likelihood = $\exp(- (3.0 - 2.8)^2 / (2 \times 0.2^2)) = 0.8$.

---
## 3.3 Weights
By using the observed output $\mathbf{y}_k$, and the weight $w_{k-1}^{(i)}$ computed at the previous time step $k-1$, calculate the intermediate weight update, denoted by $\hat{w}_k^{(i)}$, by using the density function of the measurement probability.
$$
\hat{w}_k^{(i)} = p(\mathbf{y}_k \mid \mathbf{x}_k^{(i)}) \cdot w_{k-1}^{(i)}, \quad i = 1, 2, 3, \dots, N$$
where $i$ denotes the $i^{th}$ particles, and computing the Gaussian likelihood $p(\mathbf{y}_k \mid \mathbf{x}_k^{(i)})$ can be found in Section [3.1.1](#5.1-compute-weights).
After all the intermediate weights $\hat{w}_k^{(i)}, i = 1, 2, 3, \dots, N$ are computed, compute the normalized weights $w_k^{(i)}$ as follows:
$$
w_k^{(i)} = \frac{1}{\bar{w}} \hat{w}_k^{(i)}$$
where $\bar{w} = \sum_{i=1}^{N} \hat{w}_k^{(i)}.$
### Multiple sensor fusion for State Estimation

- [Sensor Fusion and Probabilistic Methods using Force and Vision](/mZi-r7LMTL2qflO7b9W0dw)
## 3.4 Find Pairs
After finding weights in Section [3.1.2](#5.2-weights), we computed the set of particles for the time step $k$:
$$\{(\mathbf{x}_k^{(i)}, w_k^{(i)}) \mid i = 1, 2, 3, \dots, N\}$$

# 4. Resample
## 4.1 Check the Conditions for Resample
This is a resampling step that is performed only if the condition given below is **satisfied**. Calculate the constant $N_\text{eff}$:
$$N_\text{eff} = \frac{1}{\sum_{i=1}^{N} \left( w_k^{(i)} \right)^2} $$

- ==If $N_\text{eff} < \frac{N}{3}$, then generate a new set of $N$ particles from the computed set of particles==:

$$
\{ (\mathbf{x}_k^{(i)}, w_k^{(i)}) \mid i = 1, 2, 3, \dots, N \}^\text{NEW} = \text{RESAMPLE} \left( \{ (\mathbf{x}_k^{(i)}, w_k^{(i)}) \mid i = 1, 2, 3, \dots, N \}^\text{OLD} \right)$$
- ==If $N_\text{eff} > \frac{N}{3}$, the result is accepted.==
Resample particles based on their weights:
- High-weight particles are likely to be duplicated.
- Low-weight particles are likely to be discarded.
## 4.2 Resample
- Resampling is done based on the normalized weights ${w}_k^{(i)}$.
- Particles with higher weights are more likely to be selected multiple times, while particles with very low weights are likely to be discarded.
- [Example of resampling](/LEKChqMrR-qBh2vERVyDBg)

## 4.3 Generate the New Particle Set
- Replace the old set of particles with the new set drawn during resampling.
- Assign equal weights to all particles in the new set (e.g., $\tilde{w}_i = 1/N$).

# **5. Results**
After the sensor update, the particle filter is more confident about the robot’s pose, as particles now concentrate around states consistent with the sensor measurement.

# References
1. [Particle Filter](https://aleksandarhaber.com/clear-and-concise-particle-filter-tutorial-with-python-implementation-part-2-derivation-of-particle-filter-algorithm-from-scratch)
2. [Joint Probability](https://www.youtube.com/watch?v=CQS4xxz-2s4)
3. [J. V. Candy, "Bootstrap Particle Filtering," in IEEE Signal Processing Magazine, vol. 24, no. 4, pp. 73-85, July 2007, doi: 10.1109/MSP.2007.4286566.](https://ieeexplore.ieee.org/document/4286566)
4. [Resampling Methods for Particle Filtering](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=7079001)

# Example 1
## 1. Initial Setup
Given a set of particles $\{(\mathbf{x}_0^{(i)}, w_0^{(i)}) \mid i = 1, 2, 3, \dots, N\}$, where the position of each particle represents a possible robot pose $\mathbf{x}_0^{(i)}$=($x$, $y$,$v_x$, $v_y$), where the state is defined as
$$\mathbf{x}_k =
\begin{bmatrix}
x_k \\
y_k \\
v_{x_k} \\
v_{y_k}
\end{bmatrix}
$$
and the weight are equal, $w_0^{(i)}=1/N$.
## 2. Prediction
By using the dynamics model, the predicted (a priori) state estimation becomes:
$$\hat {\mathbf{x}}_k = \mathbf{F} \mathbf{x}_{k-1} + \mathbf{B} \mathbf{u}_{k-1} + \mathbf{w}_{k-1}$$
where the state transition matrix $\mathbf{F}$ and control input matrix $\mathbf{B}$ are defined as
$$
\mathbf{F} =
\begin{bmatrix}
1 & 0 & \Delta t & 0 \\
0 & 1 & 0 & \Delta t \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix},
\quad
\mathbf{B} =
\begin{bmatrix}
0.5 \Delta t^2 & 0 \\
0 & 0.5 \Delta t^2 \\
\Delta t & 0 \\
0 & \Delta t
\end{bmatrix}$$
where $\mathbf{u}_{k-1}$ is the control input (i.e., acceleration) at the previous time step $k-1$, and $\mathbf{w}_{k-1} \sim \mathcal{N}(0, \mathbf{Q}$), where $\mathbf{Q}$ is the process noise covariance matrix.

## 3. Update
### 3.1 **Measurement Model**
- linear model
Defined the measurement model $\mathbf{y}_k=\mathbf{C} \mathbf{x}_k$ (linear model), where
$$\mathbf{C}=\begin{bmatrix}
1 & 0 & 0 & 0
\end{bmatrix}$$

### 3.2 **Intermediate Weights**
By using the following relation between the predicted measurement in [3.1 Measurement Model]() and ==the observed output $\mathbf{y}_k=3$==, and the weight $w_{k-1}^{(i)}$ computed at the previous time step $k-1$, the intermediate weight update is calculated.
$$p(\mathbf{y}_k \mid \mathbf{x}_k) = (2\pi)^{-\frac{1}{2}} \det(\mathbf{R})^{-\frac{1}{2}} e^{-\frac{1}{2} (\mathbf{y}_k - \mathbf{C} \hat {\mathbf{x}}_k)^\top \mathbf{R}^{-1} (\mathbf{y}_k - \mathbf{C} \hat {\mathbf{x}}_k)},$$
the likelihood can be computed, so that the intermediate weight $\hat{w}_k^{(i)}$ can be calculated as follows:

$$
\hat{w}_k^{(i)} = p(\mathbf{y}_k \mid \mathbf{x}_k^{(i)}) \cdot w_{k-1}^{(i)}, \quad i = 1, 2, 3, \dots, N$$
### 3.3 **Normalization**
After all the intermediate weights $\hat{w}_k^{(i)}, i = 1, 2, 3, \dots, N$ are computed, compute the normalized weights $w_k^{(i)}$ as follows:
$$
w_k^{(i)} = \frac{1}{\bar{w}} \hat{w}_k^{(i)}$$
where $\bar{w} = \sum_{i=1}^{N} \hat{w}_k^{(i)}.$

Then use [3.2.3 Find Pairs]() to find the new pairs $$\{(\mathbf{x}_k^{(i)}, w_k^{(i)}) \mid i = 1, 2, 3, \dots, N\}.$$
## 4. Resample
### 4.1 Check conditions for resample
Follows are the conditions to determine resample or accept.
- If $N_\text{eff} < \frac{N}{3}$, then generate a new set of $N$ particles from the computed set of particles:

$$
\{ (\mathbf{x}_k^{(i)}, w_k^{(i)}) \mid i = 1, 2, 3, \dots, N \}^\text{NEW} = \text{RESAMPLE} \left( \{ (\mathbf{x}_k^{(i)}, w_k^{(i)}) \mid i = 1, 2, 3, \dots, N \}^\text{OLD} \right)$$
- If $N_\text{eff} > \frac{N}{3}$, the result is accepted.

### 4.2 Resample
- Resampling is done based on the normalized weights ${w}_k^{(i)}$.
- Particles with higher weights are more likely to be selected multiple times, while particles with very low weights are likely to be discarded.
- [Example of resampling](/LEKChqMrR-qBh2vERVyDBg)

### 4.3 Generate the New Particle Set
- Replace the old set of particles with the new set drawn during resampling.
- Assign equal weights to all particles in the new set (e.g., $\tilde{w}_i = 1/N$).