S47_logistics_summary.ipynb

{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "06v7V41TpNAD",
   "metadata": {},
   "source": [
    "# Chapter Summary"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5e2f7c5b",
   "metadata": {
    "tags": [
     "no-pdf"
    ]
   },
   "source": [
    "<img src=\"Figures4/S40-Warehouse_robots-01.jpg\" alt=\"Splash image with cute robot with a stalked eye\" width=\"60%\" align=center style=\"vertical-align:middle;margin:10px 0px\">"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cYSdTD63aN1Q",
   "metadata": {},
   "source": [
    "In the previous chapters, we have mainly considered discrete probability distributions. In Chapter 2, we modeled the world state using five\n",
    "discrete categories of trash, and in Chapter 3 we modeled the world as five discrete rooms.\n",
    "In this chapter, we began a serious study of *continuous* random variables, first to represent the robot's state, and then to represent\n",
    "sensor readings.\n",
    "Representing and reasoning about continuous probability distributions is more involved than working in discrete domains,\n",
    "and we introduced a set of representational and inference tools that were able to scale to these more difficult problems."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "W5kXAH61wCvo",
   "metadata": {},
   "source": [
    "## Models\n",
    "\n",
    "In this chapter, we represented the state of the robot using continuous coordinates, $x \\in \\mathbb{R}^2$,\n",
    "and we modeled uncertainty in state using a Gaussian distribution. \n",
    "Gaussian distributions have several nice properties. \n",
    "First, they are completely characterized by two parameters,\n",
    "a mean vector, $\\mu \\in \\mathbb{R}^n$, and a covariance matrix, $\\Sigma \\in \\mathbb{R}^{n \\times n}$.\n",
    "In the one-dimensional case, these are scalars, denoted by $\\mu$ and $\\sigma$.\n",
    "Perhaps more importantly, Gaussian distributions enjoy the privilege of being very good approximations\n",
    "for many stochastic aspects of real-world systems.\n",
    "Roboticists, and engineers in general, often resort to the assumption that noise, disturbances, or other stochastic aspects of\n",
    "real-world systems can be accurately approximated using Gaussian distributions.\n",
    "\n",
    "To model uncertainty in the motion model, we introduced the conditional Gaussian PDF.\n",
    "In particular, we assumed that noise in the motion model could be modeled as additive Gaussian noise,\n",
    "so that the state at time $k+1$ is defined as\n",
    "\\begin{equation}\n",
    "x_{k+1} = x_k + u_k + w_k\n",
    "\\end{equation}\n",
    "in which $x_k$ is the state at time $k$, $w_k$ is the random disturbance,\n",
    "and $u_k$ is the commanded motion at time $k$.\n",
    "Under our Gaussian assumption that $w_k \\sim N(\\mu,\\Sigma)$,\n",
    "the probability distribution for the state at time $k+1$ is given by\n",
    "the conditional Gaussian PDF\n",
    "\\begin{equation}\n",
    "p(x_{k+1}|x_{k}, u_k) = \\mathcal{N}(x_{k_1}; x_{k} +  u_k, \\Sigma)\n",
    "\\end{equation}\n",
    "Thus, by assuming Gaussian noise in the motion model, we arrive to a kind of\n",
    "\"Gaussian in/Gaussian out\" formulation, which can greatly simplify certain inference\n",
    "problems (e.g., by using the Kalman filter).\n",
    "\n",
    "We can also use continuous conditional PDFs to model sensors.\n",
    "For example, if the ideal (i.e., noise-free) sensor reading at time $k$\n",
    "is defined by a function $h(x_k)$, we can model the sensor\n",
    "output by the random variable\n",
    "\\begin{equation}\n",
    "z_k = h(x_k) + w_k\n",
    "\\end{equation}\n",
    "in which $w_k$ is the noise term (unrelated to the noise in our motion model).\n",
    "If $w_k$ is a Gaussian random variable,\n",
    "the conditional distribution for sensor measurement given the value of $h(x_k)$\n",
    "is given by\n",
    "\\begin{equation}\n",
    "\\begin{aligned}\n",
    "p(z_k|x_k) &= \\mathcal{N}(z_k;\\mu=h(x_k), \\sigma^2) \\\\\n",
    "&= \\frac{1}{\\sqrt{2\\pi\\sigma^2}} \\exp\\{-\\frac{1}{2\\sigma^2}(z_k-h(x_k))^2\\}\n",
    "\\end{aligned}\n",
    "\\end{equation}\n",
    "This approach generalizes nicely to the case of multi-dimensional sensors, as we saw\n",
    "for the case of GPS-like sensors with Gaussian noise.\n",
    "\n",
    "There are, of course, many problems for which the uncertainty cannot be adequately modeled using\n",
    "Gaussian distributions. For example, a Gaussian distribution, which has a single mode,\n",
    "cannot adequately model a multi-model distribution.\n",
    "A classic example of this situation is a robot in a long hallway that senses an office door;\n",
    "the robot has a strong belief that it is in front of a door, but no way to know which\n",
    "door.\n",
    "This situation corresponds to a probability distribution with modes at locations that are in front of office doors.\n",
    "In this chapter, we saw two ways to represent complex probability distributions: grids and samples.\n",
    "In the case of grids, we merely decompose the state space into a grid, and assign to each grid\n",
    "cell a value that corresponds to the probability that the state lies in that cell.\n",
    "In the case of samples, the situation is less structured.\n",
    "Instead of a uniform grid, sampling-based approaches represent the probability distribution by\n",
    "a collection of weighted samples (also called *particles*). The value of the sample specifies a state,\n",
    "and the weight approximates the probability mass associated to a local neighborhood of the sample.\n",
    "While grid-based representations grow exponentially with the dimension of the state space,\n",
    "sampling-based approaches are much more efficient, but require the availability of methods\n",
    "that can generate good sets of samples.\n",
    "\n",
    "Finally, in addition to introducing these methods for dealing with uncertainty,\n",
    "we also developed a simple geometric model for wheeled robot locomotion,\n",
    "specifically for the case of robots with omni-wheels.\n",
    "In particular, we developed the differential relationships between the rotation of the\n",
    "robot's wheels, and the instantaneous velocity of the robot.\n",
    "There is, of course, uncertainty associated to this motion model; however,\n",
    "rather than explicitly consider this uncertainty, we merely bundled up all\n",
    "of the uncertainties associated with robot motion into the noise parameter\n",
    "$w_k$. This simplification leads to efficient computation, but it also has\n",
    "a fairly firm theoretical basis in the Central Limit Theorem,\n",
    "a well-known theorem from probability that essentially ensures\n",
    "that the aggregate of many sources of uncertainty can be well-characterized\n",
    "using a Gaussian distribution (there are, of course, many caveats and conditions, \n",
    "which we will not consider here)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "U_TppsJF2EHG",
   "metadata": {},
   "source": [
    "## Reasoning\n",
    "\n",
    "In Chapters 2 and 3, we introduced Bayes Theorem and controlled Markov chains.\n",
    "In this chapter, we combined these two ideas to develop the Bayesian filter,\n",
    "and showed how it can be used to estimate the state of a robot that collects\n",
    "sensor data while executing motion commands under uncertain conditions.\n",
    "At each time step, the Bayesian filter can be understood as a two-stage process:\n",
    "in the first phase, the motion model is  used to calculate a *predictive distribution*,\n",
    "and in the second phase sensor data is used to upgrade the predictive distribution to the *filtering distribution*.\n",
    "\\begin{equation}\n",
    "\\begin{align*}\n",
    "\\mathbf{Prediction ~ Phase:} &~~~\n",
    "P(X_{k}|\\mathcal{Z}^{k-1},\\mathcal{U}^{k})=\\sum_{x_{k-1}}P(X_{k}|x_{k-1},u_{k-1})P(x_{k-1}|\\mathcal{Z}^{k-1},\\mathcal{U}^{k-1}) \\\\\n",
    "\\mathbf{Measurement ~ Phase:} &~~~\n",
    "P(X_{k}| \\mathcal{Z}^{k},\\mathcal{U}^{k})\n",
    "\\propto L(X_{k};z_{k})P(X_{k}|\\mathcal{Z}^{k-1},\\mathcal{U}^{k})\n",
    "\\end{align*}\n",
    "\\end{equation}\n",
    "These equations are obtained by applying the Markov property to effectively\n",
    "decouple past action and sensing histories via the marginalizing summation in the prediction phase.\n",
    "\n",
    "While elegant and compact, in the general case (i.e., for arbitrary probability distributions), direct, exact\n",
    "implementation of the Bayes filter is untenable.\n",
    "In this chapter, we introduced three ways to deal with this fact:\n",
    "Markov localization,\n",
    "Monte Carlo localization using particle filtering, and Kalman filtering.\n",
    "The former two are approximation schemes that are applicable for arbitrary probability distributions, while the latter is\n",
    "an exact method that is applicable\n",
    "only for linear systems under Gaussian uncertainty.\n",
    "\n",
    "Markov localization approximates the robot's state space using a grid representation,\n",
    "and assigning to each grid cell an approximation of the probability that the robot's state lies in that cell.\n",
    "At each time step, the filter updates the probabilities assigned to every cell in the grid.\n",
    "In practice, it is possible to ignore cells whose probability values are sufficiently small,\n",
    "and this can significantly improve the run-time performance of the approach.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "139ayrg6n433",
   "metadata": {},
   "source": [
    "Monte Carlo localization uses a set of weighted particles to approximate the probability distribution,\n",
    "hence the name *particle filtering*.\n",
    "Instead of evaluating the prediction equation in the Bayes filter, particle filtering\n",
    "generates a set of samples from the predictive distribution\n",
    "\\begin{equation}\n",
    "\\{ x_k^{(t)} \\}_{t = 1 \\dots S} \\sim \\sum_s w_{k-1}^{(s)} P(x_{k}|x_{k-1}^{(s)},u_{k-1})P(x_{k-1}^{(s)}|\\mathcal{Z}^{k-1},\\mathcal{U}^{k-1}).\n",
    "\\end{equation}\n",
    "and then weights these samples according to the measurement likelihood, yielding an approximation to\n",
    "the filtering distribution:\n",
    "\\begin{equation}\n",
    "p(X_{k}|\\mathcal{Z}^{k},\\mathcal{U}^{k}) \\approx \\{(x_k^{(t)}, L(x_k^{(t)};z_{k}))\\}_{t = 1 \\dots S}.\n",
    "\\end{equation}\n",
    "\n",
    "In contrast to Markov and Monte Carlo localization, The Kalman filter is an exact method that can\n",
    "be applied in the special case of linear motion and measurement models under Gaussian uncertainty.\n",
    "While these conditions may seem to be severely limiting, they are satisfied (or approximately satisfied)\n",
    "by many robotic systems.\n",
    "Furthermore, linearization methods can often be used to derive local, linear approximations\n",
    "to nonlinear systems.\n",
    "In this chapter, we did not provide a detailed derivation of the Kalman filtering equations,\n",
    "preferring instead to demonstrate how the Kalman filter can be implemented as the\n",
    "solution to a least-squares problem using Factor graphs."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "_GIFjHBgvYi0",
   "metadata": {},
   "source": [
    "Equipped with several solutions to the localization problem,\n",
    "we showed have value iteration could be used to construct a value function\n",
    "to solve navigation problems.\n",
    "The approach that we presented is closely aligned with Markov localization,\n",
    "in that the state space is represented by a grid,\n",
    "and each grid cell is assigned a value that approximates the value function\n",
    "for that cell.\n",
    "Value iteration proceeds by updating the entire grid at each iteration, in much\n",
    "the same style as Markov localization."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "yN_gkco6d9f-",
   "metadata": {},
   "source": [
    "Throughout this chapter, we used conditional Gaussian distributions to represent motion and measurement\n",
    "models.\n",
    "As we have seen in previous chapters, Gaussian distributions are parameterized by a mean vector\n",
    "and covariance matrix.\n",
    "In this chapter, we developed methods to estimate these parameters for the case of linear systems.\n",
    "\n",
    "We began by developing a maximum likelihood parameter estimation approach\n",
    "for a linear, scalar measurement model,\n",
    "$z_k = h(x_k) + n_k = C x_k + n_k$.\n",
    "The result was what we might have expected based on elementary statistics:\n",
    "\\begin{equation}\n",
    "\\begin{align*}\n",
    "\\hat{\\mu} &= \\frac{1}{N} \\sum_i z_i \\\\\n",
    "\\hat{C} &= \\frac{\\sum_k x_k z_k}{\\sum_k x_k^2}\\\\\n",
    "\\widehat{\\sigma^2} &= \\frac{1}{N-1} \\sum_k (\\hat{C} x_k - z_k)^2\n",
    "\\end{align*}\n",
    "\\end{equation}\n",
    "We then extended these ideas to develop a general method for multivariate linear regression\n",
    "in the case of a linear measurement model with zero-mean Gaussian noise,\n",
    "and vector-valued measurements $z_k \\in \\mathbb{R}^m$.\n",
    "In this case, we represent the measurement model as $z_k = H x_k + n_k$, and\n",
    "we assume that $n_k$ is zero-mean Gaussian with covariance matrix $R = \\sigma^2 I$.\n",
    "the estimates for $H$ and $\\sigma^2$ are given by\n",
    "\\begin{equation}\n",
    "\\begin{align*}\n",
    "\\hat{\\mu} &= 0\\\\\n",
    "\\hat{H}^T &= (\\sum_k x_k x_k^T)^{-1} \\sum_k x_k z_k^T\\\\\n",
    "\\widehat{\\sigma^2} &= \\frac{1}{(m N-m n)} \\sum_k \\sum_i |\\hat{H}_i x_k - z_{ki}|^2\n",
    "\\end{align*}\n",
    "\\end{equation}\n",
    "in which $N$ is the number of data points.\n",
    "While the derivation for these equations is a bit more complex (these are, after all, matrix equations),\n",
    "the similarity between the scalar maximum likelihood estimate and the general multivariate linear\n",
    "regression estimates are conspicuous,\n",
    "and furthermore, the extension to motion model estimation was straightforward.\n",
    "\n",
    "We concluded our tour of parameter estimation methods by briefly introducing the EM algorithm.\n",
    "The Expectation Maximization algorithm is appropriate when model parameters must\n",
    "be estimated using uncertain state estimates.\n",
    "In such cases, we can alternate between optimizing our estimate of state using the current parameter estimate\n",
    "and optimizing the parameter estimation using the current state estimate.\n",
    "While the derivations (and proof of convergence) lie beyond the scope of this book,\n",
    "the basic idea is intuitive."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "vJK4mjT6NXjE",
   "metadata": {},
   "source": [
    "## Background and History\n",
    "\n",
    "A detailed description of the kinematics of omnidirectional mobile robots is given in [Introduction to Autonomous Mobile Robots](https://mitpress.mit.edu/9780262015356/introduction-to-autonomous-mobile-robots/) by Siegwart, Nourbakhsh, Scaramuzza {cite:p}`Siegwart11book_robots`\n",
    "\n",
    "A great reference for the probability-based localization methods discussed in this chapter is the book on [Probabilistic Robotics](https://mitpress.mit.edu/9780262201629/probabilistic-robotics/) {cite:p}`Thrun05book_probabilistic`, which also discusses Markov Localization in detail. [Monte Carlo Localization](https://www.ri.cmu.edu/pub_files/pub1/dellaert_frank_1999_2/dellaert_frank_1999_2.pdf) was introduced at the 1999 ICRA conference by one of us, in {cite:p}`Dellaert99icra_mcl`.\n",
    "\n",
    "Maximum likelihood parameter estimation and the Expectation-Maximization algorithm are covered in {cite:p}`duda2012pattern`.\n",
    "The seminal work on POMDPs is reported in {cite:p}`SmaSon73` for the finite horizon case,\n",
    "and in {cite:p}`Son78` for the infinite horizon case.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "L1A8DLtlOrFI",
   "metadata": {},
   "source": []
  }
 ],
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "include_colab_link": true,
   "name": "S40_logistics_intro.ipynb",
   "provenance": []
  },
  "interpreter": {
   "hash": "c6e4e9f98eb68ad3b7c296f83d20e6de614cb42e90992a65aa266555a3137d0d"
  },
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "latex_metadata": {
   "affiliation": "Georgia Institute of Technology",
   "author": "Frank Dellaert and Seth Hutchinson",
   "title": "Introduction to Robotics"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}