Skip to content
Snippets Groups Projects
Commit 932d6404 authored by Adam Friedrich Schrey's avatar Adam Friedrich Schrey
Browse files

Correct misspelling in V10_kalmanfilter_demonstration.ipynb

parent 5bd44b50
No related branches found
No related tags found
1 merge request!1merge develop into master
%% Cell type:markdown id:fifteen-reward tags: %% Cell type:markdown id:fifteen-reward tags:
# <span style='color:OrangeRed'>V10 - Kalmanfilter Demonstration</span> # <span style='color:OrangeRed'>V10 - Kalmanfilter Demonstration</span>
%% Cell type:code id:tough-category tags: %% Cell type:code id:tough-category tags:
``` python ``` python
from systheo2functions import * from systheo2functions import *
%matplotlib inline %matplotlib inline
``` ```
%% Cell type:markdown id:alpha-kitchen tags: %% Cell type:markdown id:alpha-kitchen tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify"> <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: 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: %% Cell type:code id:respective-writer tags:
``` python ``` python
scw = Schema(0,2,0.01,2) # Instance of the simulation schematic 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) w1 = UnknownWaveSource(1,0,0.5,2*pi,0);#TransferFunction(inp,out,num,den)
scw.AddListComponents(np.array([w1])); scw.AddListComponents(np.array([w1]));
outw = scw.Run(np.array([1])) outw = scw.Run(np.array([1]))
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(2, 1, 1) ax = fig.add_subplot(2, 1, 1)
fig.set_dpi(120) fig.set_dpi(120)
ax.plot(outw[0,:],outw[1,:],'blue') ax.plot(outw[0,:],outw[1,:],'blue')
plt.title('unbekannte Wellen-Funktion') plt.title('unbekannte Wellen-Funktion')
plt.show() plt.show()
``` ```
%% Output %% Output
%% Cell type:markdown id:trained-bumper tags: %% Cell type:markdown id:trained-bumper tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify"> <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<img src="bilder/v10_roboter.png"/><br> <img src="bilder/v10_roboter.png"/><br>
Die Sensoren des Roboters erfassen den Verlauf der Wellen, aber leider ist deren Ausgabe ungenau. 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: 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: %% Cell type:markdown id:numerical-europe tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify"> <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 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: %% Cell type:code id:radio-swing tags:
``` python ``` python
om = 0.01*2*pi om = 0.01*2*pi
F = np.array([[cos(om),sin(om)], F = np.array([[cos(om),sin(om)],
[-sin(om),cos(om)]]) [-sin(om),cos(om)]])
x = np.array([0,0.5]) x = np.array([0,0.5])
y1 = [x[0]] y1 = [x[0]]
y2 = [x[1]] y2 = [x[1]]
# Zustandsvorhersage: # Zustandsvorhersage:
# x^ = F*x + G*u # x^ = F*x + G*u
# Hier mit G=0 und u=0 # Hier mit G=0 und u=0
for i in range(0,199): for i in range(0,199):
x = (dot(F,x)) x = (dot(F,x))
y1.append(x[0]) y1.append(x[0])
y2.append(x[1]) y2.append(x[1])
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(1, 1, 1) ax = fig.add_subplot(1, 1, 1)
fig.set_dpi(120) 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', 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') outw[0,:],outw[1,:],'blue')
ax.grid() ax.grid()
ax.legend(['Cosinusfunktion','Sinusfunktion','Wellen-Funktion']) ax.legend(['Cosinusfunktion','Sinusfunktion','Wellen-Funktion'])
plt.show() plt.show()
``` ```
%% Output %% Output
%% Cell type:markdown id:administrative-communist tags: %% Cell type:markdown id:administrative-communist tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify"> <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. 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: %% Cell type:code id:chief-cemetery tags:
``` python ``` python
tini = 0 # Start time tini = 0 # Start time
tfinal = 35 # End time tfinal = 35 # End time
dt = 0.01 # Time Step dt = 0.01 # Time Step
nflows = 7 # Number of data flows in the schematic nflows = 7 # Number of data flows in the schematic
Ts = 0.1 # Sampling time for discrete time Ts = 0.1 # Sampling time for discrete time
sc = Schema(tini,tfinal,dt,nflows) # Instance of the simulation schematic sc = Schema(tini,tfinal,dt,nflows) # Instance of the simulation schematic
om = 0.01*1 om = 0.01*1
F = np.array([[cos(om),sin(om)], F = np.array([[cos(om),sin(om)],
[-sin(om),cos(om)]]) [-sin(om),cos(om)]])
G=np.array([[1,0],[0,1]]) G=np.array([[1,0],[0,1]])
C=np.array([[1,0],[0,1]]) C=np.array([[1,0],[0,1]])
Q=np.array([[3.25e-06, 6.50e-05], Q=np.array([[3.25e-06, 6.50e-05],
[6.50e-05, 1.30e-03]]) [6.50e-05, 1.30e-03]])
R=np.array([[0.1]]) R=np.array([[0.1]])
xo = np.array([0,0.5]) xo = np.array([0,0.5])
Pk=np.array([[100,0],[0,100]]) Pk=np.array([[100,0],[0,100]])
c1 = UnknownWaveSource(1,0,0.5,1,0) # Wellen-Funktion c1 = UnknownWaveSource(1,0,0.5,1,0) # Wellen-Funktion
# Wellen-Funktion um pi/2 verschoben um eine cosinus-ähnliche Funktion zu erhalten: # Wellen-Funktion um pi/2 verschoben um eine cosinus-ähnliche Funktion zu erhalten:
c2 = UnknownWaveSource(2,0,0.5,1,pi/2) c2 = UnknownWaveSource(2,0,0.5,1,pi/2)
c3 = Noise(1,3,0.4) # Ausgabe Sensor sinus-ähnlich c3 = Noise(1,3,0.4) # Ausgabe Sensor sinus-ähnlich
c4 = Noise(2,4,0.4) # Ausgabe Sensor cosinus-ä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) 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])); sc.AddListComponents(np.array([c1,c2,c3,c4,c5]));
#Run the schematic and plot: #Run the schematic and plot:
out = sc.Run(np.array([1, 2, 3, 4,5])) out = sc.Run(np.array([1, 2, 3, 4,5]))
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(1, 1, 1) ax = fig.add_subplot(1, 1, 1)
fig.set_dpi(120) 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.plot(out[0,:],out[4,:],'lightgrey',out[0,:],out[3,:],'lightblue',out[0,:],out[1,:],'blue',out[0,:],out[5,:],'green')
ax.grid() ax.grid()
ax.legend(['Sensor','Sensor','Original','Ausgabe Kalmanfilter']) ax.legend(['Sensor','Sensor','Original','Ausgabe Kalmanfilter'])
plt.show() plt.show()
``` ```
%% Output %% Output
%% Cell type:markdown id:brief-egypt tags: %% Cell type:markdown id:brief-egypt tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify"> <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. Es ist zu sehen, dass der Kalmanfilter 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: %% Cell type:markdown id:anonymous-franklin tags:
## Nähere Erklärung zum verwendeten Kalmanfilter: ## Nähere Erklärung zum verwendeten Kalmanfilter:
%% Cell type:markdown id:posted-visitor tags: %% Cell type:markdown id:posted-visitor tags:
<img src="bilder/v10_kalmanfilter.png"/> <img src="bilder/v10_kalmanfilter.png"/>
%% Cell type:markdown id:colored-desert tags: %% Cell type:markdown id:colored-desert tags:
### Ablauf in KalmanFilter([...],[...],N,M,F,G,C,Q,R,xo,Pk,Ts): ### Ablauf in KalmanFilter([...],[...],N,M,F,G,C,Q,R,xo,Pk,Ts):
%% Cell type:markdown id:blocked-spanish tags: %% Cell type:markdown id:blocked-spanish tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify"> <div style="font-family: 'times'; font-size: 13pt; text-align: justify">
Zustandsvorhersage:<br> Zustandsvorhersage:<br>
x = F × x + G × u x = F × x + G × u
<br><br> <br><br>
Messvorhersage:<br> Messvorhersage:<br>
z = C × x z = C × x
<br><br> <br><br>
Innovation:<br> Innovation:<br>
v = m - z v = m - z
<br><br> <br><br>
Kovarianz der Zustandsvorhersage:<br> Kovarianz der Zustandsvorhersage:<br>
Pk = F × P × F.T + Q Pk = F × P × F.T + Q
<br><br> <br><br>
Kovarianz der Innovation:<br> Kovarianz der Innovation:<br>
S = C × Pk × C.T + R S = C × Pk × C.T + R
<br><br> <br><br>
Kalman Gain:<br> Kalman Gain:<br>
Kk = Pk × C.T × inv(S) Kk = Pk × C.T × inv(S)
<br><br> <br><br>
Kovarianzkorrektur:<br> Kovarianzkorrektur:<br>
Pk = (I - (Kk × C)) × Pk Pk = (I - (Kk × C)) × Pk
<br><br> <br><br>
Zustandskorrektur:<br> Zustandskorrektur:<br>
x = x + (Kk × v) x = x + (Kk × v)
<br><br> <br><br>
Ausgabe des Zustandsvektors:<br> Ausgabe des Zustandsvektors:<br>
Ausgabe = x Ausgabe = x
<br><br> <br><br>
<h3>Ende</h3> <h3>Ende</h3>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment