---
title: Particle Filter
tags: wiki
disqus: hackmd
---
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:
- The state transition matrix is designed based on the linear motion model (in Section 2).
- Posterior $P(x_k | y_k)$ under Gaussian distribution (in Section 3).

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

# Architecture
Below depicts the architecture of the algorithm

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

# 2. **Prediction**
**Not required!**

The key idea embedded in the MC approach is to represent the required distribution as a set of random samples rather
than a specific analytic function (e.g., Gaussian).

# 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.1.1 non-Gaussian Observation Model
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(y_k | x_k) = \kappa - \frac{1}{2} ( y_k - \mathbf{C} \mathbf{x}_k)^{T} \mathbf{R}^{-1} (y_k-\mathbf{C} \mathbf{x}_k)
$$
where:
- $\kappa$ is some positive constant to make $P(y_k | x_k)$ positive.
- $y$: Actual sensor reading (3.0 meters).
- $y_{\text{pred}}=\mathbf{C} \mathbf{x}_k$: Predicted sensor reading for the particle.
- $\mathbf{R}$: The covariance of the observation noise.

### 3.1.2 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)}.$
### 3.1.3 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
**Not required!**

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

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