Appendix - GraphSLAM Derivation.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## GraphSLAM Derivation\n",
    "\n",
    "The belief function associated with a collection of measurements with uncertainties is expressed as a product of probabilities. Assuming a gaussian uncertainty associated with both the movements and the landmark sensing measurements, this becomes a product of gaussian functions; each exponent contains the constraint imposed by a measurement or movement. For instance, moving 10 units to the right from $x_1$ to $x_2$ produces a constraint $x_2-x_1-10=0$, where \"10\" has an uncertainty $\\sigma$ to it. Then the gaussian is written as $\\exp(-(x_2-x_1-10)^2/2\\sigma^2)$, where sigma is now the half-width of the gaussian. The values of $x_1$ and $x_2$ that yield a constraint value of 0 maximizes the gaussian and therefore the belief function.\n",
    "\n",
    "Say that $N$ constraints (measurements or movements) have been collected, then the belief function can be expressed as\n",
    "\n",
    "\\begin{equation}\n",
    "\\prod_{i=0}^N \\exp(-\\frac{(\\text{constraint}_i)^2}{2\\sigma^2})\n",
    "\\end{equation}\n",
    "\n",
    "We want to use calculus to determine the combination of constraint values that maximizes the belief, but taking the derivative of a product of many gaussians with many unknowns produces a mess. Fortunately we can use the logarithm function, since it is monotonic, to simplify the belief function. The same values maximize the belief function and its logarithm. The logarithm converts the product to a sum: \n",
    "\n",
    "\\begin{equation}\n",
    "-\\log(\\text{belief}) = \\sum_{i=0}^N(\\text{constraint}_i)^2/2\\sigma^2\n",
    "\\end{equation}\n",
    "\n",
    "Let the uncertainties differ between the move function and the landmark sensing function, such that the uncertainties are $\\sigma_{mo}$ and $\\sigma_{ls}$. Further, to simplify the appearance of the equations, let weight $w_{mo} = 1/2\\sigma_{mo}^2$, and similarly for weight $w_{ls}$. Setting the partials with respect to all unknowns (i.e., the gradient) to zero and solving determines the extrema of the belief function, i.e., the values of the unknowns that maximize it. One ends up with a system of N linear equations in N unknowns that can be solved with linear algebra, where 'N' is the number of moves plus the number of landmark measurements.\n",
    "\n",
    "For example, in a 1D world, say we have three robot poses, and one landmark whose distance is measured at each pose. This yields six constraint equations (the initial pose may or may not have an uncertainty associated with it depending on how it was determined; here we associate one):\n",
    "\n",
    "\\begin{equation}\n",
    "x_0 - A = 0, x_1 - x_0 - B = 0, x_2 - x_1 - C = 0\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "L_0 - x_0 - D = 0, L_0 - x_1 - E = 0, L_0 - x_2 - F = 0\n",
    "\\end{equation}\n",
    "\n",
    "Plugging these into the $-\\log(\\text{belief})$ equation looks like this:\n",
    "\n",
    "\\begin{equation}\n",
    "-\\log(\\text{belief}) = w_{mo}(x_0 - A)^2 + w_{mo}(x_1 - x_0 - B)^2 + w_{mo}( x_2 - x_1 - C)^2 + \\\\\n",
    "                         w_{ls}(L_0 - x_0 - D)^2 + w_{ls}(L_0 - x_1 - E)^2 + w_{ls}(L_0 - x_2 - F)^2\n",
    "\\end{equation}\n",
    "\n",
    "Applying the partial derivatives with respect to $x_0$, $x_1$, $x_2$, and $L_0$, and then setting them to zero, yields a system of four equations (2's are already canceled):\n",
    "\n",
    "\\begin{align*}\n",
    "{\\partial}/{\\partial x_0} &\\to & w_{mo}(x_0 - A) - w_{mo}(x_1 - x_0 - B) - w_{ls}(L_0 - x_0 - D) = 0 \\\\\n",
    "{\\partial}/{\\partial x_1} &\\to & w_{mo}(x_1 - x_0 - B) - w_{mo}(x_2 - x_1 - C) - w_{ls}(L_0 - x_1 - E) = 0 \\\\\n",
    "{\\partial}/{\\partial x_2} &\\to & w_{mo}(x_2 - x_1 - C) - w_{ls}(L_0 - x_2 - F) = 0 \\\\\n",
    "{\\partial}/{\\partial L_0} &\\to & w_{ls}(3L_0 - (x_0+x_1+x_2+D+E+F)) = 0\n",
    "\\end{align*}\n",
    "\n",
    "Collecting coefficients for each term yields a system of equations that can be expressed as a matrix equation:\n",
    "\n",
    "\\begin{equation}\n",
    "\\large \\Omega \\mu = \\xi \\implies \\mu = \\Omega^{-1} \\xi.\n",
    "\\end{equation}\n",
    "\n",
    "These equations provide the basis for the Graph SLAM algorithm; $\\mu$ is the vector of unknowns we wish to solve for. None of the derivation above needs to be reproduced in code, but only provides the basis for the algorithm. At this point all that is needed is to construct $\\Omega$ and $\\xi$, invert $\\Omega$, and perform the matrix multiplication. Almost too easy!\n",
    "\n",
    "For our example,\n",
    "\n",
    "$\n",
    "\\quad\n",
    "{\\large\\Omega} = \\begin{bmatrix}\n",
    "2w_{mo}+w_{ls} & -w_{mo} & 0 & -w_{ls} \\\\\n",
    "-w_{mo} & 2w_{mo} + w_{ls} & -w_{mo} & -w_{ls} \\\\\n",
    "0 & -w_{mo} & w_{mo} + w_{ls} & -w_{ls} \\\\\n",
    "-w_{ls} & -w_{ls} & -w_{ls} & 3w_{ls}\n",
    "\\end{bmatrix}\\quad\n",
    "{\\large\\mu} = \\begin{bmatrix} x_0 \\\\ x_1 \\\\ x_2 \\\\ L_0\n",
    "\\end{bmatrix}\\quad\n",
    "$\n",
    "and\n",
    "$\n",
    "\\quad\n",
    "{\\large\\xi} = \n",
    "\\begin{bmatrix}\n",
    "w_{mo}(A-B) - w_{ls}D \\\\\n",
    "w_{mo}(B-C) - w_{ls}E \\\\\n",
    "w_{mo}C - w_{ls}F \\\\\n",
    "w_{ls}(D+E+F)\n",
    "\\end{bmatrix}\n",
    "$\n",
    "\n",
    "Note that $\\Omega$ is symmetric. You may also notice other patterns. $\\Omega$ and $\\xi$ can be constructed together by processing one constraint at a time. How may be determined by simply noticing how each constraint modifies $\\Omega$ and $\\xi$.\n",
    "\n",
    "A move constraint like $x_i - x_{i-1} - X$ adds weight $w_{mo}$ to both $\\Omega[i,i]$ and $\\Omega[i-1,i-1]$, $-w_{mo}$ to $\\Omega[i-1,i]$ and $\\Omega[i,i-1]$, $(w_{mo}X)$ to $\\xi[i]$, and $(-w_{mo}X)$ to $\\xi[i-1]$ where $X$ is the distance of the move.\n",
    "\n",
    "A landmark measurement constraint like $L_i - x_j - Y$, where $Y$ is the measured distance to the landmark, adds weight $w_{ls}$ to $\\Omega[j,j]$ and $\\Omega[i+N,i+N]$, $-w_{ls}$ to $\\Omega[j,i+N]$ and $\\Omega[i+N,j]$, $(w_{ls}Y)$ to $\\xi[i+N]$ and $-w_{ls}Y$ to $\\xi[j]$, assuming the landmark positions are located in $\\Omega$ and $\\xi$ after $N$ measured robot poses ($N=3$ in the example).\n",
    "\n",
    "## Extension to 2D\n",
    "\n",
    "The $y$ coordinate is considered independent from the $x$ coordinate. Each constraint equation in 1D becomes two constraint equations in 2D, so $\\Omega$ and $\\xi$ double in size. It doesn't matter where in the two matrices the $y$ constraints are put, as long as it's consistent. Alternatively, one can use the same $\\Omega$ matrix for both $x$ and $y$ but separate $\\xi$ vectors, e.g., $\\xi_x$ and $\\xi_y$.\n",
    "\n",
    "## Online SLAM\n",
    "\n",
    "If a robot operates over an extended period of time, adding poses and landmarks to its $\\Omega$ and $\\xi$ matrices all the while, these structures can get to be large enough to exceed memory limitations. At the same time, larger and larger matrices take more time to invert and multiply, so the computation time will get longer. These are not viable conditions for online operation. Fortunately it turns out that one can simply keep only the latest pose information, with no loss of accuracy, as long as one properly accounts for the prior information that has accumulated. The method is explained in [this video](https://www.youtube.com/watch?v=jaeNlxhQL1I). An additional bonus is that the implementation becomes a bit more of a proper challenge."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.7.4"
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}