Probability in Robotics

Posted on Edited on In

Noise exists in the world, ans also exists in the robot motion and observations, so robots never know exactaly of the environment and themselves. However, probality theory could represent the uncertainty and give a better guess or estimation of the world. Therefore, the state estimation is very important in Robotics.

The common applications of state estimation includes Mapping (estimate what the world looks like), Localization (estimate where it is), SLAM (estimate both Mapping and Localization), Navigation, Motion Planning (estimate which action to execute).

See prerequisite knowledge post for more details.

Recursive State Estimation

  • State Estimation: estimate the state $x$ of a system given observations $z$ and controls $y$. i.e. $p(x_t | z_{1:t}, u_{1:t})$
  • Recursive State Estimation: estimate the state based on previous distributions and new observations, then update current distributions

Recursive Bayes Filter

With Markov assumption, we could have the following equation

$$
\begin{split}
bel(x_t)
& = p(x_t | z_{1:t}, u_{1:t}) \\
& = \eta p(z_t | x_t, z_{1:t-1}, u_{1:t}) p(x_t | z_{1:t-1}, u_{1:t}) \\
& = \eta p(z_t | x_t) \int p(x_t | x_{t-1}, z_{1:t-1}, u_{1:t}) p(x_{t-1} | z_{1:t-1}, u_{1:t}) \mathrm{d} x_{t-1} \\
& = \eta p(z_t | x_t) \int p(x_t | x_{t-1}, u_{t}) p(x_{t-1} | z_{1:t-1}, u_{1:t-1}) \mathrm{d} x_{t-1} \\
& = \eta p(z_t | x_t) \int p(x_t | x_{t-1}, u_{t}) bel(x_{t-1}) \mathrm{d} x_{t-1}
\end{split}
$$

With Bayes filter, the state estimation for each time includes prediciton and correction two steps.

  • Prediction: motion model
    $$
    \overline{bel}(x_t) = \int \bbox[border: solid]{p(x_t | x_{t-1}, u_{t})} bel(x_{t-1}) \mathrm{d} x_{t-1}
    $$
  • Correction: observation model
    $$
    bel(x_t) = \eta \bbox[border: solid]{p(z_t | x_t)} \overline{bel}(x_t)
    $$

Motion and observation model are central models in the Bayes filter to be specified. There are different realizations of the Bayes filter adopted different specific assumption. e.g.

  • Kalman Filter & Extented Kalman Filter
    • Gaussians
    • Linear or linearized models
  • Particle Filter
    • Arbitrary models
    • Non-parametric

Occupancy Grip Map

  • Map
    • Grip (Occupancy) Map:
      • 2D grip of cells indicating whether that position is occupied (binary value 1 means occupied, 0 means free)
      • Non-parametric model
      • No need to rely on features detector, only need reange data
    • Voxel Map: 3D form of grip map
    • Feature Map: a set of 3D points
  • Mapping Task: Compute the map given the poses and observations

$$
m^* = \arg\max_m P(m|x_1, z_1, \cdots, x_t, z_t)
$$

Mapping Grip Map

  • Representation of grip map cells (probability of occupied)

$$
p(m) = \prod_i p(m_i)
$$

$$
p(m_i) = \left\{
\begin{matrix}
1, &\text{means the cell is occupied} \\
0, &\text{means the cell is free} \\
\text{other}, &\text{means no knowledge}
\end{matrix}
\right.
$$

  • Mapping based on sensor data and poses

$$
p(m|z_{1:t}, x_{1:t}) = \prod_i p(m_i | z_{1:t}, x_{1:t})
$$

$\quad\quad$ Based on Bayes rule, and Markov

$$
\frac{p(m_i | z_{1:t}, x_{1:t})}{1 - p(m_i | z_{1:t}, x_{1:t})} =
\frac{p(m_i|z_t,x_t)}{1-p(m_i|z_t,x_t)}
\frac{p(m_i|z_{1:t-1},x_{1:t-1})}{1-p(m_i|z_{1:t-1},x_{1:t-1})}
\frac{1-p(m_i)}{p(m_i)}
$$

$\quad\quad$ use $\log$ for the equation

$$
l(m_i | z_{1:t}, x_{1:t}) = l(m_i|z_t,x_t) + l(m_i|z_{1:t-1},x_{1:t-1}) - l(m_i)
$$

$\quad\quad$ with

$$
l(m_i|z_{1:t}, x_{1:t}) = \log \left(\frac{p(m_i | z_{1:t}, x_{1:t})}{1 - p(m_i | z_{1:t}, x_{1:t})} \right)
$$

Therefore, the $p(m_i | z_{1:t}, x_{1:t})$ could be solved recursivly with the prior knowledge and the inverse of observation model.

Inverse of Observation Model

Inverse observation model is the model input observation and output the real environment

  • 2D Laser Scanner Inverse Model
  • Sonar Inverse Model

Locomotion

  • “The power of motion from place to place”
  • Relevant to state estimation