{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "** code and text not yet correct - ignore!!! **\n",
    "\n",
    "## Erroneous Measurements\n",
    "\n",
    "If you can trust your sensor to never malfunction or provide a spurious measurement you are lucky. Skip to the next section. The reality is that sensors are imperfect. Stray voltages affect signals, birds fly in front of distance sensors. Computer vision sensors mistake shadows for pedestrians. And so on. \n",
    "\n",
    "Entire books are devoted to this topic. I've found that books which address multi-target tracking with radar to be particularly useful. I'll just address a couple of points that will greatly improve your filters with only a tiny amount of theory and code. \n",
    "\n",
    "We've discussed the *likelihood* function. Recall Bayes rule:\n",
    "\n",
    "$$\\text{posterior} = \\frac{\\text{likelihood}\\times\\text{prior}}{\\text{evidence}}$$\n",
    "\n",
    "where the likelihood is defined as\n",
    "\n",
    "$$\\mathcal L = p(\\mathbf z \\mid \\mathbf x)$$\n",
    "\n",
    "That is simply the likelihood (probability) of the measurements given the prior $\\mathbf x$. This suggests a trivial *gating* function. Here *gating* means a function that accepts or rejects the measurement based on some criteria. We assume that the measurement noise is Gaussian. Recall from the Gaussians chapter that 99.7% of all values fall within 3 standard deviations of the mean. If the measurement  $\\mathbf z > 3 \\sigma$ we can discard it as being very unlikely. Easy.\n",
    "\n",
    "In practice you probably shouldn't do that. We *model* the sensors as being Gaussian, but in practice they probably aren't. The tails might be thick, it might be a slightly different distribution such as Students t-distribution, and so on. For example, NASA uses between  $5\\sigma$ and $6\\sigma$ to accommodate the true behavior of the sensors on the Orion mission. Chances are your budget is less than NASA's, but I suspect the stakes are commensurately less. There is no way to analytically determine the correct value. You will need to run experiments with your data to see what value is reasonable for your application."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from math import sqrt\n",
    "\n",
    "def gated_fusion(pos_data, vel_data, wheel_std, ps_std, gate=3.):\n",
    "    kf = KalmanFilter(dim_x=2, dim_z=1)\n",
    "    kf.F = array([[1., 1.], [0., 1.]])\n",
    "    kf.H = array([[1., 0.], [1., 0.]])\n",
    "    kf.x = array([[0.], [1.]])\n",
    "    kf.P *= 100\n",
    "\n",
    "    xs, ts = [],  []\n",
    "    \n",
    "    # copy data for plotting\n",
    "    zs_wheel = np.array(vel_data)\n",
    "    zs_ps = np.array(pos_data)\n",
    "                     \n",
    "    last_t = 0\n",
    "    while len(pos_data) > 0 and len(vel_data) > 0:\n",
    "        if pos_data[0][0] < vel_data[0][0]:\n",
    "            t, z = pos_data.pop(0)\n",
    "            dt = t - last_t\n",
    "            last_t = t\n",
    "            p_index = 0\n",
    "            \n",
    "            kf.H = np.array([[1., 0.]])\n",
    "            kf.R[0,0] = ps_std**2\n",
    "            si\n",
    "        else:\n",
    "            t, z = vel_data.pop(0)\n",
    "            dt = t - last_t\n",
    "            last_t = t\n",
    "            p_index = 1\n",
    "            \n",
    "            kf.H = np.array([[0., 1.]])\n",
    "            kf.R[0,0] = wheel_std**2\n",
    "\n",
    "        kf.F[0,1] = dt\n",
    "        kf.Q = Q_discrete_white_noise(2, dt=dt, var=.02)\n",
    "        kf.predict()\n",
    "        std = sqrt(kf.P[p_index, p_index])\n",
    "        y = abs(kf.residual_of(z)) \n",
    "        if  y <= std * gate:\n",
    "            kf.update(np.array([z]))\n",
    "        else:\n",
    "            print('skip', p_index, kf.x.T, kf.P.diagonal(), \"%.3f\" % z, y)\n",
    "\n",
    "\n",
    "        xs.append(kf.x.T[0])\n",
    "        ts.append(t)\n",
    "\n",
    "    plot_fusion(xs, ts, zs_ps, zs_wheel)\n",
    "    \n",
    "random.seed(1123)\n",
    "pos_data, vel_data = gen_sensor_data(25, 1.5, .3)\n",
    "gated_fusion(pos_data, vel_data, 1.5, 3.0, gate=4.);\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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}