Add information and change things in V10_kalmanfilter_demonstration.ipynb

09cc1c19 · JupyterHub User · a0d8222d · 09cc1c19
Commit 09cc1c19 authored 3 years ago by JupyterHub User
--- a/notebooks/V10_kalmanfilter_demonstration.ipynb
+++ b/notebooks/V10_kalmanfilter_demonstration.ipynb
@@ -177,13 +177,17 @@
    "om = Ts\n",
    "F = np.array([[cos(om),sin(om)],\n",
    "              [-sin(om),cos(om)]])\n",
-    "G = np.array([[0,0],[0,0]])\n",
+    "G = np.array([[0,0],\n",
-    "C = np.array([[1,0],[0,0]]) # hier 4-Dimensional, aber erfüllt die gleiche Aufgabe wie C im oberen Code-Block\n",
+    "              [0,0]])\n",
-    "Q = np.array([[3.25e-06, 6.50e-05], # höhere Werte => geringere Berücksichtigung der Messung\n",
+    "C = np.array([[1,0],\n",
-    "            [6.50e-05, 1.30e-03]])\n",
+    "              [0,0]]) # hier 2x2, aber erfüllt die gleiche Aufgabe wie C im oberen Code-Block\n",
-    "R = np.array([[0.01,0],[0,0.01]]) # höhere Werte => geringere Berücksichtigung der Messung\n",
+    "Q = np.array([[3.25e-06, 6.50e-05],\n",
+    "            [6.50e-05, 1.30e-03]]) # höhere Werte => geringere Berücksichtigung der Messung\n",
+    "R = np.array([[0.01,0],\n",
+    "              [0,0.01]]) # höhere Werte => geringere Berücksichtigung der Messung\n",
    "xo = np.array([0,0.5])\n",
-    "Pk = np.array([[100,0],[0,100]]) # höhere Werte => geringere Berücksichtigung des Ausgangszustandes xo\n",
+    "Pk = np.array([[100,0],\n",
+    "               [0,100]]) # höhere Werte => geringere Berücksichtigung des Ausgangszustandes xo\n",
    "\n",
    "c1 = UnknownWaveSource(1,0,0.5,1,0) # Wellen-Funktion\n",
    "c2 = Noise(1,2,0.4) # Ausgabe Sensor\n",

 %% Cell type:markdown id:fifteen-reward tags:
 # <span style='color:OrangeRed'>V10 - Kalman-Filter Demonstration</span>
 %% Cell type:code id:tough-category tags:
 ``` python
 from systheo2functions import *
 %matplotlib inline
 ```
 %% Cell type:markdown id:alpha-kitchen tags:
 <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
 Ein Roboter balanciert auf einem Surfbrett und die Wellen unter dem Brett bewegen sich mit einer uns unbekannten Funktion:
 %% Cell type:code id:respective-writer tags:
 ``` python
 scw = Schema(0,12,0.1,2) # Instance of the simulation schematic
 w1 = UnknownWaveSource(1,0,0.5,1,0) # Wellen-Funktion
 scw.AddListComponents(np.array([w1]));
 outw = scw.Run(np.array([1]))
 fig = plt.figure()
 ax = fig.add_subplot(2, 1, 1)
 fig.set_dpi(120)
 ax.plot(outw[0,:],outw[1,:],'blue')
 plt.title('unbekannte Wellen-Funktion')
 plt.show()
 ```
 %% Output
 %% Cell type:markdown id:trained-bumper tags:
 <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
 <img src="bilder/v10_roboter.png"/><br>
 Die Sensoren des Roboters erfassen den Verlauf der Wellen (b.z.w. die Höhe der Wellen), aber leider ist deren Ausgabe ungenau. <br><br>
 Zusätzlich lässt sich der Verlauf der Wellen mathematisch durch eine Sinus-Funktion vorhersagen b.z.w. nähern. Eine <b> mathematische Näherung entspricht nicht immer ganz der Wirklichkeit </b> und so ist die uns unbekannte Wellenfunktion keine Sinus-Funktion, aber wir hoffen, dass unsere Näherung später gut genug ist, damit sie (zusammen mit der Ausgabe der Sensoren und dem Kalman-Filter) zufriedenstellende Ergebnisse liefert.
 %% Cell type:markdown id:numerical-europe tags:
 <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
 Die mathematische Vorhersage/Näherung wird durch Matrizen und Vektoren ausgedrückt (in diesem Fall mit einer Drehmatrix).<br><br>
 Zustandsvorhersage: <code>x = F × x + G × u</code> mit <code>F</code> als Drehmatrix und <code>x</code> als Zustandsvektor.
 <br><br>Dabei interessiert uns hier nur der erste Eintrag im x-Vektor <code>x[0]</code>, welcher als Messvorhersage dient und ungefähr die momentane Höhe der Welle angeben soll, also das was wir auch von den Sensoren als Messung erwarten.
 %% Cell type:code id:radio-swing tags:
 ``` python
 om = 0.1
 F = np.array([[cos(om),sin(om)],
              [-sin(om),cos(om)]])
 x = np.array([0,0.5])
 C=np.array([1,0])
 y = [dot(C,x)]
 # Zustandsvorhersage:
 # x^ = F*x + G*u
 # Hier mit G*u=0
 # Messvorhersage:
 # z = C*x^
 for i in range(0,119):
    x = (dot(F,x)) # Zustandsvorhersage
    z = dot(C,x) # Messvorhersage (hier z = dot(C,x) = x[0])
    y.append(z)
 fig = plt.figure()
 ax = fig.add_subplot(1, 1, 1)
 fig.set_dpi(120)
 ax.plot(np.arange(0.0, 120*0.1, 0.1),y,'darkblue',outw[0,:],outw[1,:],'blue')
 ax.grid()
 ax.legend(['Sinus-Funktion (Messvorhersage)','Wellen-Funktion'])
 plt.show()
 ```
 %% Output
 %% Cell type:markdown id:administrative-communist tags:
 <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
 Der Kalman-Filter hilft uns nun unsere mathematische Annäherung und die Ausgabe der Sensoren so zu kombinieren, dass der Verlauf der Wellen möglichst gut beschrieben werden kann, obwohl unser mathematisches Modell und die Sensoren des Roboters die Wirklichkeit nicht perfekt beschreiben. Wir übergeben unsere mathematische Vorhersage (die Matrix <code>F</code> und der Vektor <code>xo</code>) an den Kalman-Filter, welcher zusätzlich mit den Daten der Sensoren gefüttert wird.<br><br>
 Andere Matrizen, die an den Kalman-Filter übergeben werden (<code>Q</code>,<code>R</code>und<code>Pk</code>), beeinflussen wie stark sich die mathematische Vorhersage und die Messung auf die Ausgabe des Filters auswirken.
 %% Cell type:code id:chief-cemetery tags:
 ``` python
 tini = 0 # Start time
 tfinal = 25 # End time
 dt = 0.01 # Time Step
 nflows = 7 # Number of data flows in the schematic
 Ts = 0.1 # Sampling time for discrete time
 sc = Schema(tini,tfinal,dt,nflows) # Instance of the simulation schematic
 om = Ts
 F = np.array([[cos(om),sin(om)],
              [-sin(om),cos(om)]])
-G = np.array([[0,0],[0,0]])
+G = np.array([[0,0],
-C = np.array([[1,0],[0,0]]) # hier 4-Dimensional, aber erfüllt die gleiche Aufgabe wie C im oberen Code-Block
+              [0,0]])
-Q = np.array([[3.25e-06, 6.50e-05], # höhere Werte => geringere Berücksichtigung der Messung
+C = np.array([[1,0],
-            [6.50e-05, 1.30e-03]])
+              [0,0]]) # hier 2x2, aber erfüllt die gleiche Aufgabe wie C im oberen Code-Block
-R = np.array([[0.01,0],[0,0.01]]) # höhere Werte => geringere Berücksichtigung der Messung
+Q = np.array([[3.25e-06, 6.50e-05],
+            [6.50e-05, 1.30e-03]]) # höhere Werte => geringere Berücksichtigung der Messung
+R = np.array([[0.01,0],
+              [0,0.01]]) # höhere Werte => geringere Berücksichtigung der Messung
 xo = np.array([0,0.5])
-Pk = np.array([[100,0],[0,100]]) # höhere Werte => geringere Berücksichtigung des Ausgangszustandes xo
+Pk = np.array([[100,0],
+               [0,100]]) # höhere Werte => geringere Berücksichtigung des Ausgangszustandes xo
 c1 = UnknownWaveSource(1,0,0.5,1,0) # Wellen-Funktion
 c2 = Noise(1,2,0.4) # Ausgabe Sensor
 c3 = KalmanFilter([2],[3],0,1,F,G,C,Q,R,xo,Pk,Ts) # Kalman-Filter
 sc.AddListComponents(np.array([c1,c2,c3]));
 #Run the schematic and plot:
 out = sc.Run(np.array([1, 2, 3]))
 fig = plt.figure()
 ax = fig.add_subplot(1, 1, 1)
 fig.set_dpi(120)
 ax.plot(out[0,:],out[2,:],'lightblue',out[0,:],out[1,:],'blue',out[0,:],out[3,:],'green')
 ax.grid()
 ax.legend(['Sensor','Original','Ausgabe Kalman-Filter'])
 plt.show()
 ```
 %% Output
 %% Cell type:markdown id:brief-egypt tags:
 <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
 Es ist zu sehen, dass der Kalman-Filter es schafft, aus unserer mathematischen Vorhersage und den Daten der Sensoren, eine Ausgabe zu erzeugen, welche den Verlauf der Wellen nahekommt. Dies kann nun genutzt werden, um den Roboter möglichst gut zu balancieren.
 %% Cell type:markdown id:anonymous-franklin tags:
 ## Nähere Erklärung zum verwendeten Kalman-Filter:
 %% Cell type:markdown id:posted-visitor tags:
 <img src="bilder/v10_kalmanfilter.png"/>
 %% Cell type:markdown id:colored-desert tags:
 ### Ablauf in KalmanFilter([...],[...],N,M,F,G,C,Q,R,xo,Pk,Ts):
 %% Cell type:markdown id:blocked-spanish tags:
 <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
 Zustandsvorhersage:<br>
 x = F × x + G × u
 <br><br>
 Messvorhersage:<br>
 z = C × x
 <br><br>
 Innovation:<br>
 v = m - z
 <br><br>
 Kovarianz der Zustandsvorhersage:<br>
 Pk = F × P × F.T + Q
 <br><br>
 Kovarianz der Innovation:<br>
 S = C × Pk × C.T + R
 <br><br>
 Kalman Gain:<br>
 Kk = Pk × C.T × inv(S)
 <br><br>
 Kovarianzkorrektur:<br>
 Pk = (I - (Kk × C)) × Pk
 <br><br>
 Zustandskorrektur:<br>
 x = x + (Kk × v)
 <br><br>
 Ausgabe des Zustandsvektors:<br>
 Ausgabe = x