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",
   "metadata": {},
   "source": [
    "# Importation des modules\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "%matplotlib inline\n",
    "\n",
    "import math\n",
    "import time\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "from IPython.display import clear_output\n",
    "from matplotlib import gridspec\n",
    "\n",
    "from robmob.robot import Robot\n",
    "from robmob.rover.sensors import LDLidarSensor, RobotEspSensor\n",
    "from robmob.rover.commands import MovementFloatCommand, ResetCommand"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Modifiez la valeur de `ip_robot` selon le robot qui vous a été assigné."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "# Connexion au robot\n",
    "ip_robot = 'localhost'\n",
    "robot = Robot(ip_robot)\n",
    "robot.connect()"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "lidar = LDLidarSensor()\n",
    "robot.add_sensor(lidar)"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Utilisez cette cellule pour vérifier que votre LiDAR est bien branché et retourne des données.\n",
    "Dans le cas où la cellule suivante retourne `(0,)`, assurez-vous que le LiDAR est branché."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "data = lidar.read_buffer()\n",
    "print(data.shape)"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "robot.send_command(ResetCommand())"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Mesure de distance avec le LiDAR\n",
    "\n",
    "Comme capteur, vous utiliserez le capteur LiDAR sur le dessus de la plateforme robotique.\n",
    "Afin de ne retourner que la distance vers l'avant du véhicule, utilisez les fonctions suivantes."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "# Conversion des données du LiDAR en coordonnées 3D (sera vu plus en détail dans le laboratoire 7)\n",
    "def lidar_to_3d_homogeneous_matrix(lidar_data, cone_angle_degrees=10):\n",
    "    ranges = np.array(lidar_data['ranges'])\n",
    "    thetas = np.arange(lidar_data['angle_min'], lidar_data['angle_max']+lidar_data['angle_increment'], lidar_data['angle_increment'])[:len(ranges)]\n",
    "    xs = ranges * np.cos(thetas)\n",
    "    ys = ranges * np.sin(thetas)\n",
    "    \n",
    "    pts = np.vstack((xs, ys, np.zeros(xs.shape), np.ones(xs.shape))).T\n",
    "\n",
    "    # Keep points in front\n",
    "    delta = np.deg2rad(cone_angle_degrees)\n",
    "    mask_min = thetas > np.pi / 2 -  delta / 2\n",
    "    mask_max = thetas < np.pi / 2 + delta / 2\n",
    "    mask = np.bitwise_and(mask_min, mask_max)\n",
    "    \n",
    "    pts = pts[mask]\n",
    "    ranges = ranges[mask]\n",
    "    \n",
    "    # Remove the points that are at the origin.\n",
    "    nz = np.nonzero(np.logical_or(pts[:,0] != 0., pts[:,1] != 0.))\n",
    "    pts = pts[nz]\n",
    "    ranges = ranges[nz]\n",
    "    \n",
    "    return pts, ranges"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "pointcloud, ranges = lidar_to_3d_homogeneous_matrix(lidar.peek_data())"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "def plot_point_clouds(pcls):\n",
    "    STYLES = ['r+', 'bx', 'g*', 'md', 'ys']\n",
    "    \n",
    "    for i, pcl in enumerate(pcls):\n",
    "        plt.plot(pcl[:,0], pcl[:,1], STYLES[i], label='Nuage {}'.format(i))\n",
    "        plt.plot([0.0], [0.0], 'ko')\n",
    "        \n",
    "    plt.axis('equal')\n",
    "    plt.legend()\n",
    "    plt.show()"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "plot_point_clouds([pointcloud])"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "def estimate_distance(lidar_data, cone_angle_degrees=10):\n",
    "    pc, ranges = lidar_to_3d_homogeneous_matrix(lidar_data, cone_angle_degrees)\n",
    "    return np.mean(ranges)"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "estimate_distance(lidar.peek_data())"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Fonction de capteur\n",
    "\n",
    "Faites quelques mesures de distances. Vous devriez remarquer que les mesures sont assez précises.\n",
    "\n",
    "Afin de rendre le problème un peu plus intéressant, nous allons simuler un capteur de distance similaire au Sharp utilisé lors du laboratoire 1.\n",
    "Pour ce faire, nous allons passer la distance en mètres à la fonction `f_sensor` qui retournera une mesure en volts."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "# Modèle du capteur infrarouge\n",
    "# k1 et k2 sont les paramètres du modèle\n",
    "# noise est l'écart-type du bruit gaussien ajouté à la mesure\n",
    "def f_sensor(distance, k1=3, k2=1, noise=0):\n",
    "    z = k1 + k2/(distance*100) # convert to cm with *100\n",
    "    if isinstance(distance, np.ndarray):\n",
    "        z += np.random.randn(*distance.shape) * noise\n",
    "    else:\n",
    "        z += np.random.randn() * noise\n",
    "    return z"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "x = np.linspace(10, 150, 141)/100 # convert to meters with /100\n",
    "for _ in range(10):\n",
    "    plt.plot(x, f_sensor(x, noise=0.002))\n",
    "plt.title('Capteur de distance infrarouge')\n",
    "plt.xlabel('Distance (m)')\n",
    "plt.ylabel('Measure (V)')"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Collecte de données\n",
    "Prenez une série de mesures séparées de 5 cm, entre 30 cm et 120 cm (en commencant à 120 cm). Prenez note que le buffer prend environ 2 secondes à ce remplir, donc prenez votre temps entre les mesures.\n",
    "Soyez attentifs, car ces mesures\n",
    "serviront à calibrer votre capteur infrarouge et aussi à tester votre filtre de Kalman.\n",
    "Pour déplacer le robot, utilisez le notebook `Controller.ipynb`, comme lors du laboratoire 5.\n",
    "Si les déplacements ne sont pas précis, vous pouvez manuellement déplacer le robot près de la position désirée.\n",
    "Placez le centre du LiDAR à 120 cm du mur, notez qu'il y a une distance de cm entre le LiDAR et le devant du robot.\n",
    "\n",
    "Dans la cellule suivante, utilisez ```t``` (take) pour prendre la mesure et ```r``` (retry) pour reprendre la mesure. "
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "xs = np.array([x for x in range(30, 125, 5)])[::-1] * 0.01  # Distances à calibrer en mètres\n",
    "zs = []\n",
    "distances = []"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {
    "pycharm": {
     "is_executing": true
    }
   },
   "source": [
    "# Capture des données de calibration\n",
    "sigma_sensor = 0.002\n",
    "k1 = 3\n",
    "k2 = 1\n",
    "\n",
    "for x in xs:\n",
    "    key_in = ''\n",
    "    while key_in != 't':\n",
    "        distance = estimate_distance(lidar.peek_data())\n",
    "        z = f_sensor(distance, k1=k1, k2=k2, noise=sigma_sensor)\n",
    "        print(\"mesure pour x = %f\" % x)\n",
    "        print(\"moyenne des mesures: %f\" % z)\n",
    "        print(\"moyenne des distance: %f\" % distance)\n",
    "        print(\"(t)ake / (r)etry\")\n",
    "        key_in = input()\n",
    "    zs.append(z)\n",
    "    distances.append(distance)\n",
    "    clear_output()\n",
    "    time.sleep(0.5)\n",
    "\n",
    "zs = np.asarray(zs)\n",
    "\n",
    "print(xs.shape, zs.shape)"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Si vous êtes satisfait des données collectées, utilisez la cellule suivante pour les enregistrer"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "# On inverse les données pour commencer à la position la plus proche\n",
    "data_to_save = np.vstack((xs[::-1], distances[::-1], zs[::-1]))\n",
    "np.savetxt(\"./lab6_offline_data.txt\", data_to_save)"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 2 - Modèle du système\n",
    "\n",
    "Normalement il faudrait estimer le bruit sur le capteur infrarouge, dans le cas du Sharp la valeur de $\\sigma^2_{infrarouge} = (0.02V)^2$, ce qui correspond approximativement à un palier du convertisseur analogique-numérique. Dans notre cas, nous simulons un Sharp avec le LiDAR, il est donc possible d'utiliser la valeur du bruit directement. 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 Sensor} + \\frac{K_{2 Sensor}}{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_{2Sensor}}{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 4 - Filtre\n",
    "\n",
    "Assurez-vous de bien comprendre l'implémentation du filtre de Kalman.\n",
    "Comparez l'implémentation avec les équations vues en classe."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "# Load data\n",
    "positions, lidar_distances, measures = np.loadtxt(\"lab6_offline_data.txt\")"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "delta_t = 1  # intervalle des mesures en sec\n",
    "n_steps = positions.shape[0]\n",
    "speed = 0.05  # m/s\n",
    "sigma_rover = 0.0025  # (m) ecart-type sur un pas du robot rover\n",
    "\n",
    "Cv = sigma_rover ** 2\n",
    "Cw = sigma_sensor ** 2\n",
    "\n",
    "# Actions du système\n",
    "u = 0.05\n",
    "\n",
    "# Calibration - doit avoir été faites plus haut\n",
    "assert (k1 is not None and k2 is not None)\n",
    "\n",
    "X = positions[0].item()  # On connait exactement la position initiale du robot\n",
    "P = 0.00 ** 2\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].item()\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",
    "    # ========= Calcul des matrices Jacobiennes pour mise-a-jour =============\n",
    "    H = -k2 / X ** 2  # Je dois utiliser mon estimé pour ici, car je ne connais pas la vraie valeur\n",
    "\n",
    "    z_hat = k1 + k2 / (X * 100);\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",
    "    # Cueillette des données pour le graphique\n",
    "    graph_data[\"x\"].append(positions[i])\n",
    "    graph_data[\"x_estimate\"].append(X.item())\n",
    "    graph_data[\"z\"].append(z)\n",
    "    graph_data[\"K\"].append(K.item())\n",
    "    graph_data[\"P\"].append(P.item())\n",
    "    graph_data[\"error\"].append(positions[i] - X.item())\n",
    "    graph_data[\"time\"].append(time)\n"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": "### Partie 5 - Visualisation des résultats"
  },
  {
   "cell_type": "code",
   "metadata": {},
   "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 = k2 / (z_numpy - k1) / 100\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()"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "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')"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "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'])"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "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)"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "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'])"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "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()"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "errors = [float(e) for e in np.asarray(graph_data['error'])]\n",
    "plt.title(\"Écart-type filtre Kalman=%.3f mm\" % np.std(errors * 1000))\n",
    "plt.hist(errors[1:], bins=10)\n",
    "plt.show()"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Discussion\n",
    "\n",
    "**ATTENTION:** Il faut s'assurer de bien comprendre le code et de bien développer l'intuition pour le filtre.\n",
    "Essayez d'interpréter les graphiques et de comprendre pourquoi ils ont cette forme.\n",
    "**Questions**:\n",
    "- Quel est le comportement de la covariance P et du gain K?\n",
    "- Qu'est-ce qui explique ces comportements? Reférez-vous à la fonction de capteur et à la dynamique du système.\n",
    "\n",
    "Modifiez les différents paramètres pour voir comment le filtre se comporte."
   ]
  },
  {
   "metadata": {},
   "cell_type": "markdown",
   "source": [
    "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?\n",
    "\n",
    "À mesure que vous vous éloignez, votre capteur perd énormément de précision, et la matrice P et le gain K doivent refléter cette situation.\n",
    "Pour un scénario plus réaliste, le robot resterait toujours dans les mêmes parages, et P et K convergeraient vers une valeur fixe.\n",
    "Rappelez-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.\n",
    "> La courbe montera donc une valeur descendante pour une augmentation de la valeur absolue du gain.\n",
    "\n"
   ]
  },
  {
   "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",
   "metadata": {},
   "source": [
    "a = np.array([1, 2, 3, 4, 5])\n",
    "a_inv = a[::-1]\n",
    "print(a, a_inv)\n",
    "print(type(a_inv))"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "# TODO placez votre code ici\n",
    "positions, lidar_distances, measures = np.loadtxt(\"lab6_offline_data.txt\")"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Observez les changements sur les matrices P et K, en fonction du temps.\n",
    "Répondez à nouveau aux questions de la partie précédente."
   ]
  },
  {
   "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": "code",
   "metadata": {},
   "source": [
    "# TODO placez votre code ici\n",
    "positions, lidar_distances, measures = np.loadtxt(\"lab6_offline_data.txt\")"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Discussion\n",
    "\n",
    "Que ce passe-t'il si l'estimé de la position initial est très mauvais?\n",
    "- Modifiez la position initale du filtre et commentez sur la convergence du filtre.\n",
    "- En gardant la position initiale erronée, quel paramètre du filtre devrait être ajusté pour améliorer la performance du filtre?"
   ]
  },
  {
   "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",
   "metadata": {},
   "source": [
    "# TODO placez votre code ici\n",
    "positions, lidar_distances, measures = np.loadtxt(\"lab6_offline_data.txt\")"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "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é?\n",
    "De plus, croyez-vous que les données acquisent étaient biaisées?\n",
    "\n",
    "Quel paramètre du filtre devrait être ajusté pour améliorer la performance du filtre?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 9 - Faire varier le bruit du capteur\n",
    "\n",
    "Grâce à la fonction f_sensor, faites varier le bruit du capteur infrarouge. Comment cela affecte-t'il la\n",
    "performance du filtre? Pourquoi?\n",
    "\n",
    "Que se passe-t'il si le bruit du capteur est grand, mais que `Cw` est petit? Quel est l'impact sur la performance du filtre?"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "# TODO placez votre code ici"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Partie 10 - Évaluation de la variance (Optionnel)\n",
    "\n",
    "Comment pourriez-vous calculer la variance sur les déplacements?\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?\n"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "source": [
    "# TODO placez votre code ici\n",
    "positions, lidar_distances, measures = np.loadtxt(\"lab6_offline_data.txt\")"
   ],
   "outputs": [],
   "execution_count": null
  },
  {
   "metadata": {},
   "cell_type": "markdown",
   "source": [
    "# Partie 11 - Plusieurs capteurs\n",
    "Dans cette partie, nous allons ajouter un second capteur pour mesurer la position du robot. Pour ce faire, nous allons utiliser le capteur LiDAR pour simuler un second capteur. Placez le robot entre deux murs.\n",
    "- Que se passe-t'il pour le gain des deux capteurs selon la position?\n"
   ]
  }
 ],
 "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.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}