mj-unseld / filtering_tutorial Goto Github PK

This short tutorial aims to make readers understand Bayesian filtering intuitively. Instead of derivation of Kalman filter, I introduce Kalman filter from weighted average and moving average. I expect that readers will have intuition on Kalman filter such as meaning of equations.

• Download tutorial slides
• Download example codes in a ZIP file
• 📝 Please don't miss Roger Labbe's great book, Kalman and Bayesian Filters in Python

This tutorial contains example applications to 2-D localization with various conditions. It is important for users to know how to define the following five items in their applications. I hope that readers can build their intuition from my series of examples. My codes are based on FilterPy, but their contents and your understanding may not be limited to the library.

1. State variable
2. State transition function
3. State transition noise
4. Observation function
5. Observation noise

• From Weighted and Moving Average to Simple 1-D Kalman Filter

  • Merging two weight measurements
  • Merging two weight measurements with their variance (inverse-variance weighted average)
  • 1-D noisy signal filtering (with exponential moving average)
  • 1-D noisy signal filtering (with simple 1-D Kalman filter)

• Kalman Filter

  • 1-D noisy signal filtering
    • State variable: $\mathbf{x} = x$
    • State transition function: $\mathbf{x}_{k+1} = f(\mathbf{x}k; \mathbf{u}{k+1}) = \mathbf{x}_k$
      • Control input: $\mathbf{u}_k = [ ]$
    • State transition noise: $\mathrm{Q} = \sigma^2_Q$
    • Observation function: $\mathbf{z} = h(\mathbf{x}) = \mathbf{x}$
      • Observation: 1-D signal value
    • Observation noise: $\mathrm{R} = \sigma^2_{R}$
  • 2-D position tracking (without class inheritance)
    • State variable: $\mathbf{x} = [x, y]^\top$
    • State transition function: $\mathbf{x}_{k+1} = f(\mathbf{x}k; \mathbf{u}{k+1}) = \mathbf{x}_k$
      • Control input: $\mathbf{u}_k = [ ]$
    • State transition noise: $\mathrm{Q} = \mathrm{diag}(\sigma^2_x, \sigma^2_y)$
    • Observation function: $\mathbf{z} = h(\mathbf{x}) = [x, y]^\top$
      • Observation: $\mathbf{z} = [x_{GPS}, y_{GPS}]^\top$
    • Observation noise: $\mathrm{R} = \mathrm{diag}(\sigma^2_{GPS}, \sigma^2_{GPS})$

• Extended Kalman Filter (EKF)

  • 2-D pose tracking with simple transition noise (without class inheritance)
    • State variable: $\mathbf{x} = [x, y, \theta, v, w]^\top$
    • State transition function: Constant velocity model (time interval: $t$)

      $$\mathbf{x}_{k+1} = f(\mathbf{x}k; \mathbf{u}{k+1}) = \begin{bmatrix} x_k + v_k t \cos(\theta_k + w_k t / 2) \\ y_k + v_k t \sin(\theta_k + w_k t / 2) \\ \theta_k + w_k t \\ v_k \\ w_k \end{bmatrix}$$

      • Control input: $\mathbf{u}_k = [ ]$
    • State transition noise: $\mathrm{Q} = \mathrm{diag}(\sigma^2_x, \sigma^2_y, \sigma^2_\theta, \sigma^2_v, \sigma^2_w)$
    • Observation function: $\mathbf{z} = h(\mathbf{x}) = [x, y]^\top$
      • Observation: $\mathbf{z} = [x_{GPS}, y_{GPS}]^\top$
    • Observation noise: $\mathrm{R} = \mathrm{diag}(\sigma^2_{GPS}, \sigma^2_{GPS})$
  • 2-D pose tracking (using class inheritance)
    • Its state variable, state transition function, observation function, and observation noise are same with the above example.
    • State transition noise

      $$\mathrm{Q} = \mathrm{W}^\top \mathrm{M} \mathrm{W} \quad \text{where} \quad \mathrm{W} = \begin{bmatrix} \frac{\partial f}{\partial v} & \frac{\partial f}{\partial w} \end{bmatrix} \quad \text{and} \quad \mathrm{M} = \begin{bmatrix} \sigma^2_v & 0 \\ 0 & \sigma^2_w \end{bmatrix}$$

  • 2-D pose tracking with odometry
    • Its state transition noise, observation function, and observation noise are same with the above example.
    • State variable: $\mathbf{x} = [x, y, \theta]^\top$
    • State transition function: Constant velocity model (time interval: $t$)

      $$\mathbf{x}{k+1} = f(\mathbf{x}k; \mathbf{u}{k+1}) = \begin{bmatrix} x_k + v{k+1} t \cos(\theta_k + w_{k+1} t / 2) \\ y_k + v_{k+1} t \sin(\theta_k + w_{k+1} t / 2) \\ \theta_k + w_{k+1} t \end{bmatrix}$$

      • Control input: $\mathbf{u}_k = [v_k, w_k]^\top$
  • 2-D pose tracking with off-centered GPS
    • Its state variable, state transition function, state transition noise, and observation noise are same with the above example.
    • Observation function [Choi20]: Off-centered GPS ( $o_x$ and $o_y$ are frontal and lateral offset of the GPS.)

      $$\mathbf{z} = \begin{bmatrix} x_{GPS} \\ y_{GPS} \end{bmatrix} = h(\mathbf{x}) = \begin{bmatrix} x + o_x \cos \theta - o_y \sin \theta \\ y + o_x \sin \theta + o_y \cos \theta \end{bmatrix}$$

• Unscented Kalman Filter (UKF)

  • 2-D pose tracking with simple transition noise
    • Its five definitions (and also implementation style) are same with its corresponding EKF example.
  • 2D pose tracking
    • Its five definitions (and also implementation style) are same with its corresponding EKF example.

• Particle Filter

  • 2-D pose tracking
    • Its five definitions (and also implementation style) are same with its corresponding EKF example.

• FilterPy Documentation: FilterPy API and examples
  • Bookmarks: Kalman filter, EKF, UKF, resampling (for particle filter)
• Kalman and Bayesian Filters in Python: A great introduction on Bayesian filtering with FilterPy
  • Bookmarks: Table of Contents, Kalman filter, EKF, UKF, particle filter
• Particle filter
  • pfilter, John Williamson
  • Monte Carlo Particle Filter for Localization, Pattarawat Chormai
  • particle_filter_demo, Martin J. Laubach
  • Particle Filter for Turtle Localization, Lei Mao

This tutorial was supported by the following R&D projects in Korea.

• AI-based Localization and Path Planning on 3D Building Surfaces (granted by MSIT/NRF, grant number: 2021M3C1C3096810)