Laboratoire 6.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Laboratoire 6\n",
    "## Filtre de Kalman Étendu (EKF)\n",
    "\n",
    "### Partie 1 - Calibration du capteur"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The autoreload extension is already loaded. To reload it, use:\n",
      "  %reload_ext autoreload\n"
     ]
    }
   ],
   "source": [
    "# Importation des modules\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "%matplotlib inline\n",
    "\n",
    "import os.path\n",
    "import numpy as np\n",
    "import math\n",
    "import time\n",
    "from matplotlib import gridspec\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib as mpl\n",
    "from robmob.robot import Robot\n",
    "from robmob.sensors import SharpSensor\n",
    "from IPython.display import clear_output"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Modifiez la valeur de `ip_robot` selon le robot qui vous a été assigné."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Connexion au robot\n",
    "ip_robot = '192.168.0.111'\n",
    "robot = Robot(ip_robot)\n",
    "robot.connect()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "ename": "WebSocketConnectionClosedException",
     "evalue": "socket is already closed.",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mWebSocketConnectionClosedException\u001b[0m        Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-4-cd9bc80d5f8e>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[0msharp_sensor_id\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;36m0\u001b[0m \u001b[0;31m# 0 ou 1 selon votre robot\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      2\u001b[0m \u001b[0msharp_sensor\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mSharpSensor\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0msharp_sensor_id\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 3\u001b[0;31m \u001b[0mrobot\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0madd_sensor\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0msharp_sensor\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
      "\u001b[0;32m/home/david/repos/glo4001/robmob/robot.py\u001b[0m in \u001b[0;36madd_sensor\u001b[0;34m(self, sensor)\u001b[0m\n\u001b[1;32m     38\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     39\u001b[0m     \u001b[0;32mdef\u001b[0m \u001b[0madd_sensor\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0msensor\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 40\u001b[0;31m         \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mws\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msend\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mjson\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mdumps\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0msensor\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msubscription_message\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     41\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     42\u001b[0m         \u001b[0;32mif\u001b[0m \u001b[0msensor\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mTOPIC\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msensors\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/home/david/environments/robmob/lib/python3.5/site-packages/websocket/_app.py\u001b[0m in \u001b[0;36msend\u001b[0;34m(self, data, opcode)\u001b[0m\n\u001b[1;32m    113\u001b[0m         \"\"\"\n\u001b[1;32m    114\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 115\u001b[0;31m         \u001b[0;32mif\u001b[0m \u001b[0;32mnot\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msock\u001b[0m \u001b[0;32mor\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msock\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msend\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdata\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mopcode\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;34m==\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    116\u001b[0m             \u001b[0;32mraise\u001b[0m \u001b[0mWebSocketConnectionClosedException\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m\"Connection is already closed.\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    117\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/home/david/environments/robmob/lib/python3.5/site-packages/websocket/_core.py\u001b[0m in \u001b[0;36msend\u001b[0;34m(self, payload, opcode)\u001b[0m\n\u001b[1;32m    237\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    238\u001b[0m         \u001b[0mframe\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mABNF\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcreate_frame\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mpayload\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mopcode\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 239\u001b[0;31m         \u001b[0;32mreturn\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msend_frame\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mframe\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    240\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    241\u001b[0m     \u001b[0;32mdef\u001b[0m \u001b[0msend_frame\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mframe\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/home/david/environments/robmob/lib/python3.5/site-packages/websocket/_core.py\u001b[0m in \u001b[0;36msend_frame\u001b[0;34m(self, frame)\u001b[0m\n\u001b[1;32m    262\u001b[0m         \u001b[0;32mwith\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mlock\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    263\u001b[0m             \u001b[0;32mwhile\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 264\u001b[0;31m                 \u001b[0ml\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0m_send\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdata\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    265\u001b[0m                 \u001b[0mdata\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0ml\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    266\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/home/david/environments/robmob/lib/python3.5/site-packages/websocket/_core.py\u001b[0m in \u001b[0;36m_send\u001b[0;34m(self, data)\u001b[0m\n\u001b[1;32m    423\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    424\u001b[0m     \u001b[0;32mdef\u001b[0m \u001b[0m_send\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 425\u001b[0;31m         \u001b[0;32mreturn\u001b[0m \u001b[0msend\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msock\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    426\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    427\u001b[0m     \u001b[0;32mdef\u001b[0m \u001b[0m_recv\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mself\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mbufsize\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/home/david/environments/robmob/lib/python3.5/site-packages/websocket/_socket.py\u001b[0m in \u001b[0;36msend\u001b[0;34m(sock, data)\u001b[0m\n\u001b[1;32m    107\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    108\u001b[0m     \u001b[0;32mif\u001b[0m \u001b[0;32mnot\u001b[0m \u001b[0msock\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 109\u001b[0;31m         \u001b[0;32mraise\u001b[0m \u001b[0mWebSocketConnectionClosedException\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m\"socket is already closed.\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    110\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    111\u001b[0m     \u001b[0;32mtry\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mWebSocketConnectionClosedException\u001b[0m: socket is already closed."
     ]
    }
   ],
   "source": [
    "sharp_sensor_id = 0 # 0 ou 1 selon votre robot\n",
    "sharp_sensor = SharpSensor(sharp_sensor_id)\n",
    "robot.add_sensor(sharp_sensor)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Utilisez cette cellule pour vérifier que vous avez bien choisis le bon capeut IR. Pour ce laboratoire on utilise le capteur IR longue portée."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "data = sharp_sensor.read_buffer()\n",
    "print(data.shape)\n",
    "print(\"La lecture moyenne est de %0.2f volts\" % np.average(data))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Calibration\n",
    "Prenez une série de mesures séparées de 5 cm, entre 30 cm et 120 cm. Prenez note que le buffer prend environ 2 secondes à ce remplir, donc prenez votre temps entre les mesurese.\n",
    "Soyez attentifs, car ces mesures\n",
    "serviront à calibrer votre capteur infrarouge et aussi à tester votre filtre de Kalman.\n",
    "\n",
    "Dans la cellule suivante, utilisez ```t``` (take) pour prendre la mesure et ```r``` (retry) pour reprendre la mesure. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "xs = np.array([x for x in range(30, 125, 5)]) * 0.01 #Distances à calibrer en mètres\n",
    "zs = []"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Capture des données de calibration\n",
    "\n",
    "for x in xs:\n",
    "    key_in = ''\n",
    "    while key_in != 't':\n",
    "        buf = sharp_sensor.read_buffer()\n",
    "        z = np.mean(buf)\n",
    "        print(\"mesure pour x = %f\" % x)\n",
    "        print(\"moyenne de %d mesures: %f\" % (len(buf), z))\n",
    "        print(\"(t)ake / (r)etry\")\n",
    "        key_in = input()\n",
    "    zs.append(z)\n",
    "    clear_output()\n",
    "    time.sleep(0.5)\n",
    "\n",
    "zs = np.asarray(zs)\n",
    "\n",
    "print(xs.shape, zs.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Sauvegarde des zs de calibration sur le disque (au cas où vous redémarriez le notebook)\n",
    "np.savetxt(\"./lab6_calib.txt\", np.vstack((xs, zs)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "xs, zs = np.loadtxt(\"./lab6_calib.txt\")\n",
    "print(xs.shape, zs.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def calibrate(xs, zs):\n",
    "    \"\"\"\n",
    "    Args:\n",
    "      - xs: distances en mètres des mesures zs\n",
    "      - zs: mesures en Volts\n",
    "    Return:\n",
    "      - Fonction linéaire qui prend en entrée l'inverse de la distance en mètres et retourne une mesure en Volts \n",
    "        (c'est la fonction de capteur)\n",
    "      - Les coefficients de K_1 et K_2 de la fonction linéaire de capteur\n",
    "    \"\"\"\n",
    "    xs_inverse_meters = 1.0 / xs\n",
    "    coeffs = np.polyfit(xs_inverse_meters, zs, deg=1)\n",
    "    sensor_function = np.poly1d(coeffs)\n",
    "    return sensor_function, coeffs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "sensor_fn, coeffs = calibrate(xs, zs)\n",
    "k_1_sharp = coeffs[1]\n",
    "k_2_sharp = coeffs[0]\n",
    "print(\"K_1: %f\" % k_1_sharp)\n",
    "print(\"K_2: %f\" % k_2_sharp)\n",
    "\n",
    "#Plot calibration\n",
    "xs_inv = 1.0 / xs\n",
    "plt.scatter(xs_inv, zs)\n",
    "plt.plot(xs_inv, sensor_fn(xs_inv))\n",
    "plt.xlabel(\"1/x [1/m]\")\n",
    "plt.ylabel(\"z (V)\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![kalman-ir](./img/kalman-ir.png)\n",
    "\n",
    "\n",
    "Comme illustré à l’image ci-haut, le capteur infrarouge fonctionne essentiellement avec l’inverse de la\n",
    "distance. Il sera donc approximé par la fonction suivante :\n",
    "\n",
    "$$\n",
    "f_{infrarouge}(x) = K_{1 Sharp} + \\frac{K_{2 Sharp}}{x}, x > 30\n",
    "$$\n",
    "\n",
    "Comme cette fonction est\n",
    "bijective, on peut trouver un estimé de distance $x$ directement à partir de la mesure infrarouge $z$, avec\n",
    "l’inverse de la fonction :\n",
    "\n",
    "$$\n",
    "f^{-1}_{infrarouge}(z) = \\frac{K_{2 Sharp}}{z - K_{1 Sharp}}\n",
    "$$\n",
    "\n",
    "Vous aurez besoin de cette fonction pour initialiser l’estimé $X$ d’un filtre quand vous ne connaissez pas\n",
    "la position de départ du système. Elle sera aussi utilisée par le script pour comparer directement les\n",
    "mesures du capteur infrarouge avec vos estimés. **Notez que ces équations (1) et (2) ne sont valides que\n",
    "pour $x$ > 0.30 m : votre robot ne pourra donc jamais être à moins de 0.30 m du mur**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 2 - Modèle du système\n",
    "Comme estimé de bruit sur le capteur infrarouge, nous allons prendre la valeur de $\\sigma^2_{infrarouge} = (0.02V)^2$, ce qui correspond approximativement à un palier du convertisseur analogique-numérique. Le bruit de\n",
    "déplacement sera $\\sigma^2_{pas} = (0.002 m)^2$, ce qui donne essentiellement un écart-type de 2 mm sur un pas du\n",
    "robot. Fait à noter, le choix de ces valeurs influencera grandement le comportement du filtre. Ainsi, si\n",
    "vous choisissez un estimé de bruit de capteur $\\sigma^2_{infrarouge}$\n",
    "plus petit, vous indiquez au filtre qu’il doit\n",
    "augmenter sa confiance envers les mesures du capteur. De la même manière, si vous diminuez la valeur\n",
    "estimée du bruit sur les commandes $\\sigma^2_{pas}$, il augmentera sa confiance envers l’estimé $x(k+1\\,|\\,k)$ obtenu lors\n",
    "de la phase de prédiction. Nous y reviendrons lors des manipulations.\n",
    "\n",
    "> **NOTE** Le bruit associé à la conversion analogique-numérique est généralement modélisé comme une distribution uniforme entre\n",
    "±un demi-palier de conversion. Pour les curieux, une discussion complète de ce sujet est disponible sur wikipedia :\n",
    "http://en.wikipedia.org/wiki/Quantization_error\n",
    "\n",
    "La dynamique du système et la commande sont linéaires pour ce système:\n",
    "\n",
    "$$\n",
    "x(k + 1\\,|\\,k) = x(k) + u(k)\n",
    "$$\n",
    "\n",
    "Ce qui nous donne les matrices ```F``` (ou $\\Phi$ sur les acétates) $= [1]$ et ```G``` (ou $\\Gamma$ sur les acétates) $= [1]$. La fonction de mesure, telle qu'exprimée précédemment, est quant à elle non-linéaire:\n",
    "\n",
    "$$\n",
    "f_{infrarouge}(x) = K_{1 Sharp} + \\frac{K_{2 Sharp}}{x}, x > 30\n",
    "$$\n",
    "\n",
    "Pour l’utiliser dans un filtre de Kalman, nous allons donc devoir linéariser cette fonction autour du point\n",
    "d’opération $x$ (la position estimée du robot). La jacobienne ```H``` (ou $\\Lambda$ sur les acétates) est utilisé par le\n",
    "filtre de Kalman étendu (EKF) pour effectuer cette linéarisation :\n",
    "\n",
    "$$\n",
    "H = \\Big[\\frac{\\delta}{\\delta x} f_{infrarouge}(x)\\Big] = \\Big[ \\frac{-K_{2Sharp}}{x^2}\\Big]\n",
    "$$\n",
    "\n",
    "Cette matrice ```H``` (ou $\\Lambda$ sur les acétates) est de taille $n\\times m$, où $n$ est le nombre de capteurs et $m$ la longueur\n",
    "du vecteur d’état. Pour ce système, sa taille est de 1x1, car nous avons une variable d’état (la position),\n",
    "et un seul capteur (infrarouge Sharp). \n",
    "\n",
    "> **IMPORTANT!** La valeur numérique de ```H``` est recalculée à chaque\n",
    "itération par le filtre EKF, car la pente de la fonction du capteur dépend de la position $x$. Aussi, le filtre\n",
    "utilise l’estimé ```X``` pour la calculer, car nous ne connaissons pas la vraie valeur de $x$. C’est d’ailleurs une\n",
    "des sources d’échec du filtre : si ```X``` et $x$ sont très différents, alors la valeur de ```H``` sera incorrecte, et le filtre\n",
    "risque de diverger. Plus la fonction sera non-linéaire, et plus la distance |```X```-$x$| posera problème."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 3 - Collecte de données\n",
    "\n",
    "Il est possible d’utiliser un filtre de manière offline, c’est-à-dire après que les données soient toutes\n",
    "capturées par le robot. Ceci a l’avantage de faciliter votre familiarisation avec ce filtre.\n",
    "\n",
    "Dans cette partie, nous allons collecter des données que nous allons ensuite utiliser dans la partie suivante sur le filtre de Kalman.\n",
    "\n",
    "Placez votre robot à la position de départ 0.30 m. Le script suivant fait avancer le robot à coups de 0.05m en enregistrant à chaque fois la mesure de l'infrarouge."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "delta_t = 1       # intervalle des mesures en sec\n",
    "speed = 0.05      # m/s\n",
    "\n",
    "x_0 = 0.3\n",
    "x_final = 1.20\n",
    "distance = (x_final - x_0)\n",
    "n_steps = math.ceil(distance / speed / delta_t)\n",
    "step_distance = delta_t * speed\n",
    "\n",
    "positions = np.zeros((n_steps,))\n",
    "measures = np.zeros((n_steps,))\n",
    "\n",
    "for i in range(n_steps):\n",
    "    positions[i] = x_0 + i * step_distance\n",
    "    measures[i] = sharp_sensor.peek_data()\n",
    "    \n",
    "    \n",
    "    robot.linear_movement_precise(step_distance, speed)\n",
    "    time.sleep(0.6) #Laisser le temps au buffer du Sharp de se remplir\n",
    "\n",
    "print(np.vstack((positions, measures)))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Si vous êtes satisfait des données collectées, utilisez la cellule suivante pour les enregistrer"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "data_to_save = np.vstack((positions, measures))\n",
    "np.savetxt(\"./lab6_offline_data.txt\", data_to_save)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### Partie 4 - Filtre\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Load data\n",
    "positions, measures = np.loadtxt(\"./lab6_offline_data.txt\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "delta_t = 1          # intervalle des mesures en sec\n",
    "n_steps = positions.shape[0]\n",
    "speed = 0.05         # m/s\n",
    "sigma_sharp = 0.02   # (V) ecart-type sur voltage du capteur infrarouge\n",
    "sigma_kobuki = 0.006 # (m) ecart-type sur un pas du robot kobuki\n",
    "\n",
    "Cv = sigma_kobuki ** 2\n",
    "Cw = sigma_sharp ** 2\n",
    "\n",
    "# Actions du système\n",
    "u = 0.05\n",
    "\n",
    "# Calibration - doit avoir été faites plus haut\n",
    "assert(k_1_sharp is not None and k_2_sharp is not None)\n",
    "\n",
    "\n",
    "X = positions[0]  # On connait exactement la position initiale du robot\n",
    "P = 0.0\n",
    "\n",
    "#Données à recueillir pour tracer les graphiques\n",
    "graph_data = {\n",
    "    \"x\": [positions[0]],\n",
    "    \"x_estimate\": [X],\n",
    "    \"z\": [measures[0]],\n",
    "    \"K\": [0],\n",
    "    \"P\": [P],\n",
    "    \"error\": [positions[0] - X],\n",
    "    \"time\": [0]\n",
    "}\n",
    "\n",
    "for i in range(1, n_steps): # On commence le compteur à 1, car 0 est l'état initial à 30cm, et 1 est après\n",
    "                            # la première commande\n",
    "    time = i * delta_t\n",
    "    \n",
    "    z = measures[i]\n",
    "    \n",
    "    F = np.array([1])       # Phi, pour dynamique systeme\n",
    "    G = np.array([1])       # Gamma, pour matrice de commande\n",
    "    \n",
    "    # Propagation de l'estimé\n",
    "    X = F * X + G * u\n",
    "    P = F * P * F.T + G * Cv * G.T  # Propagation covariance\n",
    "\n",
    "    \n",
    "    # ========= Calcul des matrices Jacobiennes pour mise-a-jour =============\n",
    "    H = -k_2_sharp / X ** 2  # Je dois utiliser mon estimé pour ici, car je ne connais pas la vraie valeur\n",
    "\n",
    "    z_hat =  k_1_sharp + k_2_sharp / X;\n",
    "\n",
    "    # ======== Mise-a-jour ========\n",
    "    K = P * H.T / (H * P * H.T + Cw)   # Gain Kalman\n",
    "    r = (z - z_hat)                    # Innovation\n",
    "    X = X + K*r\n",
    "    P = (1 - K * H) * P\n",
    "    \n",
    "    \n",
    "    # Cueillette des données pour le graphique\n",
    "    graph_data[\"x\"].append(positions[i])\n",
    "    graph_data[\"x_estimate\"].append(X)\n",
    "    graph_data[\"z\"].append(z)\n",
    "    graph_data[\"K\"].append(K)\n",
    "    graph_data[\"P\"].append(P)\n",
    "    graph_data[\"error\"].append(positions[i] - X)\n",
    "    graph_data[\"time\"].append(time)\n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 5 - Visualisation des résultats"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=(6, 12))\n",
    "gs = gridspec.GridSpec(3, 1)\n",
    "\n",
    "ax1 = plt.subplot(gs[0])\n",
    "ax1.set_title(\"Position (commandes)\")\n",
    "ax1.plot(graph_data['time'], graph_data['x'], '-o', label=\"Commandes\")\n",
    "ax1.grid(True)\n",
    "\n",
    "ax2 = plt.subplot(gs[1])\n",
    "ax2.set_title(\"Estimé de position (filtre)\")\n",
    "ax2.plot(graph_data['time'], graph_data['x_estimate'], '-o', label=\"Estimé Filtre\")\n",
    "ax2.set_ylabel('position (m)')\n",
    "ax2.grid(True)\n",
    "\n",
    "# On inverse la fonction de capteur pour trouver la position estimée\n",
    "# par le capteur infrarouge\n",
    "z_numpy = np.array(graph_data['z'])\n",
    "x_sharp_estimated = k_2_sharp / (z_numpy - k_1_sharp)\n",
    "ax3 = plt.subplot(gs[2])\n",
    "ax3.set_title(\"Estimé de position (sharp)\")\n",
    "ax3.plot(graph_data['time'], x_sharp_estimated, '-o', label=\"Estimé Sharp\")\n",
    "ax3.set_xlabel(\"temps (s)\")\n",
    "ax3.grid(True)\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plt.title(\"Position et estimées\")\n",
    "plt.xlabel(\"temps (s)\")\n",
    "plt.ylabel(\"position (m)\")\n",
    "plt.plot(graph_data['time'], graph_data['x'], label=\"Commandes\")\n",
    "plt.scatter(graph_data['time'], graph_data['x_estimate'], marker='x', label=\"Estimé Filtre\")\n",
    "plt.scatter(graph_data['time'], x_sharp_estimated, marker='o', color='red', label=\"Estimé Sharp\")\n",
    "plt.legend(loc='upper left')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plt.title(\"Erreur en fonction du temps\")\n",
    "plt.xlabel(\"temps (s)\")\n",
    "plt.ylabel(\"erreur (m)\")\n",
    "plt.scatter(graph_data['time'], graph_data['error'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "cov = np.array(graph_data['P'])*1000000\n",
    "plt.title(\"Covariance en fonction du temps\")\n",
    "plt.xlabel(\"temps (s)\")\n",
    "plt.ylabel(\"Covariance ((mm)^2)\")\n",
    "plt.scatter(graph_data['time'], cov)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plt.title(\"Gain de Kalman en fonction du temps\")\n",
    "plt.xlabel(\"temps (s)\")\n",
    "plt.ylabel(\"Gain\")\n",
    "plt.scatter(graph_data['time'], graph_data['K'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "sharp_errors = graph_data['x'] - x_sharp_estimated\n",
    "plt.title(\"Écart-type infrarouge=%.3f mm\" % np.std(sharp_errors * 1000))\n",
    "plt.hist(sharp_errors, bins=12)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "errors = np.asarray(graph_data['error'])\n",
    "plt.title(\"Écart-type filtre Kalman=%.3f mm\" % np.std(errors * 1000))\n",
    "plt.hist(errors, bins=10)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Discussion\n",
    "\n",
    "Quel comportement voyez-vous? En particulier, vous devriez observer que \n",
    "\n",
    "- la précision sur la position estimée diminue au fil du temps, ce qui reflète l’accumulation progressive des erreurs;\n",
    "\n",
    "- la matrice de covariance P reflète bien cette situation, en accroissant avec le temps;\n",
    "- la valeur absolue 2 du gain K du filtre augmente au début, car le filtre doit tenir compte de plus en plus de la mesure pour corriger l’estimé de la position à mesure que notre estimé perd de la précision.\n",
    "\n",
    "\n",
    "Normalement, la valeur de la covariance P et du gain K se stabilise, ce qui n’est pas le cas en ce\n",
    "moment. Pourquoi? Eh bien à mesure que vous vous éloignez, votre capteur perd énormément de\n",
    "précision, et la matrice P et le gain K doivent refléter cette situation. Pour un scénario plus réaliste, le\n",
    "robot resterait toujours dans les mêmes parages, et P et K convergeraient vers une valeur fixe. Rappelez-\n",
    "vous que pour ce système :\n",
    "\n",
    "$$\n",
    "K = \\frac{PH^T}{HPH^T + \\sigma^2_{infrarouge}}\n",
    "$$\n",
    "\n",
    "\n",
    "> **NOTE** Pour ce système, le gain K est négatif. La courbe montera donc une valeur descendante pour une augmentation de la valeur absolue du gain."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 6 - Trajet inverse\n",
    "Répétez l'expérience précédente mais en faisant comme si le robot était parti loin du mur pour s'en approcher. Vous pouvez utiliser les données que vous avez déjà enregistrées pour faire cette expérience.\n",
    "\n",
    "Voici comment inverser un array numpy:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "a = np.array([1, 2, 3, 4, 5])\n",
    "a_inv = a[::-1]\n",
    "print(a, a_inv)\n",
    "print(type(a_inv))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Observez les changements sur les matrices P et K, en fonction du temps."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 7 - Position initiale inconnue\n",
    "Pour ce cas, nous allons assumer que votre robot ne connaisse pas sa position au début. Pour\n",
    "refléter cette situation, changer les valeurs initiales de $X$ et $P$:\n",
    "\n",
    "$$\n",
    "X = f^{-1}_{infrarouge}(z)\n",
    "$$\n",
    "\n",
    "$$\n",
    "P = H^{-1} \\sigma^2_{infrarouge}(H^T)^{-1}\n",
    "$$\n",
    "\n",
    "La valeur de $P$ indique au filtre que la précision de l’estimé est égale à celle du capteur. Pourquoi pré- et\n",
    "post-multiplier par l’inverse de H? Parce que $\\sigma^2_{infrarouge}$\n",
    "est défini dans l’espace des mesures du capteur (en\n",
    "Volt), et qu’il faut donc estimer quelle est la précision de l’estimé de la position (en m). Cette opération\n",
    "va faire cette conversion pour nous.\n",
    "\n",
    "Enlever les modifications précédentes (celles qui faisaient tourner les données en sens inverse), et\n",
    "exécuter le filtre à nouveau. Quel comportement voyez-vous? En particulier, vous devriez observer que :\n",
    "la matrice de covariance $P$ diminue après la première itération, car on a acquis de l’information."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 8 - Erreurs biaisées\n",
    "Si la valeur moyenne des pas de votre robot ne correspond pas à $u$ (dans le scripte de la partie 4), ceci correspond à la situation\n",
    "où le bruit de déplacement est biaisé, ce qui viole les hypothèses d’utilisation de ce type de filtre. Cette\n",
    "situation est très néfaste pour le filtre, qui aura tendance à avoir une erreur beaucoup plus élevée que\n",
    "normalement. \n",
    "Pour ce test, utilisez les données du fichier de calibration à la place des données acquises automatiquement, car celle-ci sont sans biais. Autrement dit, utilisez:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "positions, measures = np.loadtxt(\"./lab6_calib.txt\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "En modifiant légèrement la valeur de $u$ dans le début du code du filtre (par exemple le faire passer de\n",
    "0.05 à 0.0475), regardez comment le filtre accumule une erreur importante : vous verrez les cercles verts\n",
    "qui s’éloignent progressivement de la valeur réelle. À la lumière de cette expérience, croyez-vous qu’il\n",
    "est important de ne pas avoir un système contenant une erreur de commande biaisé? De plus, croyez-vous que les données acquisent automatiquement étaient biaisées?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 9 - Évaluation de la variance (optionnel)\n",
    "\n",
    "Faites 8 déplacements de 10 pas de 0.05 m, en utilisant le script de la partie 3. Partez toujours du\n",
    "même endroit. Mesurez la position finale du robot avec la règle, et notez-là. Calculez l’erreur sur la\n",
    "position finale pour les 8 séries. Quelle est la variance sur cette erreur? Est-elle comparable à la variance\n",
    "estimée par le filtre?"
   ]
  }
 ],
 "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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}