Correct misspelling in V10_kalmanfilter_demonstration.ipynb

59da8b93 · Adam Friedrich Schrey · 8523adaf · 59da8b93
Commit 59da8b93 authored 3 years ago by Adam Friedrich Schrey
--- a/notebooks/V10_kalmanfilter_demonstration.ipynb
+++ b/notebooks/V10_kalmanfilter_demonstration.ipynb
@@ -25,7 +25,7 @@
   "metadata": {},
   "source": [
    "<div style=\"font-family: 'times'; font-size: 13pt; text-align: justify\">\n",
-    "Ein Roboter balanciert auf einem Surfbrett und die Wellen unter dem Brett bewegen sich mit einer uns unbekannten Funtion:"
+    "Ein Roboter balanciert auf einem Surfbrett und die Wellen unter dem Brett bewegen sich mit einer uns unbekannten Funktion:"
   ]
  },
  {

 %% Cell type:markdown id:fifteen-reward tags:

 # <span style='color:OrangeRed'>V10 - Kalmanfilter 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 Funtion:
+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,2,0.01,2) # Instance of the simulation schematic
 w1 = UnknownWaveSource(1,0,0.5,2*pi,0);#TransferFunction(inp,out,num,den)
 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, aber leider ist deren Ausgabe ungenau.
 Zusätzlich lässt sich der Verlauf der Wellen mathematisch durch eine Sinus-Funktion annähern. Wir versuchen den Verlauf der Wellen-Funktion durch eine Drehmatrix vorherzusagen:

 %% Cell type:markdown id:numerical-europe tags:

 <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
 Zustandsvorhersage: <code>x = F × x + G × u</code> mit <code>F</code> als Drehmatrix und <code>x[0]</code> als Höhe der Welle

 %% Cell type:code id:radio-swing tags:

 ``` python
 om = 0.01*2*pi

 F = np.array([[cos(om),sin(om)],
              [-sin(om),cos(om)]])
 x = np.array([0,0.5])

 y1 = [x[0]]
 y2 = [x[1]]

 # Zustandsvorhersage:
 # x^ = F*x + G*u
 # Hier mit G=0 und u=0
 for i in range(0,199):
    x = (dot(F,x))
    y1.append(x[0])
    y2.append(x[1])

 fig = plt.figure()
 ax = fig.add_subplot(1, 1, 1)
 fig.set_dpi(120)
 ax.plot(np.arange(0.0, 200*0.01, 0.01),y2,'lightgray',np.arange(0.0, 200*0.01, 0.01),y1,'darkblue',
        outw[0,:],outw[1,:],'blue')
 ax.grid()
 ax.legend(['Cosinusfunktion','Sinusfunktion','Wellen-Funktion'])
 plt.show()
 ```

 %% Output



 %% Cell type:markdown id:administrative-communist tags:

 <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
 Der Kalmanfilter 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 Kalmanfilter, welcher zusätzlich mit den Daten der Sensoren gefüttert wird.

 %% Cell type:code id:chief-cemetery tags:

 ``` python
 tini = 0 # Start time
 tfinal = 35 # 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 = 0.01*1
 F = np.array([[cos(om),sin(om)],
              [-sin(om),cos(om)]])
 G=np.array([[1,0],[0,1]])
 C=np.array([[1,0],[0,1]])
 Q=np.array([[3.25e-06, 6.50e-05],
            [6.50e-05, 1.30e-03]])
 R=np.array([[0.1]])
 xo = np.array([0,0.5])
 Pk=np.array([[100,0],[0,100]])

 c1 = UnknownWaveSource(1,0,0.5,1,0) # Wellen-Funktion
 # Wellen-Funktion um pi/2 verschoben um eine cosinus-ähnliche Funktion zu erhalten:
 c2 = UnknownWaveSource(2,0,0.5,1,pi/2)

 c3 = Noise(1,3,0.4) # Ausgabe Sensor sinus-ähnlich
 c4 = Noise(2,4,0.4) # Ausgabe Sensor cosinus-ähnlich

 c5 = KalmanFilter([3,4],[5,6],0,2,F,G,C,Q,R,xo,Pk,0.01)


 sc.AddListComponents(np.array([c1,c2,c3,c4,c5]));

 #Run the schematic and plot:
 out = sc.Run(np.array([1, 2, 3, 4,5]))

 fig = plt.figure()
 ax = fig.add_subplot(1, 1, 1)
 fig.set_dpi(120)
 ax.plot(out[0,:],out[4,:],'lightgrey',out[0,:],out[3,:],'lightblue',out[0,:],out[1,:],'blue',out[0,:],out[5,:],'green')
 ax.grid()
 ax.legend(['Sensor','Sensor','Original','Ausgabe Kalmanfilter'])
 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 Kalmanfilter es schafft, aus unserer mathematischen Vorhersage und den Daten der Sensoren, eine Ausgabe zu erzeugen, welche den der Verlauf der Wellen nahe kommt. 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 Kalmanfilter:

 %% 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>
 x
 <br><br>
 <h3>Ende</h3>