Kalman2.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from numpy import pi, cos, sin, sqrt, diag\n",
    "from numpy.linalg import inv\n",
    "from numpy.random import randn\n",
    "\n",
    "\n",
    "t = np.linspace(0, 2*pi, 100)\n",
    "dt = t[1] - t[0]\n",
    "\n",
    "# position\n",
    "x = 2*cos(t)\n",
    "y = sin(2*t)\n",
    "\n",
    "# velocity\n",
    "dxdt = -2*sin(t)\n",
    "dydt = 2*cos(2*t)\n",
    "\n",
    "# accel\n",
    "d2xdt2 = -2*cos(t)\n",
    "d2ydt2 = -4*sin(2*t)\n",
    "\n",
    "# jerk\n",
    "d3xdt3 = 2*sin(t)\n",
    "d3ydt3 = -8*cos(2*t)\n",
    "\n",
    "\n",
    "fig, axs = plt.subplots(2, 2, figsize=(10,10))\n",
    "\n",
    "#plt.plot(t_mem, co.T[0][0])\n",
    "axs[0, 0].plot(x, y, label=\"Pos\")\n",
    "axs[0, 0].set_xlabel('Tempo (s)')\n",
    "axs[0, 0].set_ylabel('Posição (m)')\n",
    "axs[0, 0].legend(loc=\"upper right\")\n",
    "axs[0, 0].grid(True)\n",
    "\n",
    "axs[0, 1].plot(t, dxdt, label=\"Vel X (m/s)\")\n",
    "axs[0, 1].plot(t, dydt, label=\"Vel Y (m/s)\")\n",
    "axs[0, 1].set_xlabel('Tempo (s)')\n",
    "axs[0, 1].legend(loc=\"upper right\")\n",
    "axs[0, 1].grid(True)\n",
    "\n",
    "axs[1, 0].plot(t, d2xdt2, label=\"Aceleração X (m/s^2)\")\n",
    "axs[1, 0].plot(t, d2ydt2, label=\"Aceleração Y (m/s^2)\")\n",
    "axs[1, 0].set_xlabel('Tempo (s)')\n",
    "axs[1, 0].set_ylabel('Aceleração')\n",
    "axs[1, 0].legend(loc=\"upper right\")\n",
    "axs[1, 0].grid(True)\n",
    "\n",
    "axs[1, 1].plot(t, d3xdt3, label=\"Jerk X (m/s^3)\")\n",
    "axs[1, 1].plot(t, d3ydt3, label=\"Jerk Y (m/s^3)\")\n",
    "axs[1, 1].set_xlabel('Tempo (s)')\n",
    "axs[1, 1].set_ylabel('Posição (m)')\n",
    "axs[1, 1].legend(loc=\"upper right\")\n",
    "axs[1, 1].grid(True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# angular speed (scalar)\n",
    "omega = (dxdt*d2ydt2 - dydt*d2xdt2) / (dxdt**2 + dydt**2)\n",
    "\n",
    "# speed (scalar)\n",
    "speed = sqrt(dxdt**2 + dydt**2)\n",
    "\n",
    "# measurement error\n",
    "gps_sig = 0.1\n",
    "omega_sig = 0.3\n",
    "speed_sig = 0.1\n",
    "\n",
    "# noisy measurements\n",
    "x_gps = x + gps_sig * randn(*x.shape)\n",
    "y_gps = y + gps_sig * randn(*y.shape)\n",
    "omega_sens = omega + omega_sig * randn(*omega.shape)\n",
    "speed_sens = speed + speed_sig * randn(*speed.shape)\n",
    "\n",
    "fig, axs = plt.subplots(2, 2, figsize=(10,10))\n",
    "\n",
    "#plt.plot(t_mem, co.T[0][0])\n",
    "axs[0, 0].scatter(x_gps, y_gps,color ='r', label=\"Pos (GPS)\")\n",
    "axs[0, 0].plot(x, y, label=\"Pos\")\n",
    "axs[0, 0].set_xlabel('Tempo (s)')\n",
    "axs[0, 0].set_ylabel('Posição (m)')\n",
    "axs[0, 0].legend(loc=\"upper right\")\n",
    "axs[0, 0].grid(True)\n",
    "\n",
    "axs[0, 1].scatter(t, speed_sens,color ='r', label=\"Vel medida (m/s)\")\n",
    "axs[0, 1].plot(t, speed, label=\"Velocidade\")\n",
    "axs[0, 1].set_xlabel('Tempo (s)')\n",
    "axs[0, 1].legend(loc=\"upper right\")\n",
    "axs[0, 1].grid(True)\n",
    "\n",
    "axs[1, 0].scatter(t, omega_sens,color ='r', label=\"Aceleração Y (m/s^2)\")\n",
    "axs[1, 0].plot(t, omega, label=\"Velocidade angular (º/s)\")\n",
    "axs[1, 0].set_xlabel('Tempo (s)')\n",
    "axs[1, 0].set_ylabel('(º/s)')\n",
    "axs[1, 0].legend(loc=\"upper right\")\n",
    "axs[1, 0].grid(True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "A = np.array([\n",
    "    [1, dt, (dt**2)/2, 0, 0, 0],\n",
    "    [0, 1, dt, 0, 0, 0],\n",
    "    [0, 0, 1, 0, 0, 0],\n",
    "    [0, 0, 0, 1, dt, (dt**2)/2],\n",
    "    [0, 0, 0, 0, 1, dt],\n",
    "    [0, 0, 0, 0, 0, 1],\n",
    "    ])\n",
    "\n",
    "Q1 = np.array([(dt**3)/6, (dt**2)/2, dt, 0, 0, 0])\n",
    "Q1 = np.expand_dims(Q1, 1)\n",
    "Q2 = np.array([0, 0, 0, (dt**3)/6, (dt**2)/2, dt])\n",
    "Q2 = np.expand_dims(Q2, 1)\n",
    "\n",
    "j_var = max(np.var(d3xdt3), np.var(d3ydt3))\n",
    "Q = j_var * (Q1 @ Q1.T + Q2 @ Q2.T)\n",
    "\n",
    "H = np.array([\n",
    "    [1, 0, 0, 0, 0, 0],\n",
    "    [0, 0, 0, 1, 0, 0],\n",
    "    ])\n",
    "\n",
    "R = diag(np.array([gps_sig**2, gps_sig**2]))\n",
    "\n",
    "x_init = np.array([x[0], dxdt[0], d2xdt2[0], y[0], dydt[0], d2ydt2[0]])\n",
    "P_init = 0.01 * np.eye(len(x_init))  # small initial prediction error"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# create an observation vector of noisy GPS signals\n",
    "observations = np.array([x_gps, y_gps]).T\n",
    "\n",
    "# matrix dimensions\n",
    "nx = Q.shape[0]\n",
    "ny = R.shape[0]     \n",
    "nt = observations.shape[0]\n",
    "\n",
    "# allocate identity matrix for re-use\n",
    "Inx = np.eye(nx)\n",
    "\n",
    "# allocate result matrices\n",
    "x_pred = np.zeros((nt, nx))      # prediction of state vector\n",
    "P_pred = np.zeros((nt, nx, nx))  # prediction error covariance matrix\n",
    "x_est = np.zeros((nt, nx))       # estimation of state vector\n",
    "P_est = np.zeros((nt, nx, nx))   # estimation error covariance matrix\n",
    "K = np.zeros((nt, nx, ny))       # Kalman Gain\n",
    "\n",
    "# set initial prediction\n",
    "x_pred[0] = x_init\n",
    "P_pred[0] = P_init\n",
    "\n",
    "# for each time-step...\n",
    "for i in range(nt):\n",
    "\n",
    "    # prediction stage\n",
    "    if i > 0:\n",
    "        x_pred[i] = A @ x_est[i-1]\n",
    "        P_pred[i] = A @ P_est[i-1] @ A.T + Q\n",
    "\n",
    "    # estimation stage\n",
    "    y_obs = observations[i]\n",
    "    K[i] = P_pred[i] @ H.T @ inv((H @ P_pred[i] @ H.T) + R)\n",
    "    x_est[i] = x_pred[i] + K[i] @ (y_obs - H @ x_pred[i])\n",
    "    P_est[i] = (Inx - K[i] @ H) @ P_pred[i]\n",
    "    \n",
    "    \n",
    "\n",
    "plt.plot(x_est[:, 0], x_est[:, 3], label='Pos estimada')\n",
    "plt.scatter(x_gps, y_gps,color ='r', label=\"Pos (GPS)\", s=10)\n",
    "plt.plot(x, y, label=\"Pos real\")\n",
    "plt.xlabel('X (m)')\n",
    "plt.ylabel('Y (m)')\n",
    "plt.grid(True)\n",
    "plt.legend(loc=\"upper right\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def eval_h(x_pred):\n",
    "\n",
    "    # expand prediction of state vector\n",
    "    px, vx, ax, py, vy, ay = x_pred\n",
    "\n",
    "    # compute angular vel\n",
    "    w = (vx*ay - vy*ax) / (vx**2 + vy**2)\n",
    "\n",
    "    # compute speed\n",
    "    s = sqrt(vx**2 + vy**2)\n",
    "\n",
    "    return np.array([px, py, w, s])\n",
    "\n",
    "def eval_H(x_pred):\n",
    "\n",
    "    # expand prediction of state vector\n",
    "    px, vx, ax, py, vy, ay = x_pred\n",
    "    V2 = vx**2 + vy**2\n",
    "\n",
    "    # angular vel partial derivs\n",
    "    dwdvx = (V2*ay - 2*vx*(vx*ay-vy*ax)) / (V2**2)\n",
    "    dwdax = -vy / V2\n",
    "    dwdvy = (-V2*ax - 2*vy*(vx*ay-vy*ax)) / (V2**2)\n",
    "    dwday = vx / V2\n",
    "\n",
    "    # speed partial derivs\n",
    "    dsdvx = vx / sqrt(V2)\n",
    "    dsdvy = vy / sqrt(V2)\n",
    "\n",
    "    return np.array([\n",
    "        [1, 0, 0, 0, 0, 0],\n",
    "        [0, 0, 0, 1, 0, 0],\n",
    "        [0, dwdvx, dwdax, 0, dwdvy, dwday],\n",
    "        [0, dsdvx, 0, 0, dsdvy, 0],\n",
    "        ])\n",
    "\n",
    "# redefine R to include speedometer and gyro variances\n",
    "R = diag(np.array([gps_sig**2, gps_sig**2, omega_sig**2, speed_sig**2]))\n",
    "\n",
    "# create an observation vector of all noisy signals\n",
    "observations = np.array([x_gps, y_gps, omega_sens, speed_sens]).T\n",
    "\n",
    "# matrix dimensions\n",
    "nx = Q.shape[0]\n",
    "ny = R.shape[0]     \n",
    "nt = observations.shape[0]\n",
    "\n",
    "# allocate identity matrix for re-use\n",
    "Inx = np.eye(nx)\n",
    "\n",
    "# allocate result matrices\n",
    "x_pred = np.zeros((nt, nx))      # prediction of state vector\n",
    "P_pred = np.zeros((nt, nx, nx))  # prediction error covariance matrix\n",
    "x_est = np.zeros((nt, nx))       # estimation of state vector\n",
    "P_est = np.zeros((nt, nx, nx))   # estimation error covariance matrix\n",
    "K = np.zeros((nt, nx, ny))       # Kalman Gain\n",
    "\n",
    "# set initial prediction\n",
    "x_pred[0] = x_init\n",
    "P_pred[0] = P_init\n",
    "\n",
    "# for each time-step...\n",
    "for i in range(nt):\n",
    "\n",
    "    # prediction stage\n",
    "    if i > 0:\n",
    "        x_pred[i] = A @ x_est[i-1]\n",
    "        P_pred[i] = A @ P_est[i-1] @ A.T + Q\n",
    "\n",
    "    # estimation stage\n",
    "    y_obs = observations[i]\n",
    "    y_pred = eval_h(x_pred[i])\n",
    "    H_pred = eval_H(x_pred[i])\n",
    "    K[i] = P_pred[i] @ H_pred.T @ inv((H_pred @ P_pred[i] @ H_pred.T) + R)\n",
    "    x_est[i] = x_pred[i] + K[i] @ (y_obs - y_pred)\n",
    "    P_est[i] = (Inx - K[i] @ H_pred) @ P_pred[i]\n",
    "    \n",
    "plt.plot(x_est[:, 0], x_est[:, 3], label='Pos estimada')\n",
    "plt.scatter(x_gps, y_gps,color ='r', label=\"Pos (GPS)\", s=10)\n",
    "plt.plot(x, y, label=\"Pos real\")\n",
    "plt.xlabel('X (m)')\n",
    "plt.ylabel('Y (m)')\n",
    "plt.grid(True)\n",
    "plt.legend(loc=\"upper right\")\n",
    "plt.show()"
   ]
  },
  {
   "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
}