Particle Filter - non-Gaussian Distribution

Author: Teng-Hu Cheng
email: [e-mail]
Date: Nov. 2024
Website: https://ncrl.lab.nycu.edu.tw/member/

目錄

• Particle Filter - non-Gaussian Distribution
• 目錄
• Preliminary
• Assumptions
• Scenario
• Architecture
• 1. Initial Setup
• 2. Prediction (State Transition Probability)
• 3. Update
  • 3.1 Compute Weights
    • 3.1.1 non-Gaussian Observation Model
    • 3.1.2 Weights
    • Multiple sensor fusion for State Estimation
    • 3.1.3 Find Pairs
• 4. Resample
  • 4.1 Check the Conditions for Resample
    • 4.2 Resample
    • 4.3 Generate the New Particle Set
• 5. Results
• References
• Example 1
  • 1. Initial Setup
  • 2. Prediction
  • 3. Update
    • 3.1 Measurement Model
    • 3.2 Intermediate Weights
    • 3.3 Normalization
  • 4. Resample
    • 4.1 Check conditions for resample
    • 4.2 Resample
    • 4.3 Generate the New Particle Set

Preliminary

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

Assumptions

The following assumtion are made in this article:

• The state transition matrix follows Markov property (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 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 is a commonly used state transition models.

For a 2D motion system with position

${\mathbf{x}}_k$:

$${\mathbf{x}}_k=\left[\begin{array}{c}{x}_{1k}\\ x_{2k}\end{array}\right]$$
The updated state becomes:

$${\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}$$
where the state transition matrix

$\mathbf{F}$ is defined as

$$\mathbf{F}=\left[\begin{array}{cc}0.7& 0.8\\ 0.3& 0.2\end{array}\right].$$
Note that the elements in

$\mathbf{F}$ can be transition matrix following Markov property.

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.

Note: You can check out "normpdf" function in Matlab for details.

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)},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.
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}{\overline{w}}{\hat{w}}_k^{(i)}$$
where

$\overline{w}=\sum _{i=1}^N{\hat{w}}_k^{(i)}.$

Multiple sensor fusion for State Estimation

• Sensor Fusion and Probabilistic Methods using Force and Vision

3.1.3 Find Pairs

After finding weights in Section 3.1.2, 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}(\{({\mathbf{x}}_k^{(i)},w_k^{(i)})\mid i=1,2,3,\dots ,N{\}}^{\text{OLD}})$$

• 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

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
2. Joint Probability
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.
4. Resampling Methods for Particle Filtering

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=\left[\begin{array}{c}{x}_k\\ y_k\\ v_{x_k}\\ v_{y_k}\end{array}\right]$$
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}=\left[\begin{array}{cccc}1& 0& \mathrm{\Delta }t& 0\\ 0& 1& 0& \mathrm{\Delta }t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],\mathbf{B}=\left[\begin{array}{cc}0.5\mathrm{\Delta }{t}^2& 0\\ 0& 0.5\mathrm{\Delta }{t}^2\\ \mathrm{\Delta }t& 0\\ 0& \mathrm{\Delta }t\end{array}\right]$$
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}=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right]$$

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{)}^{\mathrm{\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)},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}{\overline{w}}{\hat{w}}_k^{(i)}$$
where

$\overline{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}(\{({\mathbf{x}}_k^{(i)},w_k^{(i)})\mid i=1,2,3,\dots ,N{\}}^{\text{OLD}})$$

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