Introduction to Bayes Filter

Course Note SLAM (by Cyrill Stachniss)

Jihong Ju on October 2, 2018

State Estimation

Estimate the state of x of a system given obesrvatioins z and controls u

\[p(x | z, u)\]

Recursive Bayes Filter

Define belief

\[bel(x_t) = p(x_{t} | z_{1:t}, u_{1:t}) \\\]

With Bayes rule

\[= \eta p(z_t | x_{t}, z_{1:t-1}, u_{1:t}) p(x_{t}|z_{1:t-1},u_{1:t})\]

With Markov assumption:

\[= \eta p(z_t | x_{t}) p(x_{t}|z_{1:t-1},u_{1:t})\]

With law of total probability

\[= \eta p(z_t | x_t) \int_{x_{t-1}} p(x_t | x_{t-1}, z_{1:t-1}, u_{1:t}) p(x_{t-1} | z_{1:t-1}, u_{1:t}) dx_{t-1}\]

With again Markov assumption

\[= \eta p(z_t | x_t) \int_{x_{t-1}} p(x_t | x_{t-1}, u_t) p(x_{t-1} | z_{1:t-1}, u_{1:t}) dx_{t-1}\]

With the assumption the current state $ x_{t} $ is independent of the future controls $ u_{t+1} $

\[= \eta p(z_t | x_{t}) \int_{x_{t-1}} p(x_t | x_{t-1}, u_t) p(x_{t-1} | z_{1:t-1}, u_{1:t-1}) dx_{t-1} \\ = \eta p(z_t | x_{t}) \int_{x_{t-1}} p(x_t | x_{t-1}, u_t) bel(x_{t-1}) dx_{t-1}\]

A recursive term!

Prediction and correction step

Prediction step

\[\bar{bel}(x_t) = \int p(x_t \vert x_{t-1}, u_t) bel(x_{t-1}) dx_{t-1}\]

where $ p(x_t \vert x_{t-1}, u_t) $ is so-called the motion model

Correction step

\[bel(x_t) = p(z_t \vert x_t) \bar{bel}(x_t)\]

where $ p(z_t \vert x_t) $ is the observation model.

Different Realizations

Framework for recursive state estimation

Different relaization and properties

  • Linear vs. non-linear models for motion/observation models
  • Gaussian distribution only?
  • Parametric filters

Kalman filter & friends

  • Gaussians
  • Linear or linearized models (note: motion is often non-linear because of rotations)

Particle fileter

  • Non-parametric
  • Arbitrary

Robot Motion models

  • Robot motion is inherently uncertain, How to model uncertainty?

Probabilistic Motion Models

Specify a posterior probabiilty that carries the robot from x to x’: \(p(x_t | x_{t-1}, u_t)\)

Odometry-based

Standard odometry model

#TODO the nice diagram

Probability Dist

  • Noise in $ (x, y, \theta) $, e.g. $ u \sim N(0, \Sigma) $

#TODO the nice diagram and examples

Velocity-based

$ (x, y, \theta) $ to $ (x’, y’, \theta’) $ with translation velocity v and rotation velocity w.

  • Problem: No arc could describe $ \theta’ \neq \theta $
  • Fix: Extra term for the final rotation

Sensor Model

\[p(z_t|x_t)\]

Model for Laser Scanners

Measure Time-of-Flight

Scan z consists of k measurements

\[z_t = (z_t^1, ..., z_t^k)\] \[p(z_t | x_t, m) = \prod_{i=1}^k p(z_t^i | x_t, m)\]

Assumption: Individual measurements are independent given the robot position

Beam-Endpoint Model

Physically flaw bu_t efficient

Ray-cast Model

Mixture of four models: exponential decay in distance, gaussian at obstacle, delta at the max-distance and a uniform distribution.

Model for perceiving landmarks with Range-Bearing Sensors

Range-Bearing

\[z_t^i = (r_t^i, \phi_t^i)^T\]

Robot’s pose

\[(x, y, \theta)^T\]

Observation of feature j at location $ (m_{j,x}, m_{j,y}) $

Reading materials

Bayes filter

  • PR Chapter 2
  • Introduction to Mobile Robotics Chapter 5

Motion and sensor models

  • PR 5 & 6
  • Course 6 & 7