09-MehrdimKalmTeil1.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Der mehrdimensionale Kalman-Filter, Teil 1 Grundlagen\n",
    "22.4.2024, S. Mack"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# display plots inside this notebook\n",
    "%matplotlib inline \n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Die wichtigsten Erkenntnisse des vorangegangenen Jupyter-Notebooks zu den mehrdimensionalen Gaußverteilungen waren:\n",
    "* Aus der skalaren Zufallsvariable und dem skalaren Erwartungswert werden beim Übergang von der eindimensionalen auf die mehrdimensionale Gaußverteilung Vektoren. Der Vektor $\\boldsymbol\\mu$ des Erwartungswerts besitzt für jede Zufallsvariable $i$ im entsprechenden Element den dazugehörigen Erwartungswert $\\mu_i$.\n",
    "* Aus der skalaren Standardabweichung wird im n-dimensionalen Fall eine $n \\times n$-Matrix, die sogenannte Kovarianzmatrix $\\mathbf\\Sigma$. In deren Diagonalelementen $\\Sigma_{ii}$ stecken die Varianzen der einzelnen Zufallsvariablen $X_i$. Außerhalb der Diagonalen befinden sich in den Elementen $\\Sigma_{ij}$ die Kovarianzen, also das Korrelationsausmaß zwischen den beiden Zufallsvariablen $X_i$ und $X_j$.\n",
    "* Beim Kalman-Filter sind die Zustandsgrößen die Zufallsvariablen. Auch sie werden zu einem Vektor, dem Zustandsvektor, zusammengefasst.\n",
    "* Der Korrekturschritt ist wie im eindimensionalen Fall eine Multiplikation der beiden jetzt mehrdimensionalen Gaußverteilungen von Vorhersage und Messung. Die Mathematik dazu ändert sich nicht wesentlich.\n",
    "* Die Korrelation zweier Zustandsgrößen führt dazu, dass dieser Korrekturschritt einen erheblich genaueren Schätzwert liefert als wären diese unkorreliert.\n",
    "* Selbst wenn eine Zustandsgröße nicht gemessen wird, kann ihr Wert aufgrund der Korrelation genau geschätzt werden. In diesem Fall tut man so als gäbe es diesen Messwert - jedoch mit sehr großer Standardabweichung. Die Funktion des Kalman-Filters ist in diesem Fall primär gar nicht das Filtern der Messdaten sondern das Schätzen eines Paramters des physikalischen Modells, welcher oft gar nicht mit Sensoren erfasst werden kann.  \n",
    "\n",
    "Im mehrdimensionalen Kalman-Filter **spielt die Korrelation zweier Zustandsgrößen** die zentrale Rolle.  \n",
    "\n",
    "Dieses Jupyter-Notebook behandelt nur eine spezielle Unterklasse mehrdimensionaler Kalman-Filter: Filter mit einem physikalischen Modell, welches auf den newtonschen Bewegungsgleichungen beruht. \n",
    "Hierfür wird aber die allgemeine Form eines Kalman-Filters verwendet, so dass sich an den hier vorgestellen Algorithmen und an der Mathematik nichts wesentlich ändert, falls ein Kalman-Filter z.B. für die Ladezustand einer Batterie verwendet werden soll. Wenn dann auch noch die dem physikalischen Modell zugrunde liegenden Differentialgleichungen die selbe Struktur haben, dann kann der nachfolgend entwickelte Kalman-Filter sogar 1:1 darauf übertragen werden. Beispielsweise wird aus \"Abstand\" dann die Zustandsgröße \"Ladung\" und aus \"Abstandsänderung\" die Zustandsgröße \"Strom\".\n",
    "\n",
    "In diesem Sinne werden im nächsten Kapitel zuerst einmal die newtonschen Bewegungsgleichungen wiederholt:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    " ## Die newtonschen Bewegungsgleichungen\n",
    "\n",
    "Die einfachste newtonsche Bewegungsgleichung berechnet bei bekannter Geschwindigkeit $v$ die Position $x$ in Abhängigkeit von der verstrichenen Zeit $t$:\n",
    "\n",
    "$$x(t) = vt + x_0$$\n",
    "\n",
    "Beispielsweise startet ein Fahrzeug an der Position $x_0 = 13 m$, besitzt eine konstante Geschwindigkeit $v = 10 m/s$ und fährt $t = 12 s$ lang. Am Ende dieser Zeit erreicht es somit die Position $ 133 m = 10 m/s\\cdot 12 s + 13 m$ .\n",
    "\n",
    "In einer etwas komplexeren Bewegungsgleichung besitzt das Fahrzeug zusätzlich eine Beschleunigung $a$. Dann ist nicht nur die Position $x$ sondern auch die (momentane) Geschwindigkeit $v$ von der verstrichenen Zeit $t$ abhängig:\n",
    "\n",
    "$$x(t) = \\int_0^t v(t)\\, dt + v_0t + x_0 = \\frac{1}{2}at^2 + v_0t + x_0$$\n",
    "\n",
    "Beide Bewegungsgleichungen sind Lösungen des sogenannten zweiten newtonschen Gesetzes $F = m \\cdot a$. Der Fall $a=0$ ist streng genommen das erste newtonsche Gesetz: \"Wirkt auf einen Körper keine Kraft, so ist seine Geschwindigkeit zeitlich konstant\". Somit gilt mit $v(t) = v = const.$:\n",
    "\n",
    "$$\\begin{aligned} v &= \\frac{dx}{dt}\\\\\n",
    "dx &= v\\, dt \\\\\n",
    "\\int_{x_0}^x\\, dx &= \\int_0^t v\\, dt\\\\\n",
    "x(t) - x_0 &= vt - 0\\\\\n",
    "x(t) &= vt + x_0\\end{aligned}$$\n",
    "\n",
    "**Die Ausgangsformel $v = {dx}/{dt}$ ist eine** (triviale) **Differentialgleichung**.  \n",
    "**Für die hier behandelte Unterklasse von Kalman-Filtern sind es genau solche Differentialgleichungen der Zustandsgrößen, welche die Grundlage für das physikalisches Modell bilden.** Hier sind es konkret die newtonschen Gesetze für einen Kalman-Filter, der die Bewegung des Roboterfahrzeugs tracken soll."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Kalman-Filter: Von eindimensional auf mehrdimensional\n",
    "Nachfolgend ist nochmals der grundlegende Algorithmus eines Kalman-Filters wie schon im Jupyter-Notebook des eindimensionalen Kalman-Filters grafisch dargestellt.  \n",
    "\n",
    "<img src=\"grafikKalman_VorhersKorr.png\" width=\"800\">   \n",
    "\n",
    ">Ein Kalman-Filter erstellt eine Vorhersage, macht eine Messung und berechnet daraus einen Korrekturwert (auch Schätzwert genannt), der zwischen dem Vorhersagewert und dem Messwert liegt.  \n",
    "\n",
    "Der generische Kalman-Filter-Algorithmus für den mehrdimensionalen Fall lässt sich in neun Schritten beschreiben: \n",
    "  \n",
    "**Initialisierung**\n",
    "\n",
    "1. Ein physikalisches Modell erstellen und daraus eine mathematische Berechnungsformel für die Vorhersage ableiten.\n",
    "2. Einen Startwert der Zustandsgrößen (Werte, Varianzen) willkürlich festlegen.\n",
    "    \n",
    "**Vorhersage**\n",
    "\n",
    "1. Die Zustandswerte für den nächsten Zeitschritt mit Hilfe des Prozessmodells vorhersagen.\n",
    "2. Die Unsicherheit des Prozessmodells in die Varianzen der Vorhersagewerte einfließen lassen.\n",
    "    \n",
    "**Korrektur**\n",
    "\n",
    "1. Messungen jeder Zustandsgröße durchführen und wenn möglich deren Varianzen bestimmen, falls diese nicht als  konstant angenommen werden.\n",
    "2. Die Residuen jeweils von Messwert minus Vorhersagewert berechnen.\n",
    "3. Die Gewichtungsfaktoren (Kalman Gain) über den jeweiligen Vergleich der Varianzen von Messwert und Vorhersagewert berechnen.\n",
    "4. Mit den Gewichtungsfaktoren die Korrekturwerte jeweils zwischen Messwert und Vorhersagewert verorten.\n",
    "5. Die Varianzen der Korrekturwerte jeweils über die Varianzen von Mess- und Vorhersagewerte berechnen.\n",
    "\n",
    "Gegenüber dem entsprechenden Abschnitt im Jupyter-Notebook zum eindimensionalen Kalman-Filter hat sich hier bei einigen Begriffen nur der Numerus von Einzahl auf Mehrzahl geändert - der Algorithmus ist der selbe geblieben."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Im Jupyter-Notebook zum eindimensionalen Kalman-Filter wurde schon der Übergang von der allgemeinen statistischen Notation ($\\mu$, $\\sigma^2$ usw.) zur Kalman-Notation ($x$, $P$, $Q$, usw.) dargestellt. Die folgenden Tabellen bauen darauf auf und erhalten nun eine weitere Spalte für den mehrdimensionalen Kalman-Filter. Aus den Skalaren werden nun fett gedruckte Vektoren (Kleinbuchstaben) bzw. Matrizen (Großbuchstaben):\n",
    "\n",
    "<u>Vorhersage</u>\n",
    "\n",
    "$\\begin{array}{|l|l|l|}\n",
    "\\hline\n",
    "\\text{Notation} &  \\text{Notation}& \\text{Notation}& \\text{Notation mehrd.}\\\\\n",
    "\\text{eindim. Statistik} & \\text{eindim. Kalman} & \\text{mehrdim. Kalman} & \\text{Kalman mit $\\mathbf{u = 0}$}\\\\\n",
    "\\hline\n",
    "\\bar \\mu = \\mu + \\mu_{f_x} & \\bar x = \\hat x + dx & \\bar{\\mathbf x} = \\mathbf{F \\hat x} + \\mathbf{Bu} &  \\bar{\\mathbf x} = \\mathbf{F \\hat x}\\\\\n",
    "\\bar\\sigma^2 = \\sigma_x^2 + \\sigma_{f_x}^2 & \\bar P = \\hat P + Q & \\bar{\\mathbf P} = \\mathbf{F{\\hat P}F}^\\mathsf T + \\mathbf Q &  \\bar{\\mathbf P} = \\mathbf{F{\\hat P}F}^\\mathsf T + \\mathbf Q \\\\\n",
    "\\hline\n",
    "\\end{array}$\n",
    "\n",
    "Auch ohne Kenntnisse in linearer Algebra lassen sich folgende Korrespondenzen zwischen der eindimensionalen und mehrdimensionalen Kalman-Notation aus den Gleichungen oben ablesen:\n",
    "\n",
    "$\\mathbf {\\hat x}$ und $\\mathbf {\\hat P}$ sind der Erwartungswertvektor und die Kovarianzmatrix der Zustandsgröße. Sie entsprechen in der Gaußstatistik-Notation den Skalaren $\\hat x$ und $\\sigma^2$ im eindimensionalen Fall bzw. dem Vektor $\\boldsymbol \\mu$ sowie der Matrix $ \\boldsymbol \\Sigma$ im mehrdimensionalen Fall.\n",
    "\n",
    "$\\mathbf F$ ist ebenfalls eine Matrix und wird als *Übergangsmatrix* bezeichnet. In ihr steckt das physikalische Modell, ihre Multiplikation mit dem Zustandsvektor $\\mathbf{\\hat x}$ ergibt die Vorhersage $\\mathbf{\\bar x}$. \n",
    "\n",
    "$\\mathbf Q$ ist die Kovarianzmatrix des Prozessrauschens. Sie entspricht im eindimensionalen Fall $\\sigma^2_{f_x}$. In ihr stecken die Unsicherheiten des Modells sowie modellbedingte Korrelationen zwischen den einzelnen Zustandsgrößen.\n",
    "\n",
    "$\\mathbf B$ und $\\mathbf u$ sind neu. Sie beschreiben den Einfluss einer äußeren deterministischen Störung oder Steuerung des Systems. Mann nennt sie *Störmatrix* bzw. *Steuermatrix* und *Störgröße* bzw. *Steuergröße*."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<u>Korrektur</u>\n",
    "\n",
    "$\\begin{array}{|l|l|l|}\n",
    "\\hline\n",
    "\\text{Notation} &  \\text{Notation}& \\text{Notation}& \\text{Notation mehrdim.}\\\\\n",
    "\\text{eindim. Statistik} & \\text{eindim. Kalman} & \\text{mehrdim. Kalman} & \\text{Kalman mit $\\mathbf H = \\mathbf I$}\\\\\n",
    "\\hline\n",
    "& y = z - \\bar x & \\mathbf y = \\mathbf z - \\mathbf{H\\bar x} & \\mathbf y = \\mathbf z - \\mathbf{\\bar x} \\\\\n",
    "& K = \\frac{\\bar P}{\\bar P+R}&\n",
    "\\mathbf K = \\mathbf{\\bar{P}H}^\\mathsf T (\\mathbf{H\\bar{P}H}^\\mathsf T + \\mathbf R)^{-1} & \n",
    "\\mathbf K = \\mathbf{\\bar{P}} (\\mathbf{\\bar{P}} + \\mathbf R)^{-1} \\\\\n",
    "\\mu=\\frac{\\bar\\sigma^2\\, \\mu_z + \\sigma_z^2 \\, \\bar\\mu} {\\bar\\sigma^2 + \\sigma_z^2} & \\hat x = \\bar x + Ky & \\mathbf{\\hat x} = \\mathbf {\\bar x} + \\mathbf{Ky} & \\mathbf{\\hat x} = \\mathbf {\\bar x} + \\mathbf{Ky} \\\\\n",
    "\\sigma^2 = \\frac{\\sigma_1^2\\sigma_2^2}{\\sigma_1^2+\\sigma_2^2} & \\hat P = (1-K)\\bar P &\n",
    "\\mathbf {\\hat P} = (\\mathbf I - \\mathbf{KH})\\mathbf{\\bar{P}} & \n",
    "\\mathbf {\\hat P} = (\\mathbf I - \\mathbf{K})\\mathbf{\\bar{P}} \\\\\n",
    "\\hline\n",
    "\\end{array}$\n",
    "\n",
    "\n",
    "$\\mathbf H$ ist die Beobachtungsmatrix. Ihr Sinn und Zweck wird später behandelt. Sie hat etwas damit zu tun, dass eine Zustandsgröße $x_i$ oft nicht 1:1 dem entsprechenden Messwert $z_i$ entspricht. Vorerst wird aber angenommen, dass Messgröße und Zustandsgröße identisch sind, also $\\mathbf H = \\mathbf I$ gilt. Dies ist z.B. bei der Abstandmessung des Beispiel-Roboterfahrzeugs der Fall. Somit kann $\\mathbf H$ aus den Gleichungen oben entfernt werden. Dadurch wird die Ähnlichkeit zum eindimensionalen Fall leichter sichtbar, wie in der rechten Spalte der obigen Tabelle dargestellt:  \n",
    "\n",
    "$\\mathbf z$ und $\\mathbf R$ sind der Messwertvektor und dessen Kovarianzmatrix (aka \"Messunsicherheiten\"). Diese entsprechen $z$ bzw. $\\sigma_z^2$ für den eindimensionalen Fall. $\\mathbf R$ besitzt meistens nur Diagonalelemente, da die unterschiedlichen Messwerte unkorreliert streuen.\n",
    "\n",
    "$\\mathbf y$ und $\\mathbf K$ bezeichnen das Residuum und den Kalman-Gain - im mehrdimensionalen Fall in Form eines Vektors bzw. einer Matrix. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Der Übergang von Eindimensional zu Mehrdimensional spiegelt sich mathematisch im Übergang von eindimensionale auf mehrdimensionale Gaußverteilungen wieder:\n",
    "\n",
    "-  Der Schätzwert und dessen Unsicherheit eines Zustands wird durch eine (mehrdimensionale) Gaußverteilung repräsentiert.\n",
    "-  Der Messwert und dessen Unsicherheit wird durch eine (mehrdimensionale) Gaußverteilung repräsentiert.\n",
    "-  Auch das Prozessmodell wird durch eine (mehrdimensionale) Gaußverteilung repräsentiert, da es ebenfalls mit Unsicherheiten behaftet ist.\n",
    "\n",
    "Um einen Kalman-Filter auf die Beine zu stellen, muss man also die Übergangsmatrix $\\mathbf F$ deren Kovarianzmatrix $\\mathbf Q$ (Prozessrauschen), die Beobachtungsmatrix $\\mathbf H$ und ggfs. die Störmatrix $\\mathbf B$ bestimmen. Falls nicht vom Sensor geliefert benötigt man zusätzlich für den Messwert $\\mathbf z $ die Kovarianzmatrix  $\\mathbf R$ der Messunsicherheiten.   \n",
    "Dann benötigt man noch einen Startwert für den ersten Schätzwertvektor $\\hat{\\mathbf x}_0$  und dessen Kovarianzmatrix $\\mathbf {\\hat P}$. Die Eingangsgrößen des Kalmanfilters sind dann die Messwerte $\\mathbf z $ sowie ggfs. die Störgrößen $\\mathbf u$.\n",
    "\n",
    "#### Bemerkung zur Notation:\n",
    "In der Literatur wird meistens das Symbol $\\mathbf x$ nicht nur für den Zustandsvektor sondern auch für den Schätzwert, also den Erwartungswert des Zustandsvektors verwendet. In diesen Jupyter-Notebooks wird jedoch zwischen $\\mathbf x$ und $\\mathbf {\\hat x}$ unterschieden. Das Dach gibt an, dass es sich um einen Schätzwert, also dem Ergebnis des Korrekturschritts handelt. Gleiches gilt für die Kovarianzmatrix $\\mathbf P$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Konkret: Zweidimensionaler Kalman-Filter für das Roboterfahrzeug\n",
    "## Beispiel Roboterfahrzeug\n",
    "Nachfolgend soll ein zweidimensionaler Kalman-Filter entwickelt werden, der genau wie im Jupyter-Notebook des g-h-Filters bzw. des eindimensionalen Kalman-Filters ein Roboterfahrzeug tracken soll.\n",
    "<img src=\"RoboFahrzeug15Grad.png\" width=\"400\">\n",
    "Wie im schon bekannten Bild oben dargestellt, fährt das Roboterfahrzeug geradlinig schräg zu einer Wand. Gemessen wird der Abstand zur Wand.  \n",
    "Anders als beim eindimensionalen Kalman-Filter soll nun auch die Änderung des Wandabstands getrackt und ausgegeben werden. Die Relativgeschwindigkeit zur Wand ist also die zweite Zustandsgröße in dem hier zu entwickelnden *mehrdimensionalen* Kalman-Filter.  \n",
    "Anders als im eindimensionalen Fall soll nun das physikalische Modell nicht starr *nur für einen* vorgegebenen Winkel (= Relativgeschwindigkeit) zur Wand funktionieren.\n",
    "Der zweidimensionale Kalman-Filter des folgenden Abschnitts soll am Beispiel des Roboterfahrzeugs entwickelt und im folgenden Jupyter-Notebook mithilfe einer Simulation getestet werden.  \n",
    "\n",
    "Der Zustandsgrößenvektor $\\mathbf x$ besteht nun aus den beiden Komponenten *Abstand zur Wand* $x_0$ und *Relativgeschwindigkeit zur Wand* $\\frac{d}{dt} x_0 = x_1$.  \n",
    "Der zu entwickelnde Kalman-Filter soll nun beide Zustandsgrößen tracken, d.h. er soll Schätzwerte für den Abstand und die Abstandsänderung ausgeben sowie deren Varianzen berechnen. Normalerweise gibt es zu jeder Zustandsgröße einen Messwert.\n",
    ">Gibt es zu einer Zustandsgröße einen Messwert, spricht man von einer ***beobachteten* Zustandsgröße**. Im Fall der Relativgeschwindigkeit zur Wand gibt es jedoch beim Beispiel des Roboterfahrzeugs keinen Sensor. Daher handelt es sich bei dieser Zustandsgröße um eine sogenannte ***verdeckte* Zustandsgröße**.  "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Der Kalman-Filter soll nun also nicht nur den Wandabstand sondern auch die Abstandsänderung schätzen.** Das klingt jetzt nicht besonders sexy, führt aber dazu, dass anders als im eindimensionalen Fall dieser neue Kalmanfilter nicht auf einen bestimmten Winkel zur Wand voreingestellt ist. Er funktioniert wesentlich besser, falls der wahre Winkel nicht bekannt ist, und in der Folge das physikalische Modell diesen falsch annimmt. Außerdem kann nun der Schätzwert der Abstandsänderung als Reglereingangsgröße verwendet werden.     \n",
    "Diese neue Funktion des Kalman-Filters, den Wert der verdeckten Zustandsgröße auszugeben, ist in diesem speziellen Fall nicht wirklich der Rede wert. Es gibt aber andere Kalman-Filter, welche verdeckte Zustandsgrößen schätzen, die gar keiner Messung zugänglich sind. Das beste Beispiel hierfür ist das ESP eines Autos, welches die Querbewegung des Fahrzeugs als verdeckte Zustandsgröße schätzt.  \n",
    "\n",
    "Nun wird zuerst der Vorhersageschritt und anschließend der Korrekturschritt des Kalmanfilters betrachtet. Es wird jeweils der Algorithmus, die mathematische und anschließend die softwareseitige Implementierung behandelt.\n",
    "\n",
    "## Der Vorhersageschritt und das physikalische Modell (Prozessmodell) beim zweidimensionalen Kalman-Filter\n",
    "\n",
    "In den nun folgenden Abschnitten werden die einzelnen Parameter für den Vorhersageschritt entwickelt und diskutiert."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Der Zustandsvektor\n",
    "In diesem zweidimensionalen Fall besitzt der Zustandsvektor $\\mathbf x$ zwei Elemente (Abstand und Abstandsänderung). Betragen beispielsweise die Erwartungswerte des Abstands 1,0 m und der Abstandsänderung 0,45 m pro Sekunde, dann gilt für den Zustandsvektor:\n",
    "\n",
    "$$\\boldsymbol \\mu = \\mathbf x = \\begin{bmatrix}1,0\\\\0,45\\end{bmatrix}$$\n",
    "\n",
    "Von hier an wird nur noch die Kalman-Notation verwendet, der Erwartungswert des Zustandsvektors wird also mit $ \\mathbf x $ statt mit $\\boldsymbol \\mu$ bezeichnet. Seine Elemente \"$x_i$\" bezeichnen jeweils eine Zustandsgröße.  \n",
    "\"$x_i$\" muss nicht unbedingt für einen Weg oder einen Abstand sehen, wie der Buchstabe \"x\" vielleicht andeutet. Sondern  \"$x_0$\" könnte eine Batteriespannung sein und \"$x_1$\" beispielsweise ein Batterieladezustand.   \n",
    "\n",
    "Für das Roboterfahrzeug ist der Zustandsvektor $\\mathbf x$ gleich\n",
    "$$\\mathbf x =\\begin{bmatrix}x_0 \\\\ \\dot x_0\\end{bmatrix}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Die Kovarianzmatrix des Zustandsvektors\n",
    "Beim eindimensionalen Kalman-Filter war $P$ noch ein Skalar und entsprach der Varianz. Nun ist $\\mathbf P$ eine Matrix mit den Varianzen von $x_0$ und $\\dot x_0 = x_1$ als Diagonalelemente.  \n",
    "Nachfolgend ist für eine Beispielmatrix $\\mathbf P$ für die Varianz von $x_0$ der Wert 0.25  und für die Varianz von $\\dot x_0 = x_1$ der Wert 0.009 angenommen:  \n",
    "\n",
    "$$\\mathbf P = \\begin{bmatrix}0.25 & 0 \\\\ 0&0.0009\\end{bmatrix}$$\n",
    "\n",
    "Die beiden Varianzen für den Startwert von $\\mathbf P$ sind wieder mal aus der Hüfte geschossen. Sie sollten aber sinnvoll gewählt werden und in etwa dem Positions- bzw. der Geschwindigkeitsintervall entsprechen, in dem das Fahrzeug vermutet wird:  \n",
    "In diesem Beispiel würde man zum Startzeitpunkt des Filters das Roboterfahrzeug maximal $3\\sigma \\: \\hat{=} \\: 3 \\cdot \\sqrt{0,25\\:m^2} = 1,5\\:m$ weiter weg von der Wand als die angenommenen $x_0 = 1\\:m$ mit einer Relativgeschwindigkeit von maximal $3\\sigma \\: {\\hat{=}} \\: 3 \\cdot \\sqrt{0,0009\\:m^2/s^2} = 0.09\\:m/s$ schneller/langsamer als die angenommenen $x_1 = 0,45\\:m/s$ vermuten.  \n",
    "\n",
    "Welche Startwerte sollen den beiden Kovarianzen (den Elementen außerhalb der Diagonalen) in $\\mathbf {\\hat P}$ zugewiesen werden? Die Antwort darauf ist recht einfach: Die Kovarianzen, also die Matrixelemente außerhalb der Diagonalen erhalten den Startwert 0. Wie später gezeigt wird, werden diese Kovarianzen automatisch durch das physikalische Modell, also bei der Multiplikation mit der Übergangsmatrix $\\mathbf F$, mit Werten gefüllt.  \n",
    "Mit ``NumPy`` kann man recht einfach über folgenden Code eine Kovarianzmatrix $\\mathbf P$ instanziieren:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[0.25  , 0.    ],\n",
       "       [0.    , 0.0009]])"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "P = np.diag([0.25, 0.0009])\n",
    "P"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Die Übergangsmatrix F\n",
    "Das physikalische Modell steckt in einer nun zu erstellenden Übergangsmatrix $\\mathbf F$: Der aktuelle Schätzwert  $\\hat{\\mathbf x}$ multipliziert mit $\\mathbf F$ ergibt den Vorhersagewert $\\mathbf {\\bar x}$:  \n",
    "\n",
    "$$\\mathbf{\\bar x} = \\mathbf{F \\hat x}$$ \n",
    "\n",
    "Für den eindimensionalen Kalman-Filter wurde hierfür die Funktion $\\bar x = \\hat x + dx$ verwendet, was gleichbedeutend mit der Funktion $\\bar x = \\hat x + \\dot x \\cdot dt$ ist.  \n",
    "Für den nun vorliegenden zweidimensionalen Fall muss also eine Matrix $\\mathbf F$ gefunden werden, für die gilt:  \n",
    "\n",
    "$$\\begin{bmatrix} \\bar {x_0} \\\\ \\bar{\\dot {x_0}}\\end{bmatrix} = \\begin{bmatrix}? & ? \\\\? & ?\\end{bmatrix}\\begin{bmatrix}\\hat{x_0}\\\\ \\hat{\\dot{x_0}} \\end{bmatrix}$$  \n",
    "\n",
    "Zur Erinnerung in Sachen Lineare Algebra:  \n",
    "Die Multiplikation $\\mathbf{Ax}=\\mathbf b$ eines Vektors $\\mathbf x$ mit einer hier willkürlich gewählten Matrix $\\mathbf A$ wird folgendermaßen durchgeführt:  \n",
    "\n",
    "$$\\mathbf{A} = \\begin{bmatrix}2& 3 \\\\ 4&-1\\end{bmatrix},\\, \\mathbf x = \\begin{bmatrix}x_0\\\\x_1\\end{bmatrix}, \\mathbf b=\\begin{bmatrix}2x_0 + 3x_1\\\\4x_0 - 1x_1\\end{bmatrix}$$\n",
    "\n",
    "Eine geradlinige Bewegung des Roboterfahrzeug heißt nicht anderes, dass dessen Relativgeschwindigkeit $\\dot x_0$ zur Wand konstant ist. Für die beiden Komponenten des Zustandsvektors $\\mathbf x$ gilt also:\n",
    "$$\n",
    "\\begin{aligned}\n",
    "\\bar{x_0} &= \\hat{x_0} + \\hat{\\dot{x_0}} \\Delta t \\\\\n",
    "\\bar{\\dot{x_0}} &= \\hat{\\dot{x_0}}\n",
    "\\end{aligned}\n",
    "$$\n",
    "\n",
    "\n",
    "\n",
    "Oder anders geschrieben:\n",
    "$$\n",
    "\\begin{aligned}\n",
    "\\bar{x_0} &= 1\\hat{x_0} + \\Delta t \\hat{\\dot{x_0}} &\\bar{x_0}&= 1\\hat{x_0} + \\Delta t \\hat{x_1}\\\\\n",
    "\\bar{\\dot{x_0}} &= 0\\hat{x_0} + 1 \\hat {\\dot{x_0}} &\\bar{x_1}&= 0\\hat{x_0} + 1\\hat{x_1}\n",
    "\\end{aligned}\n",
    "$$\n",
    "\n",
    "Nun ist es fast offensichtlich, welche Elemente die Übergangsmatrix $\\mathbf F$ haben muss:  \n",
    "\n",
    "$$\n",
    "\\mathbf{\\bar x} = \\mathbf{F \\hat x} \\quad \\leftrightarrows \\quad \\begin{bmatrix}\\bar{x_0} \\\\ \\bar{x_1}\\end{bmatrix} = \\begin{bmatrix}1&\\Delta t  \\\\ 0&1\\end{bmatrix}  \\begin{bmatrix}\\hat{x_0} \\\\ \\hat{x_1} \\end{bmatrix}\\\\\n",
    "\\Rightarrow \\mathbf{F} = \\begin{bmatrix}1&\\Delta t  \\\\ 0&1\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "Im anschließenden Codebeispiel wird diese Matrix exemplarisch für ein Abtastintervall $ \\Delta t = 0,1 s $ instanziiert."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[1. , 0.1],\n",
       "       [0. , 1. ]])"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dt = 0.1\n",
    "F = np.array([[1, dt],[0, 1]])\n",
    "F"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Die Kovarianzmatrix des Prozessrauschens Q\n",
    "Unser physikalisches Modell des Roboterfahrzeugs (der \"Prozess\") nimmt bisher an, dass sich das Fahrzeug absolut geradlinig mit absolut konstanter Geschwindigkeit entlang dieser Linie bewegt.  \n",
    "Dies ist aber in der Realität sicherlich nicht der Fall: Es wird aufgrund  sich ändernder Bodenbeschaffenheit wohl nicht exakt geradeaus fahren. Werden dessen Räder von getrennten Elektromotoren angetrieben, deren jeweilige Drehzahlen untereinander leicht schwanken, wird das Roboterfahrzeug auch um die ideale Fahrtrichtung etwas herum \"taumeln\". Zu guter Letzt kann auch noch die Geschwindigkeit entlang der Idealline schwanken.   \n",
    "\n",
    "Die Zustandsgröße *Abstand* als auch die Zustandsgröße *Abstandsänderung* wird im allgemeinen Fall folglich schwanken und sollte also mit einer Fluktuation $\\mathbf w$ beaufschlagt werden. $\\mathbf w$ ist ein Array, dessen erstes Element die Fluktuation $w_0$ des Abstands, und dessen zweites Element die Fluktuation $w_1$ der Abstandsänderung enthält.  \n",
    "Diese Fluktuationen sind zum Glück mittelwertfrei, was die Mathematik wesentlich vereinfacht. Die mathematische Betrachtung vereinfacht sich weiter, denn es müssen primär nur die Fluktuationen der Abstandsänderung $w_1$ berücksichtigt werden. Die Fluktuationen des Abstands $w_0$ ergeben sich daraus über das zugrunde liegende physikalische Modell.  \n",
    "\n",
    "Es ist zuerst wichtig intuitiv zu verstehen, welche Bedeutung die Matrix $\\mathbf Q$ hat:  \n",
    "Der eindimensionale Kalman-Filter verwendet für die Vorhersage die Funktion $\\bar P = \\hat P + Q$. Im mehrdimensionalen Fall sieht die entsprechende Formel nicht viel anders aus: $\\bar{\\mathbf P} = \\mathbf{F{\\hat P}F}^\\mathsf T + \\mathbf Q$.  \n",
    "$\\mathbf Q$ ist also das Delta, um das sich die Kovarianzmatrix bei der Vorhersage vergrößert, analog zur (skalaren) Varianz im eindimensionalen Fall, die sich ebenfalls infolge der Addition einer Gaußverteilung vergrößerte. Die **Diagonalelemente $\\mathbf Q_{00}$ sowie $\\mathbf Q_{11}$ von $\\mathbf Q$  enthalten die Varianzen der Fluktuationen von Abstand und Abstandsänderung.** Dabei sind die Varianzen der Fluktuationen das gleiche wie die Varianzen der über das physikalische Modell vorhergesagten Zustandswerte \"Abstand\" und \"Abstandsänderung\"."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "*In dem hier betrachteten zweidimensionalen Kalman-Filter nimmt man an, die Abstandsänderung bleibt während eines Zeitschritts konstant, fluktuiert aber leicht zwischen zwei Zeitschritten. Dieses Streuen der Abstandsänderung zwischen den Zeitschritten hat eine gaußförmige Verteilung mit einer Varianz* $\\mathit{Var}(w_1) = E[{w_1}^2]$. Somit ergibt sich $\\mathbf Q_{11} = \\mathit{Var}(w_1)$.\n",
    "\n",
    "Die verbleibende Herausforderung ist es nun, bei Kenntnis von $\\mathbf Q_{11}$ für das Roboterfahrzeug die restlichen Elemente der Matrix $\\mathbf Q$ zu erzeugen. Dafür sorgen die newtonschen Bewegungsgleichungen, welche in der Übergangsmatrix $\\mathbf F$ stecken. Die mathematische Herleitung hierfür findet sich in der Jupyter-Notebookreihe von R. Labbe und werden in der Funktion ``Q_zero_mean`` unten implementiert.\n",
    "  \n",
    "In diesem Jupyter Notebook wird im folgenden Abschnitt nur angedeutet, wie aus $\\mathbf Q_{11}$ das Element $\\mathbf Q_{00}$ bestimmt wird:\n",
    ">Für das real im Projekt Automatisierungstechnik eingesetzte Roboterfahrzeug stelle man sich vor, es stehe parallel zur Wand und alle Radmotoren würden mit der gleichen Solldrehzahl gesteuert: Dann würde das Fahrzeug (plausible Annahme aus der Hüfte geschossen) etwa um 0,003 m von einer Ideallinie parallel zur Wand abweichen ($3\\sigma$).  \n",
    "*Eine vernünftige Annahme für die Standardabweichung des Abstands von $w_0$ ist somit $\\sigma(w_0) = 0,001\\:m$.*  Dies entspricht für $dt = 1\\:s$ einer Standardabweichung der Abstandsänderung von $\\sigma(w_1) = 2 /dt \\cdot 10 ^{-3} \\:(m/s)$. \n",
    "Die Standardabweichung der Abstandsänderng ist doppelt so groß wie die Standardabweichung des Abstands. Denn eine Fluktuation des Abstands von zuerst $-0,001\\:m$ und im nächsten Zeitschritt auf dann $+0,001\\:m$ benötigt eine Fluktuation der Abstandsänderung um $+0,002\\:m/s$. Dadurch entsteht ein Faktor 2, welcher beim Übergang von der Standardabweichung auf die Varianz durch das Quadrieren zum Faktor 4 wird. Somit gilt $\\mathit{Var}(w_1) = 4 \\cdot \\mathit{Var}(w_0)$ also $\\mathbf Q_{00} = 1/4 \\cdot \\mathbf Q_{11}$.  \n",
    "\n",
    "Die folgende Funktion ``Q_zero_mean`` bewerkstelligt die Berechnung der fehlenden Elemente von $\\mathbf Q$, insbesondere der Kovarianzen außerhalb der Diagonalen. Sie benötigt als Eingangswerte die Dimension von $\\mathbf Q$, die Größe des Zeitschritts sowie die Varianz der Abstandsänderung $\\mathit{Var}(w_1)$.  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[1.e-06, 2.e-06],\n",
       "       [2.e-06, 4.e-06]])"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from scipy.linalg import block_diag\n",
    "\n",
    "def Q_zero_mean(dim, dt=1., var=1):\n",
    "    '''generates start value of process covariance matrix based on given estimate\n",
    "    of variance for the highest order state variable (e.g. velocity for two dimensions)'''\n",
    "    if not (dim == 2 or dim == 3 or dim == 4):\n",
    "        raise ValueError(\"dim must be between 2 and 4\")\n",
    "\n",
    "    if dim == 2:\n",
    "        Q = np.array([[.25*dt**4, .5*dt**3],\n",
    "             [ .5*dt**3,    dt**2]])\n",
    "    elif dim == 3:\n",
    "        Q = np.array([[.25*dt**4, .5*dt**3, .5*dt**2],\n",
    "             [ .5*dt**3,    dt**2,       dt],\n",
    "             [ .5*dt**2,       dt,        1]])\n",
    "    else:\n",
    "        Q = np.array([[(dt**6)/36, (dt**5)/12, (dt**4)/6, (dt**3)/6],\n",
    "             [(dt**5)/12, (dt**4)/4,  (dt**3)/2, (dt**2)/2],\n",
    "             [(dt**4)/6,  (dt**3)/2,   dt**2,     dt],\n",
    "             [(dt**3)/6,  (dt**2)/2 ,  dt,        1.]])\n",
    "\n",
    "    #return block_diag(*[Q]) * var \n",
    "    return Q*var\n",
    "\n",
    "Q_zero_mean(dim=2, dt=1., var=4e-6)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Die Steuermatrix und die Steuergröße\n",
    "Der Kalman-Filter wurde bekanntlich im Apolloprogramm der NASA zum ersten Mal eingesetzt. Dort ging es u.a. darum, bei der Mondlandung den Abstand zur Oberfläche zu tracken. Hierbei mussten natürlich auch die Steuergrößen also die Bremsraketen im Filter berücksichtigt werden. Diese Information wollte man für den Filter natürlich nicht einfach ignorieren.  \n",
    "Nicht zuletzt aus diesem historischen Anlass ist bei einem Kalman-Filter sozusagen \"von Werk aus\" schon die Einbeziehung *externer Steuer- oder anders ausgedrückt Störgrößen* vorhanden.  \n",
    "\n",
    "Für ein lineares System kann der Einfluss einer Steuergröße als lineares Gleichungssystem mathematisch modelliert werden. Ein lineares Gleichungssystem ist jedoch nichts Anderes als eine Vektor-Matrix-Multiplikation aus der linearen Algebra:  \n",
    "\n",
    "$$\\Delta\\bar{\\mathbf x} = \\mathbf{Bu}$$\n",
    "\n",
    "Hier steht $\\mathbf u$ für die Steuergröße und $\\mathbf B$ für die Steuermatrix.  \n",
    "Beispielsweise könnte $\\mathbf u$ das Puls-Pausen-Verhältnis des PWM-Signals sein, welches die Motoren des Roboterfahrzeugs steuert. $\\mathbf u$ multipliziert mit $\\mathbf{B}$ ergibt \n",
    "$\\Delta[\\begin{smallmatrix}{\\bar x} \\\\ \\bar {\\dot x}\\end{smallmatrix}]$.  \n",
    "D.h. die Multiplikation der Steuergröße mit der Steuermatrix ergibt die Änderung der vorhergesagten Zustandsgrößen.  \n",
    "\n",
    "Somit ergibt sich für den mehrdimensionalen Vorhersageschritt die folgende Formel:  \n",
    "\n",
    "$$\\mathbf{\\bar x} = \\mathbf{Fx} + \\mathbf{Bu}$$  \n",
    "\n",
    "Die Übergangsmatrix $\\mathbf{F}$ berechnet die Vorhersage für den neuen Zustandsvektor $\\mathbf {\\bar x}$. Zusätzlich darauf wird noch dessen Änderung durch die externe Steuerung aufaddiert.  \n",
    "\n",
    ">Im Beispiel des hier betrachteten Roboterfahrzeugs gibt es keine (Fern-)Steuerung: Daher wird $\\mathbf B$ als Null angenommen.\n",
    "\n",
    "### Zusammenfassung Initialisierung Vorhersage\n",
    "Für den Kalman-Filter müssen folgende Arrays (Vektoren bzw. Matrizen) initialisiert werden:\n",
    "* $\\mathbf{\\hat x}$, $\\mathbf {\\hat P}$: Der Startwert und die Kovarianzmatrix des Zustandsvektors (\"nullter\" Schätzwert)\n",
    "* $\\mathbf F$,  $\\mathbf Q$: Das Prozessmodell in Form der Übergangsmatrix und das zugehörige Prozessrauschen\n",
    "* $\\mathbf B$, $\\mathbf u$: Optional die Steuermatrix und die von extern wirkende Steuergröße"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Die Vorhersagefunktion\n",
    "Zu guter Letzt muss nun nur noch eine Vorhersagefunktion ``predict(x, P, F, Q, B, u)`` in Python implementiert werden. Diese erhält die o.g. Größen übergeben und gibt den vorhergesagten Zustandsvektor $\\mathbf{\\bar x}$ sowie dessen Kovarianzmatrix $\\bar{\\mathbf P}$ zurück."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def predict(x, P, F, Q, B, u):\n",
    "    x_prior = np.dot(F, x)+ np.dot(B, u)\n",
    "    P_prior = np.dot(F, P).dot(F.T) + Q #A.dot(B) is the same as np.dot(A,B)\n",
    "    return x_prior,P_prior   "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Mit den im vorangegangenen Text erarbeiteten Initialisierungen für das Roboterfahrzeug wird nun die obige Vorhersagefunktion exemplarisch vier mal angewendet. Als Zeitschritt wurde hier 2 Sekunden gewählt, damit die Werte des Zustandsvektors und dessen Kovarianzmatrix nicht zu klein sind.  \n",
    "Normalerweise würde ja nach jedem Vorhersageschritt noch ein Korrekturschritt folgen. Die vier Zeitschritte hier sollen nur den Einfluss der Vorhersage auf den Zustandsvektor $\\mathbf{\\bar x}$ sowie dessen Kovarianzmatrix $\\bar{\\mathbf P}$ verdeutlichen."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.   0.05] x start\n",
      "[[0.25  0.   ]\n",
      " [0.    0.001]] P start\n",
      "\n",
      "[0.1  0.05] x\n",
      "[[0.254 0.002]\n",
      " [0.002 0.001]] P\n",
      "\n",
      "[0.2  0.05] x\n",
      "[[0.264 0.004]\n",
      " [0.004 0.001]] P\n",
      "\n",
      "[0.3  0.05] x\n",
      "[[0.283 0.005]\n",
      " [0.005 0.001]] P\n",
      "\n",
      "[0.4  0.05] x\n",
      "[[0.308 0.007]\n",
      " [0.007 0.001]] P\n"
     ]
    }
   ],
   "source": [
    "x = np.array([0., 0.05])\n",
    "P = np.diag([0.25, 0.0009])\n",
    "dt = 2\n",
    "F = np.array([[1, dt],[0, 1]])\n",
    "Q = Q_zero_mean(dim=2, dt=1., var=4e-6)\n",
    "B = 0\n",
    "\n",
    "print(np.around(x,decimals=3), 'x start')\n",
    "print(np.around(P,decimals=3), 'P start')\n",
    "for _ in range(4):\n",
    "    print()\n",
    "    x,P = predict(x, P, F, Q, B, u=0)\n",
    "    print(np.around(x,decimals=3), 'x')\n",
    "    print(np.around(P,decimals=3), 'P')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Am Werteverlauf des Zustandsvektors $\\mathbf{\\bar x}$ ist gut zu erkennen, dass das Roboterfahrzeug sich pro 2 Sekunden Zeitschritt jeweils um 0,1 m von der Wand weg bewegt. Seine Geschwindigkeit bleibt wie erwartet (bzw. im physikalischen Modell festgelegt) konstant bei 0,05 m/s.  \n",
    "\n",
    "Interessanter ist, wie sich die Kovarianzmatrix $\\bar{\\mathbf P}$ entwickelt: Die Varianz des Abstands steigt bei jeder Vorhersage aufgrund des Prozessrauschens an. Die Varianz der Abstandsänderung bleibt konstant, denn dies ist ja eine (konstante) Eigenschaft des physikalischen Modells. Beides jedoch kein Wunder.  \n",
    "\n",
    ">Was schon eher in die Kategorie \"Wunder\" fällt, sind die Außerdiagonalelemente von $\\bar{\\mathbf P}$:  \n",
    "Durch das physikalische Modell in der Matrix $\\mathbf F$ entwickelt sich automatisch in der Matrix $\\bar{\\mathbf P}$ eine Korrelation zwischen Abstand und Abstandsänderung. \n",
    "\n",
    "Im nachfolgenden Plot sind die Konfidenzellipsen der Start-Matrix $\\mathbf P$ und der vier Vorhersage-Matrizen $\\bar{\\mathbf P}$ als Konfidenzellipsen dargestellt. Die Zunahme der Korrelation deutlich durch ein Kippen der Ellipse sichtbar. Die Zentren der Ellipsen (schwarze Kreuze) symbolisieren die Erwartungswerte. Deren x-Komponente zeigt das gleichförmige Ansteigen des Abstands zur Wand. Die y-Komponente zeigt die konstante Abstandsänderung. Die Breite der Ellipsen in x-Richtung wird bei jeder Vorhersage etwas größer (zunehmend schlechtere Lokalisierung durch Prozessrauschen), die Höhe in y-Richtung bleibt jedoch konstant."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import scipy.linalg as linalg\n",
    "from matplotlib.patches import Ellipse\n",
    "\n",
    "# Computes orientation, width and height of the ellipse of covariance matrix\n",
    "def covariance_ellipse(sigma):\n",
    "    U, s, _ = linalg.svd(sigma)\n",
    "    orientation = math.atan2(U[1, 0], U[0, 0])\n",
    "    width = math.sqrt(s[0])\n",
    "    height = math.sqrt(s[1])\n",
    "    if height > width: raise ValueError('width must be greater than height')       \n",
    "    return (orientation, width, height)\n",
    "\n",
    "#plots the ellipse of covariance matrix\n",
    "def plot_covariance(mu, sigma=None, std=None, axis_equal=True, show_center=True,\n",
    "                    facecolor='none', edgecolor='k',alpha=1.0, x_lim=None, y_lim=None, ls='solid'):\n",
    "    ellipse = covariance_ellipse(sigma)\n",
    "    angle = np.degrees(ellipse[0])\n",
    "    width = ellipse[1] * 2.\n",
    "    height = ellipse[2] * 2.\n",
    "\n",
    "    #fig, ax = plt.subplots()\n",
    "    ax = plt.gca() # get handle of current plot\n",
    "    ax.grid()\n",
    "    for sd in std:\n",
    "        e = Ellipse(xy=mu, width=sd*width, height=sd*height, angle=angle,\n",
    "                    facecolor=facecolor, edgecolor=edgecolor, alpha=alpha, lw=2, ls=ls)\n",
    "        ax.add_patch(e)\n",
    "    x, y = mu\n",
    "    \n",
    "    if axis_equal: plt.axis('equal')\n",
    "    if show_center: plt.scatter(x, y, marker='+',alpha=alpha,color=edgecolor)\n",
    "    if x_lim is not None: ax.set_xlim(x-x_lim,x+x_lim)\n",
    "    if y_lim is not None: ax.set_ylim(y-y_lim,y+y_lim)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Text(0,0.5,'Abstandsänderung (m/s)')"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "x = np.array([0., 0.05])\n",
    "P = np.diag([0.25, 0.0009])\n",
    "dt = 2\n",
    "F = np.array([[1, dt],[0, 1]])\n",
    "Q = Q_zero_mean(dim=2, dt=1., var=4e-6)\n",
    "B = 0\n",
    "u = 0\n",
    "fig, ax = plt.subplots()\n",
    "plot_covariance(x, P, alpha=1, std=[1], axis_equal=False, x_lim=1, y_lim=0.1)\n",
    "for i in range(4):\n",
    "    x,P = predict(x, P, F, Q, B, u)\n",
    "    plot_covariance(x, P, alpha=1/(i+2), std=[1], axis_equal=False, x_lim=1, y_lim=0.1)\n",
    "    \n",
    "ax.set_xlabel('Abstand (m)')\n",
    "ax.set_ylabel('Abstandsänderung (m/s)')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    ">**Letztlich ist dieses automatische Erscheinen der Korrelation die zentrale Eigenschaft, die diesen zweidimensionalen Kalman-Filter im Vergleich zum eindimensionalen auszeichnet.**  \n",
    "Wie im Jupyter-Notebook zu den mehrdimensionalen Gaußverteilungen gezeigt, bewirkt diese Korrelation anschließend im Korrekturschritt die wesentlich bessere Schätzung der Zustandsgröße \"Abstand\" sowie überhaupt eine Schätzung der verdeckten Zustandsgröße \"Abstandsänderung\"."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Der Korrekturschritt beim zweidimensionalen Kalman-Filter\n",
    "Der Korrekturschritt ist beim Übergang zum zweidimensionalen Kalman-Filter auch nicht komplizierter als der Vorhersageschritt. Ähnlich wie die Steuermatrix $\\mathbf B$ bei der Vorhersage gibt es hier auch einen neuen Parameter, die Beobachtungsmatrix $\\mathbf H$.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Die Beobachtungsmatrix\n",
    "Bisher gab es zu jedem Zustandswert immer den entsprechenden Messwert. Daher konnte das Residuum $y$ einfach aus der Differenz zwischen dem Vorhersagewert und dem Messwert berechnet werden: $y = z - \\bar x$.  \n",
    "Aber schon beim Beispiel Roboterfahrzeug gibt es für die verdeckte Zustandsgröße Abstandsänderung keinen Messwert.\n",
    "Außerdem gibt es oft Situationen, bei denen ein Messwert mehrdeutig ist - wie z.B. beim Phasenverfahren des Lidar-Sensors. In diesem Fall kann zwar über den Zustandswert eindeutig auf einen Messwert geschlossen werden. Umgekehrt ist das jedoch nicht möglich.  \n",
    "Aus diesen Gründen ist es praktikabler, das Residuum nicht im \"Zustandsraum\" sondern im \"Messwertraum\" zu berechnen. Sprich man wandelt den Zustandsvektor der Vorhersage $\\mathbf x$ zuerst in einen Messwertvektor um. *Im Fall einer verdeckten Zustandsgröße fehlt dann im Messwertvektor die entsprechende Komponente, wie gleich gezeigt wird.*\n",
    "**Die Umwandlung vom Zustandsraum in den Messwertraum übernimmt die Beobachtungsmatrix $\\mathbf H$.**\n",
    "\n",
    "$$\\mathbf y = \\mathbf z - \\mathbf{H \\bar x}$$  \n",
    "\n",
    "Entsprechend ändert sich die grafische Darstellung des eindimensionalen Kalmanfilters wie folgt:\n",
    "\n",
    "<img src=\"grafikKalman_VorhersKorr_mehrDim.png\" width=\"800\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In dem hier betrachteten Fall ist der Zustandswert \"Abstand\" das selbe wie der Messwert \"Abstand\". Jedoch ist der Zustandsraum zweidimensional (Abstand und Abstandsänderung), der Messwertraum jedoch nur eine eindimensionale Teilmenge (Abstand) davon.  \n",
    "Welche Elemente muss die Beobachtungsmatrix $\\mathbf H$ für das Roboterfahrzeug besitzen?  \n",
    "Bei der Abstandsänderung handelt es ich um eine verdeckte Zustandsgröße. Daher muss $\\mathbf H$ eine solche Gestalt haben, dass  $\\mathbf{H\\bar x}$ einen eindimensionalen Messwertvektor $\\mathbf z$ ergibt.  \n",
    "\n",
    "$$\\mathbf z = \\begin{bmatrix}z_0\\end{bmatrix}$$\n",
    "\n",
    "Die Berechnungsformel für das Residuum, welches auch ein eindimensionaler Vektor ist, muss dann folgende Form annehmen:  \n",
    "\n",
    "$$\n",
    "\\begin{aligned}\n",
    "\\textbf{y} &= \\mathbf z - \\mathbf{H\\bar x}  \\\\\n",
    "\\begin{bmatrix}y \\end{bmatrix} &= \\begin{bmatrix}z_0\\end{bmatrix} - \\begin{bmatrix}?&?\\end{bmatrix} \\begin{bmatrix}x_0 \\\\  x_1\\end{bmatrix}\n",
    "\\end{aligned}\n",
    "$$\n",
    "\n",
    "$\\mathbf H$ muss also eine 1x2-Matrix sein. Und für das Residuum $\\mathbf y$ muss gelten $\\mathbf{y} = [z_0 - \\bar x_0]$, denn es hängt nur vom Abstand $x_0$ ab.  \n",
    "\n",
    "Folglich berechnet sich die Beobachtungsmatrix für das Roboterfahrzeug zu $\\mathbf H=\\begin{bmatrix}1&0\\end{bmatrix}$.  \n",
    "Nachfolgend wird diese Matrix im Code implementiert."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "H = np.array([[1., 0.]])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Der Messwertvektor und dessen Kovarianzmatrix\n",
    "Der Messwertvektor beinhaltet die Sensormesswerte des Prozesses, d.h. hier die Messwerte des Abstandssensors des Roboterfahrzeugs. Da wegen der verdeckten Zustandsgröße \"Abstandsänderung\" nur *ein einzelner* Messwert im Vektor $\\mathbf z$ vorliegt, gilt:\n",
    "\n",
    "$$\\mathbf z = \\begin{bmatrix}z_0\\end{bmatrix}$$\n",
    "\n",
    "Bei zwei Messwerten würde beispielsweise gelten: \n",
    "\n",
    "$$\\mathbf z = \\begin{bmatrix}z_0 \\\\ z_1\\end{bmatrix}$$\n",
    "\n",
    "In der Kovarianzmatrix $\\mathbf R$ von $\\mathbf z$ sind die Messunsicherheiten enthalten. Sie ist eine quadratische Matrix der Form $n \\times n$, wobei n die Anzahl der Sensoren bzw. unterschiedlichen Messgrößen ist. Deren Diagonalelemente sind meist bekannt, da es sich um die Varianzen = Messunsicherheiten der einzelnen Sensoren handelt.   \n",
    "\n",
    "Im hier betrachteten Beispiel des Roboterfahrzeugs lässt sich die Kovarianzmatrix $\\mathbf R$ ganz einfach zu\n",
    "\n",
    "$$R = \\begin{bmatrix}\\sigma^2_z\\end{bmatrix}$$ \n",
    "\n",
    "bestimmen.\n",
    "\n",
    "Bei zwei unterschiedlichen Messwerten mit einer angenommenen Standardabweichung $\\sigma_{z_0} = 0,1 m$ und $\\sigma_{z_1} = 0,4 m$ würde sich folgende Matrix ergeben: \n",
    "$$R = \\begin{bmatrix}0,01&0\\\\0&0,16\\end{bmatrix}$$ \n",
    "\n",
    "Im Fall des Roboterfahrzeug mit nur einem Sensor mit $\\sigma_{z_0} = 0,1 m$ berechnet sich die Kovarianzmatrix des Messwerts zu $R = \\begin{bmatrix}0,01\\end{bmatrix}$. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "sigma_z = 0.1\n",
    "R = np.array([[sigma_z**2]])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Für das Roboterfahrzeug ist der zweidimensionale Kalman-Filter nun komplett initialisiert und alle Parameter sind berechnet. Der Vorhersage-Algorithmus wurde oben schon in Python-Code implementiert. Gleiches muss nur noch mit dem Korrektur-Algorithmus geschehen.\n",
    "Dies, sowie die Simulation des Roboterfahrzeugs sind die Inhalte des nächsten Jupyter-Notebooks. Darin wird dann auch der zweidimensionale Kalman-Filter an verschiedenen Simulationsdaten für das Roboterfahrzeug getestet."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Bemerkung zu den physikalischen Einheiten\n",
    "Um die Darstellung der Gleichungen in diesem Jupyter-Notebook möglichst übersichtlich zu halten, wurde in den meisten Fällen auf Einheiten verzichtet. Im Fall des Roboterfahrzeugs müssten korrekter Weise bei den Abständen immer die Einheit \"m\", bei den Abstandsänderungen \"m/s\" und bei der Zeit die Einheit \"s\" aufgeführt werden.  \n",
    "Grundsätzlich macht es Sinn, die Einheiten weg zu lassen. Im Gegenzug ist jedoch für jede Größe peinlich darauf zu achten, dass immer nur diese eine festgelegte Einheit verwendet wird. **Dies wird beispielsweise wichtig, wenn ein Kalmanfilter in einem Simulinkmodell implementiert wird, da dort die \"Sampling Time\" sicher nicht gleich 1 Sekunde betragen wird.**"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.11.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}