06_modello_lineare_dinamico_e_filtro_di_kalman_con_numpy.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Questo notebook è fortemente ispirato all'articolo *A Kalman Filtering Tutorial for Undergraduate Students*.\n",
    "\n",
    "### Riferimenti bibliografici:\n",
    "\n",
    "* Rhudy, M., B.; Salguero, R., A. & Holappa, K. (2017), [A Kalman Filtering Tutorial for Undergraduate Students](http://aircconline.com/ijcses/V8N1/8117ijcses01.pdf)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Modello lineare dinamico e filtro di Kalman con NumPy"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Indice"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. [Modello lineare dinamico](#modello_lineare_dinamico)<br>\n",
    "2. [Filtro di Kalman](#kalman)<br>\n",
    "3. [Esempio](#esempio)<br>\n",
    "    3.1 [Oggetto in caduta libera senza resistenza aerea](#oggetto_caduta)<br>\n",
    "    3.2 [Termini del problema](#termini)<br>\n",
    "    3.3 [Generazione delle serie degli stati e degli output](#generazione)<br>\n",
    "    3.4 [Simulazione di misurazioni tramite l'aggiunta di rumore a y](#misurazioni)<br>\n",
    "    3.5 [Filtraggio di Kalman](#filtraggio)<br>\n",
    "    3.6 [Grafico degli stati veri e stimati](#grafico)<br>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "import inspect\n",
    "\n",
    "%load_ext autoreload\n",
    "%autoreload 2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 1. Modello lineare dinamico <a id=modello_lineare_dinamico> </a>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$$\n",
    "\\begin{align}\n",
    "    \\textbf{x}_k &= \\textbf{F}_{k-1}\\textbf{x}_{k-1} + \\textbf{G}_{k-1}\\textbf{u}_{k-1} + \\textbf{w}_k \\tag{1} \\\\\n",
    "    \\textbf{y}_k &= \\textbf{H}_k\\textbf{x}_k  + \\textbf{v}_k \\tag{2}\n",
    "\\end{align}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Dimensioni delle variabili del sistema a tempo discreto"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "| Variabile  | Descrizione                          | Dimensione     |\n",
    "|------------|--------------------------------------|----------------|\n",
    "|$\\textbf{x}$| Vettore di stato                     |$n_x \\times 1$  |\n",
    "|$\\textbf{y}$| Vettore di output                    |$n_y \\times 1$  |\n",
    "|$\\textbf{u}$| Vettore di input                     |$n_u \\times 1$  |\n",
    "|$\\textbf{w}$| Vettore del rumore del processo      |$n_x \\times 1$  |\n",
    "|$\\textbf{v}$| Vettore del rumore di misurazione    |$n_y \\times 1$  |\n",
    "|$\\textbf{F}$| Matrice del sistema - stato          |$n_x \\times n_x$|\n",
    "|$\\textbf{G}$| Matrice del sistema - input          |$n_x \\times n_u$|\n",
    "|$\\textbf{H}$| Matrice delle osservazioni           |$n_y \\times n_x$|\n",
    "|$\\textbf{P}$| Matrice di covarianza di $\\textbf{x}$|$n_x \\times n_x$|\n",
    "|$\\textbf{Q}$| Matrice di covarianza di $\\textbf{w}$|$n_x \\times n_x$|\n",
    "|$\\textbf{R}$| Matrice di covarianza di $\\textbf{v}$|$n_y \\times n_y$|\n",
    "|$\\textbf{z}$| Misurazione di $\\textbf{y}$          |$n_y \\times 1$  |"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 2. Filtro di Kalman <a id=kalman> </a>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Algoritmo** filtro di Kalman\n",
    "\n",
    "$\\hat{\\textbf{x}}_0$: stima iniziale del vettore di stato. <br>\n",
    "$\\hat{\\textbf{P}}_0$: stima iniziale della matrice di covarianza dell'errore di stato.\n",
    "\n",
    "1. $\\hat{\\textbf{x}}_{k|k-1} = \\textbf{F}_{k-1}\\hat{\\textbf{x}}_{k-1} + \\textbf{G}_{k-1}\\textbf{u}_{k-1},\\quad \\textit{vettore di stato previsto}$;\n",
    "2. $\\hat{\\textbf{P}}_{k|k-1} = \\textbf{F}_{k-1}\\hat{\\textbf{P}}_{k-1}\\textbf{F}^\\top_{k-1} + \\textbf{Q}_{k-1},\\quad \\textit{matrice di covarianza dell'errore di stato prevista}$;\n",
    "3. $\\textbf{K}_k = \\hat{\\textbf{P}}_{k|k-1}\\textbf{H}^\\top_k(\\textbf{H}_k\\hat{\\textbf{P}}_{k|k-1}\\textbf{H}^\\top_k + \\textbf{R}_k)^{-1},\\quad \\textit{matrice del guadagno di Kalman}$;\n",
    "4. $\\hat{\\textbf{x}}_{k} = \\hat{\\textbf{x}}_{k|k-1} + \\textbf{K}_{k}(\\textbf{z}_k - \\textbf{H}_k\\hat{\\textbf{x}}_{k|k-1}),\\quad \\textit{correzione del vettore di stato previsto}$;\n",
    "5. $\\hat{\\textbf{P}}_{k} = (\\textbf{I} - \\textbf{K}_k\\textbf{H}_k)\\hat{\\textbf{P}}_{k|k-1},\\, \\textbf{I}\\, \\text{matrice identità},\\quad \\textit{correzione della matrice di covarianza dell'errore di stato prevista}$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 3. Esempio <a id=esempio> </a>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.1 Oggetto in caduta libera senza resistenza aerea <a id=oggetto_caduta> </a>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Obiettivo**: determinare l'altezza $h$ di un oggetto in caduta libera senza resistenza aerea, in base all'informazione incerta riguardo la sua altezza iniziale e a delle misure della sua posizione a tempi successivi fornite da un telemetro laser.\n",
    "\n",
    "Sia $g$ l'accelerazione dovuta alla gravità ($g = 9.80665\\, m/s^2$), usando la cinematica delle particelle abbiamo che\n",
    "\n",
    "$\\ddot{h}(t) = -g,\\quad \\textit{accelerazione}\\, a(t) = a \\label{1}\\tag{1}$\n",
    "\n",
    "Dall'equazione che relaziona velocità e accelerazione $v(t) = \\int_{t_0}^t a(t)\\, dt$, quando $a(t)$ è costante come nel nostro caso, abbiamo che $v(t) = v(t_0) + a\\Delta t$ con $\\Delta t = t - t_0$ (e quindi $t_0 = t - \\Delta t$), otteniamo\n",
    "\n",
    "$\\dot{h}(t) =\\dot{h}(t_0) - g\\Delta t,\\quad \\textit{velocità}\\, v(t) \\label{2}\\tag{2}$\n",
    "\n",
    "Integrando di nuovo otteniamo l'equazione del *moto uniformemente accelerato* $s(t) = \\int_{t_0}^t a\\Delta t + v(t_0)\\, dt = s(t_0) + v(t_0)\\Delta t + \\frac{1}{2}a\\Delta t^2$. Nel nostro caso\n",
    "\n",
    "$h(t) = h(t_0) + \\dot{h}(t_0)\\Delta t -\\frac{1}{2}g(\\Delta t)^2,\\quad \\textit{posizione}\\, s(t) \\label{3}\\tag{3}$\n",
    "\n",
    "Passando ad un indice di tempo discreto $t = k \\Delta t$ e ponendo $h_k = h(k \\Delta t)$, possiamo riscrivere $\\eqref{2}$ e $\\eqref{3}$ come\n",
    "\n",
    "$\\dot{h}_k =\\dot{h}_{k-1} - g\\Delta t,\\quad \\textit{velocità}\\, v_k \\label{4}\\tag{4}$\n",
    "\n",
    "$h_k = h_{k-1} + \\dot{h}_{k-1}\\Delta t -\\frac{1}{2}g(\\Delta t)^2,\\quad \\textit{posizione}\\, s_k \\label{5}\\tag{5}$\n",
    "\n",
    "Siamo interessato alla stima della posizione dell'oggetto, sappiamo quindi che la posizione deve essere inclusa nel vettore di stato. Tuttavia, dato che anche la velocità appare nell'equazione della posizione, è necessario ottenere anche questa informazione. Un modo per averla è includere la velocità nel vettore di stato. Definiamo quindi il vettore di stato del filtro di Kalman come\n",
    "\n",
    "$\\textbf{x}_k = \\begin{pmatrix} h_k \\\\ \\dot{h}_k \\end{pmatrix}\\label{6}\\tag{6}$\n",
    "\n",
    "che risulta nella seguente espressione\n",
    "\n",
    "$\\textbf{x}_k = \\begin{pmatrix} h_{k-1} + \\dot{h}_{k-1}\\Delta t -\\frac{1}{2}g(\\Delta t)^2 \\\\ \\dot{h}_{k-1} - g\\Delta t \\end{pmatrix}\\label{7}\\tag{7}$\n",
    "\n",
    "Con questa definizione, possiamo riscrivere queste equazioni in termini del vettore di stato, $\\textbf{x}$, come in\n",
    "\n",
    "$\\textbf{x}_k = \\begin{pmatrix} 1 & \\Delta t \\\\ 0 & 1 \\end{pmatrix}\\textbf{x}_{k-1} + \\begin{pmatrix} -\\frac{1}{2}(\\Delta t)^2 \\\\ -\\Delta t \\end{pmatrix}g \\label{8}\\tag{8}$\n",
    "\n",
    "Abbiamo quindi il problema nella forma necessaria per applicare il filtro di Kalman\n",
    "\n",
    "$\\textbf{x}_k = \\textbf{F}_{k-1}\\textbf{x}_{k-1} + \\textbf{G}_{k-1}\\textbf{u}_{k-1} + \\textbf{w}_k \\label{9}\\tag{9}$\n",
    "\n",
    "$\\textbf{F}_{k-1} = \\begin{pmatrix} 1 & \\Delta t \\\\ 0 & 1 \\end{pmatrix},\\, \\textbf{G}_{k-1} = \\begin{pmatrix} -\\frac{1}{2}(\\Delta t)^2 \\\\ -\\Delta t \\end{pmatrix},\\, \\textbf{u}_{k-1} = g, \\textbf{w}_k = \\begin{pmatrix} 0 \\\\ 0 \\end{pmatrix} \\label{10}\\tag{10}$\n",
    "\n",
    "Ponendo $\\textbf{w}_k$ uguale a zero stiamo assumendo che non ci sia errore nell'equazione stessa. Per questo problema, questa è un'assunzione, dato che ci potrebbe essere un disturbo dovuto alla resistenza dell'aria o ad altre fonti. Se il caso generale con rumore gaussiano è $\\textbf{w}_k \\sim \\mathcal{N}(0, \\textbf{Q}_k)$, stiamo quindi ponendo\n",
    "\n",
    "$\\textbf{Q}_k = \\begin{pmatrix} 0 & 0 \\\\ 0 & 0 \\end{pmatrix} \\label{11}\\tag{11}$\n",
    "\n",
    "Consideriamo ora la parte relativa alle misurazioni del sistema.\n",
    "\n",
    "Immaginiamo uno scenario dove la posizione dell'oggetto può essere misurata con un telemetro laser con una deviazione standard dell'errore $\\textbf{v}$ pari a 2 $m$. La matrice di covarianza di $\\textbf{v}$, $\\textbf{R}$, è quindi pari a $(2\\,m)^2 = 4\\,m^2$. Notare che sia $\\textbf{v}$ che $\\textbf{R}$ sono in questo caso degli scalari essendoci solo un termine nel vettore di output $\\textbf{y}$ dato dalla relazione\n",
    "\n",
    "$\\textbf{y}_k = h_k  + \\textbf{v}_k \\label{12}\\tag{12}$\n",
    "\n",
    "che possiamo riscrivere come\n",
    "\n",
    "$\\textbf{y}_k = \\begin{pmatrix} 1 & 0 \\end{pmatrix}\\textbf{x}_k + \\textbf{v}_k \\label{13}\\tag{13}$\n",
    "\n",
    "Rispetto all'equazione generica\n",
    "\n",
    "$\\textbf{y}_k = \\textbf{H}_k\\textbf{x}_k  + \\textbf{v}_k \\label{14}\\tag{14}$\n",
    "\n",
    "nel nostro caso abbiamo che\n",
    "\n",
    "$\\textbf{H}_k = \\begin{pmatrix} 1 & 0 \\end{pmatrix} \\label{15}\\tag{15}$\n",
    "\n",
    "Sia la vera altezza inziale $h_0 = 100\\, m$ e sia la nostra stima di essa $\\hat{h}_0 = \\hat{\\textbf{x}}_0[0]  = 105\\, m$. Dato che abbiamo stimato a occhio la posizione inziale, cosideriamo una varianza relativamente alta dell'errore commesso, $\\hat{\\textbf{P}}_0[0, 0] = 10\\, m^2$. Assumiamo che l'oggetto parta da fermo $\\hat{\\dot{h}}_0 = \\hat{\\textbf{x}}_0[1] = 0\\, m/s$. Quest'assunzione è piuttosto ragionevole, possiamo quindi considerare una piccola incertezza, $\\hat{\\textbf{P}}_0[1, 1] = 0.01\\, m^2/s^2$. Infine immaginiamo di prendere $N = 1000$ misurazioni e che $\\Delta t$ sia pari a $0.001\\, s$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.2 Termini del problema <a id=termini> </a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "u = np.array([[9.80665]]) # vettore (scalare in questo caso) di input\n",
    "dt = 0.001 # tempo di campionamento (s)\n",
    "F = np.array( # matrice del sistema - stato\n",
    "    [[1, dt],\n",
    "     [0, 1]]\n",
    ")\n",
    "G = np.array( # matrice (vettore in questo caso) del sistema - input\n",
    "    [[-1 / 2 * dt ** 2],\n",
    "     [-dt]]\n",
    ")\n",
    "H = np.array([[1, 0]]) # matrice (vettore in questo caso) delle osservazioni\n",
    "Q = np.zeros((2, 2)) # matrice di covarianza dell'errore del processo w\n",
    "R = np.array([[4]]) # matrice di covarianza (scalare in questo caso) dell'errore di output v\n",
    "x0 = np.array( # vettore del vero stato inziale\n",
    "    [[100],\n",
    "     [0]]\n",
    ")\n",
    "x0_hat = np.array( # vettore di stato inziale ipotizzato\n",
    "    [[105],\n",
    "     [0]]\n",
    ")\n",
    "P0_hat = np.array([ # matrice di covarianza inziale del vettore di stato ipotizzata\n",
    "    [10, 0],\n",
    "    [0, 0.01]]\n",
    ")\n",
    "N = 1000 # numero di passi temporali\n",
    "t = dt * np.arange(0, N + 1) # vettore del tempo (s)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "from msbd.modello_lineare import ModelloLineareDinamico\n",
    "\n",
    "print(inspect.getsource(ModelloLineareDinamico))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Esercizio\n",
    "\n",
    "1. Completare il metodo `eq1` della classe `ModelloLineareDinamico` definita in `msbd/modello_lineare/modello_lineare_dinamico.py`;\n",
    "2. Completare il metodo `eq2` della classe `ModelloLineareDinamico` definita in `msbd/modello_lineare/modello_lineare_dinamico.py`;\n",
    "2. Scrivere dei test per verificare che i metodi implementati siano corretti."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.3 Generazione delle serie degli stati e degli output <a id=generazione> </a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = x0\n",
    "y = np.array([[]])\n",
    "\n",
    "for k in range(1, N + 1):\n",
    "    x_k = ModelloLineareDinamico.eq1(F, x[:, [k - 1]], G, u)\n",
    "    x = np.hstack([x, x_k])\n",
    "    \n",
    "    y_k = ModelloLineareDinamico.eq2(H, x[:, [k]])\n",
    "    y = np.hstack([y, y_k])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.4 Simulazione di misurazioni tramite l'aggiunta di rumore a y <a id=misurazioni> </a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.random.seed(42)\n",
    "\n",
    "v = np.random.multivariate_normal(np.zeros(1), cov=R, size=N).reshape(y.shape) # nota: in questo caso bastava np.random.normal\n",
    "\n",
    "z = y + v"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Esercizio\n",
    "\n",
    "1. Completare il metodo `partial_fit` della classe `ModelloLineareDinamico` definita in `msbd/modello_lineare/modello_lineare_dinamico.py`;\n",
    "2. Scrivere dei test e verificare che il metodo implementato sia corretto."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.5 Filtraggio di Kalman <a id=filtraggio> </a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fk = ModelloLineareDinamico(x0_hat, P0_hat)\n",
    "\n",
    "for k in range(0, N):\n",
    "    fk.partial_fit(F, G, u, H, Q, R, z[:, [k]])\n",
    "    \n",
    "h_hat = np.array(fk.x_hat_)[:, 0, 0] # array delle altezze\n",
    "v_hat = np.array(fk.x_hat_)[:, 1, 0] # array delle velocità"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.6 Grafico degli stati veri e stimati <a id=grafico> </a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure(figsize=(8, 6))\n",
    "\n",
    "plt.suptitle(\"Esempio di filtraggio di Kalman, stati veri e stimati\")\n",
    "\n",
    "plt.subplot(211)\n",
    "plt.plot(t[1:], z[0, :], color=\"lime\", label=\"Misurata\")\n",
    "plt.plot(t, h_hat, ls='--', color=\"blue\", lw=3, label=\"Stimata\")\n",
    "plt.plot(t, x[0, :], ls=':', color=\"red\", lw=2, label=\"Vera\")\n",
    "plt.ylabel(\"$x_0 = altezza\\, (m)$\")\n",
    "plt.grid(ls='--')\n",
    "plt.legend()\n",
    "\n",
    "plt.subplot(212)\n",
    "plt.plot(t, v_hat, ls='--', color=\"blue\", lw=3, label=\"Stimata\")\n",
    "plt.plot(t, x[1, :], ls=':', color=\"red\", lw=2, label=\"Vera\")\n",
    "plt.xlabel(\"secondi ($s$)\")\n",
    "plt.ylabel(\"$x_1 = velocità \\, (m/s)$\")\n",
    "plt.grid(ls='--')\n",
    "plt.legend()\n",
    "\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.6.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}