localization_exercises/4_7_SLAM/2_2. Include Landmarks, solution.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Updating Constraint Matrices\n",
    "\n",
    "To implement Graph SLAM, a matrix and a vector (omega and xi, respectively) are introduced. The matrix is square and labelled with all the robot poses (xi) and all the landmarks (Li). Every time you make an observation, for example, as you move between two poses by some distance `dx` and can relate those two positions, you can represent this as a numerical relationship in these matrices.\n",
    "\n",
    "Below you can see a matrix representation of omega and a vector representation of xi.\n",
    "\n",
    "<img src='images/omega_xi.png' width=20% height=20% />\n",
    "\n",
    "\n",
    "### Solving for x, L\n",
    "\n",
    "To \"solve\" for all these poses and landmark positions, we can use linear algebra; all the positional values are in the vector `mu` which can be calculated as a product of the inverse of omega times xi.\n",
    "\n",
    "---\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Constraint Updates\n",
    "\n",
    "In the below code, we construct `omega` and `xi` constraint matrices, and update these according to landmark sensor measurements and motion.\n",
    "\n",
    "#### Sensor Measurements\n",
    "\n",
    "When you sense a distance, `dl`, between a pose and a landmark, l, update the constraint matrices as follows:\n",
    "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `l`\n",
    "* Add `-dl` and `dl` to xi at the rows for `xt` and `l`\n",
    "\n",
    "The values 2 instead of 1 indicate the \"strength\" of the measurement.\n",
    "\n",
    "You'll see three new `dl`'s as new inputs to our function `Z0, Z1, Z2`, below.\n",
    "\n",
    "#### Motion\n",
    "When your robot moves by some amount `dx` update the constraint matrices as follows:\n",
    "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `xt+1`\n",
    "* Add `-dx` and `dx` to xi at the rows for `xt` and `xt+1`\n",
    "\n",
    "## QUIZ: Include three new sensor measurements for a single landmark, L."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "\n",
    "def mu_from_positions(initial_pos, move1, move2, Z0, Z1, Z2):\n",
    "    \n",
    "    ## TODO: construct constraint matrices\n",
    "    ## and add each position/motion constraint to them\n",
    "    \n",
    "    # initialize constraint matrices with 0's\n",
    "    # Now these are 4x4 because of 3 poses and a landmark\n",
    "    omega = np.zeros((4,4))\n",
    "    xi = np.zeros((4,1))\n",
    "    \n",
    "    # add initial pose constraint\n",
    "    omega[0][0] = 1\n",
    "    xi[0] = initial_pos\n",
    "    \n",
    "    # account for the first motion, dx = move1\n",
    "    omega += [[1., -1., 0., 0.],\n",
    "              [-1., 1., 0., 0.],\n",
    "              [0., 0., 0., 0.],\n",
    "              [0., 0., 0., 0.]]\n",
    "    xi += [[-move1],\n",
    "           [move1],\n",
    "           [0.],\n",
    "           [0.]]\n",
    "    \n",
    "    # account for the second motion\n",
    "    omega += [[0., 0., 0., 0.],\n",
    "              [0., 1., -1., 0.],\n",
    "              [0., -1., 1., 0.],\n",
    "              [0., 0., 0., 0.]]\n",
    "    xi += [[0.],\n",
    "           [-move2],\n",
    "           [move2],\n",
    "           [0.]]\n",
    "    \n",
    "    \n",
    "    ## TODO: Include three new sensor measurements for the landmark, L\n",
    "    # incorporate first sense\n",
    "    omega += [[1., 0., 0., -1.],\n",
    "              [0., 0., 0., 0.],\n",
    "              [0., 0., 0., 0.], \n",
    "              [-1., 0., 0., 1.]]\n",
    "    xi += [[-Z0],\n",
    "           [0.0],\n",
    "           [0.0],\n",
    "           [Z0]]\n",
    "\n",
    "    # incorporate second sense\n",
    "    omega += [[0., 0., 0., 0.],\n",
    "              [0., 1., 0., -1.],\n",
    "              [0., 0., 0., 0.], \n",
    "              [0., -1., 0., 1.]]\n",
    "    xi += [[0.],\n",
    "           [-Z1],\n",
    "           [0.],\n",
    "           [Z1]]\n",
    "    \n",
    "    # incorporate third sense\n",
    "    omega += [[0., 0., 0., 0.],\n",
    "              [0., 0., 0., 0.],\n",
    "              [0., 0., 1., -1.], \n",
    "              [0., 0., -1., 1.]]\n",
    "    xi += [[0.],\n",
    "           [0.],\n",
    "           [-Z2],\n",
    "           [Z2]]\n",
    "    \n",
    "    # display final omega and xi\n",
    "    print('Omega: \\n', omega)\n",
    "    print('\\n')\n",
    "    print('Xi: \\n', xi)\n",
    "    print('\\n')\n",
    "    \n",
    "    ## TODO: calculate mu as the inverse of omega * xi\n",
    "    ## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse\n",
    "    omega_inv = np.linalg.inv(np.matrix(omega))\n",
    "    mu = omega_inv*xi\n",
    "    return mu\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Omega: \n",
      " [[ 3. -1.  0. -1.]\n",
      " [-1.  3. -1. -1.]\n",
      " [ 0. -1.  2. -1.]\n",
      " [-1. -1. -1.  3.]]\n",
      "\n",
      "\n",
      "Xi: \n",
      " [[-18.]\n",
      " [ -3.]\n",
      " [  1.]\n",
      " [ 17.]]\n",
      "\n",
      "\n",
      "Mu: \n",
      " [[-3.]\n",
      " [ 2.]\n",
      " [ 5.]\n",
      " [ 7.]]\n"
     ]
    }
   ],
   "source": [
    "# call function and print out `mu`\n",
    "mu = mu_from_positions(-3, 5, 3, 10, 5, 2)\n",
    "print('Mu: \\n', mu)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "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.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}