Simple Kalman Filter

KalmanFilters.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simple 1D Kalman Tutorial \n",
    "\n",
    "Implement a simple Kalman filter.\n",
    "\n",
    "## Measurment update equations\n",
    "\n",
    "$\\mu' = \\frac{1}{r^2\\sigma^2} * [\\mu * r^2 + v * \\sigma^2]$\n",
    "\n",
    "$\\sigma'^2 = \\frac{1}{\\frac{1}{r^2} + \\frac{1}{sigma^2}}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(12.4, 1.6)\n"
     ]
    }
   ],
   "source": [
    "def update(mu1, var1, mu2, var2):\n",
    "    next_mu    = 1.0 / (var1 + var2) * (mu1 * var2 + mu2 * var1)\n",
    "    next_sigma = 1.0 / ((1.0 / var2) + (1.0 / var1))\n",
    "    return next_mu, next_sigma\n",
    "\n",
    "\n",
    "print(update(10, 8, 13, 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Prediction update equations\n",
    "\n",
    "$\\mu' = \\mu' + u$\n",
    "\n",
    "$\\sigma'^2 = \\sigma'^2 + r^2$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(22, 8)\n"
     ]
    }
   ],
   "source": [
    "def predict(mu1, var1, mu2, var2):\n",
    "    next_mu = mu1 + mu2\n",
    "    next_sigma = var1 + var2\n",
    "    return next_mu, next_sigma\n",
    "\n",
    "\n",
    "print(predict(10, 4, 12, 4))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    " ## Kalman estimation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Update:  4.998000799680128 3.9984006397441023\n",
      "Predict: 5.998000799680128 4.998400639744102\n",
      "Update:  5.99911130859809 2.221906243057098\n",
      "Predict: 6.99911130859809 3.221906243057098\n",
      "Update:  6.9995077801500045 1.784518455168001\n",
      "Predict: 8.999507780150005 2.784518455168001\n",
      "Update:  8.999709798209999 1.6416896636470566\n",
      "Predict: 9.999709798209999 2.641689663647057\n",
      "Update:  9.999825224119345 1.5909744642880308\n",
      "Predict: 10.999825224119345 2.590974464288031\n"
     ]
    }
   ],
   "source": [
    "measurments = [5,6,7,9,10]\n",
    "motion = [1,1,2,1,1]\n",
    "measurment_sig = 4\n",
    "motion_sig = 1\n",
    "mu = 0\n",
    "sig =  10000\n",
    "\n",
    "for i in range(len(measurments)):\n",
    "    mu, sig = update(mu, sig, measurments[i], measurment_sig)\n",
    "    print('Update:  {} {}'.format(mu, sig))\n",
    "    mu, sig = predict(mu, sig, motion[i], motion_sig)\n",
    "    print('Predict: {} {}'.format(mu, sig))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simple Multivariate Kalman Filter\n",
    "\n",
    "## Measurement Update equations\n",
    "\n",
    "The definitions for the state space:\n",
    "\n",
    "+ $x$: state estimate\n",
    "+ $F$: state transitions (mapping from state to state)\n",
    "+ $u$: motion vector\n",
    "+ $P$: uncertainty       (uncertainty of transition)\n",
    "\n",
    "Then the motion update (prediction) is:\n",
    "+ $x' = Fx+u$\n",
    "+ $P' = FPF^T$\n",
    "\n",
    "The definitions for the observation space:\n",
    "\n",
    "+ $z$: the sensor measurement\n",
    "+ $H$: measurment function mapping from state space into measurment space\n",
    "+ $R$: measurement noise \n",
    "\n",
    "Then the residual error between the measurment and the state update is: $y=z - Hx'$\n",
    "The Kalman gain (scaler on the error) is defined as:\n",
    "+ $S = HPH^T+R$\n",
    "+ $K = PH^{T}S^{-1}$\n",
    "\n",
    "The measurement update is:\n",
    "\n",
    "+ $x' = x + Ky$\n",
    "+ $P' = [I _ KH] P$\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "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.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}