{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Extended Kalman Filter Localization"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 150680 characters]\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 2,
     "metadata": {
      "image/png": {
       "width": 600
      }
     },
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from IPython.display import Image\n",
    "Image(filename=\"ekf.png\",width=600)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![EKF](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif)\n",
    "\n",
    "This is a sensor fusion localization with Extended Kalman Filter(EKF).\n",
    "\n",
    "The blue line is true trajectory, the black line is dead reckoning\n",
    "trajectory,\n",
    "\n",
    "the green point is positioning observation (ex. GPS), and the red line\n",
    "is estimated trajectory with EKF.\n",
    "\n",
    "The red ellipse is estimated covariance ellipse with EKF.\n",
    "\n",
    "Code: [PythonRobotics/extended\\_kalman\\_filter\\.py at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_filter.py)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Filter design\n",
    "\n",
    "In this simulation, the robot has a state vector includes 4 states at time $t$.\n",
    "\n",
    "$$\\textbf{x}_t=[x_t, y_t, \\phi_t, v_t]$$\n",
    "\n",
    "x, y are a 2D x-y position, $\\phi$ is orientation, and v is velocity.\n",
    "\n",
    "In the code, \"xEst\" means the state vector. [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L168)\n",
    "\n",
    "And, $P_t$ is covariace matrix of the state,\n",
    "\n",
    "$Q$ is covariance matrix of process noise, \n",
    "\n",
    "$R$ is covariance matrix of observation noise at time $t$ \n",
    "\n",
    "　\n",
    "\n",
    "The robot has a speed sensor and a gyro sensor.\n",
    "\n",
    "So, the input vecor can be used as each time step\n",
    "\n",
    "$$\\textbf{u}_t=[v_t, \\omega_t]$$\n",
    "\n",
    "Also, the robot has a GNSS sensor, it means that the robot can observe x-y position at each time.\n",
    "\n",
    "$$\\textbf{z}_t=[x_t,y_t]$$\n",
    "\n",
    "The input and observation vector includes sensor noise.\n",
    "\n",
    "In the code, \"observation\" function generates the input and observation vector with noise [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L34-L50)\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Motion Model\n",
    "\n",
    "The robot model is \n",
    "\n",
    "$$ \\dot{x} = vcos(\\phi)$$\n",
    "\n",
    "$$ \\dot{y} = vsin((\\phi)$$\n",
    "\n",
    "$$ \\dot{\\phi} = \\omega$$\n",
    "\n",
    "\n",
    "So, the motion model is\n",
    "\n",
    "$$\\textbf{x}_{t+1} = F\\textbf{x}_t+B\\textbf{u}_t$$\n",
    "\n",
    "where\n",
    "\n",
    "$\\begin{equation*}\n",
    "F=\n",
    "\\begin{bmatrix}\n",
    "1 & 0 & 0 & 0\\\\\n",
    "0 & 1 & 0 & 0\\\\\n",
    "0 & 0 & 1 & 0 \\\\\n",
    "0 & 0 & 0 & 0 \\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "$\\begin{equation*}\n",
    "B=\n",
    "\\begin{bmatrix}\n",
    "cos(\\phi)dt & 0\\\\\n",
    "sin(\\phi)dt & 0\\\\\n",
    "0 & dt\\\\\n",
    "1 & 0\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "$dt$ is a time interval.\n",
    "\n",
    "This is implemented at [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L53-L67)\n",
    "\n",
    "Its Jacobian matrix is\n",
    "\n",
    "$\\begin{equation*}\n",
    "J_F=\n",
    "\\begin{bmatrix}\n",
    "\\frac{dx}{dx}& \\frac{dx}{dy} & \\frac{dx}{d\\phi} &  \\frac{dx}{dv}\\\\\n",
    "\\frac{dy}{dx}& \\frac{dy}{dy} & \\frac{dy}{d\\phi} &  \\frac{dy}{dv}\\\\\n",
    "\\frac{d\\phi}{dx}& \\frac{d\\phi}{dy} & \\frac{d\\phi}{d\\phi} &  \\frac{d\\phi}{dv}\\\\\n",
    "\\frac{dv}{dx}& \\frac{dv}{dy} & \\frac{dv}{d\\phi} &  \\frac{dv}{dv}\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "$\\begin{equation*}\n",
    "　=\n",
    "\\begin{bmatrix}\n",
    "1& 0 & -v sin(\\phi)dt &  cos(\\phi)dt\\\\\n",
    "0 & 1 & v cos(\\phi)dt & sin(\\phi) dt\\\\\n",
    "0 & 0 & 1 & 0\\\\\n",
    "0 & 0 & 0 & 1\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Observation Model\n",
    "\n",
    "The robot can get x-y position infomation from GPS.\n",
    "\n",
    "So GPS Observation model is\n",
    "\n",
    "$$\\textbf{z}_{t} = H\\textbf{x}_t$$\n",
    "\n",
    "where\n",
    "\n",
    "$\\begin{equation*}\n",
    "B=\n",
    "\\begin{bmatrix}\n",
    "1 & 0 & 0& 0\\\\\n",
    "0 & 1 & 0& 0\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "Its Jacobian matrix is\n",
    "\n",
    "$\\begin{equation*}\n",
    "J_H=\n",
    "\\begin{bmatrix}\n",
    "\\frac{dx}{dx}& \\frac{dx}{dy} & \\frac{dx}{d\\phi} &  \\frac{dx}{dv}\\\\\n",
    "\\frac{dy}{dx}& \\frac{dy}{dy} & \\frac{dy}{d\\phi} &  \\frac{dy}{dv}\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "$\\begin{equation*}\n",
    "　=\n",
    "\\begin{bmatrix}\n",
    "1& 0 & 0 & 0\\\\\n",
    "0 & 1 & 0 & 0\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Extented Kalman Filter\n",
    "\n",
    "Localization process using Extendted Kalman Filter:EKF is\n",
    "\n",
    "=== Predict ===\n",
    "\n",
    "$x_{Pred} = Fx_t+Bu_t$\n",
    "\n",
    "$P_{Pred} = J_FP_t J_F^T + Q$\n",
    "\n",
    "=== Update ===\n",
    "\n",
    "$z_{Pred} = Hx_{Pred}$ \n",
    "\n",
    "$y = z - z_{Pred}$\n",
    "\n",
    "$S = J_H P_{Pred}.J_H^T + R$\n",
    "\n",
    "$K = P_{Pred}.J_H^T S^{-1}$\n",
    "\n",
    "$x_{t+1} = x_{Pred} + Ky$\n",
    "\n",
    "$P_{t+1} = ( I - K J_H) P_{Pred}$\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ref:\n",
    "\n",
    "- [PROBABILISTIC\\-ROBOTICS\\.ORG](http://www.probabilistic-robotics.org/)"
   ]
  }
 ],
 "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.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
