kramann.info
© Guido Kramann

Login: Passwort:










14 Das Kalmanfilter

14 (EN google-translate)

14 (PL google-translate)

Im Zusammenhang mit Zustandsreglern mit Beobachter hat sich das Kalman-Filter bewährt. Nach seinem Erfinder Rudolf Emil Kalman, handelt es sich um einen optimalen Schätzer, der auf Grundlage vergangener verrauschter Zustandsmessungen und einem möglichst genauen Modell, aktuelle Zustände filtern und zukünftige vorhersagen kann. Zur Erinnerung eine Darstellung eines Zustandsreglers (s. Kapitel 8), eines Zustandsreglers mit Beobachter (s. Kapitel 9) und eines Zustandsreglers mit Beobachter und Kalman Filter:

Das Kalman Filter wurde während des Apollo-Programms zum ersten mal praktisch umgesetzt, s. folgenden Artikel:

"Das Kalman Filter wird 50.", http://www.convict.lu/htm/rob/Das%20Kalman%20Filter%20wird%2050_v_1.4.pdf
Prinzip-Blockschaltbild eines Zustandsreglers.

Bild 14-1: Prinzip-Blockschaltbild eines Zustandsreglers.

Zustandsregler mit Beobachtermodell: Die Differenz zwischen realem Systemzustand und simuliertem Systemzustand wird auf den Eingang der simulierten Regelstrecke rückgekoppelt.

Bild 14-2: Zustandsregler mit Beobachtermodell: Die Differenz zwischen realem Systemzustand und simuliertem Systemzustand wird auf den Eingang der simulierten Regelstrecke rückgekoppelt.

Zustandsregler mt Beobachtermodell und Kalmanfilter (Filtermatrix L).

Bild 14-3: Zustandsregler mt Beobachtermodell und Kalmanfilter (Filtermatrix L).

  • In Gleichungsform im Zeitbereich ausgedrückt ergibt sich:

$ \dot y_s = A y_s + B R \left(-y_s\right) + L \left(y-y_s\right) $

Formel 14-1: Simuliertes System beim Zustandsregler mt Beobachtermodell und Kalmanfilter als Filtermatrix L.


$ \dot y = A y + B R \left(-y_s\right) $

Formel 14-2: Reales System beim Zustandsregler mit Beobachtermodell und Kalmanfilter als Filtermatrix L.


  • Gegenüber dem Zustandsregler mt Beobachtermodell unterscheidet sich die Block-Struktur mit Kalman Filter nur darin, dass die rückgeführte Differenz zwischen realem und simuliertem Zustand vor der Rückführung durch den Kalman Filter L geleitet wird.
  • Bereits im Kapitel 9 wurde für L ein konstanter Verstärkungsfaktor eingesetzt.
  • Beim Kalman Filter wird L nun optimal bestimmt mit:

$ L = PC^TS^{-1} $

Formel 14-3: Bestimmung des Kalmanfilters.


  • Die Herleitung der Formel basiert auf der Annahme, dass auf das zu regelnde lineare System normalverteiltes weißes Rauschen rx, bzw. ry wirkt:

$ \dot x = Ax+ Bu + r_x $

Formel 14-4: Einwirkung von Störungen auf das zu regelnde System, hier in allgemeiner Zustandsraumdarstellung, Zustandsgleichungen.


$ y = Cx + Du + r_y $

Formel 14-5: Zustandsraumdarstellung, zugehörige Ausgangsgleichungen.


  • x: Zustandsvektor nx1
  • u: Eingangsvektor rx1
  • y: Ausgangsvektor mx1
  • A: Systemmatrix nxn
  • B: Eingangsmatrix nxr
  • C: Ausgangsmatrix mxn
  • D: Durchgangsmatrix mxr
  • (Vergl. Kapitel 9)
  • Vereinfacht im Fall C==Einheitsmatrix, und D=0:

$ \dot y = Ay+ Bu + r_y $

Formel 14-6: Einwirkung von Störungen auf das zu regelnde System, vereinfacht.


  • Die Matrizen P, C, S bestimmen sich folgendermaßen:
  • P entspricht der P-Matrix nach Riccati, vergl. Kapitel 8.
  • C ist die Ausgangsmatrix
  • S ist die Kovarianzmatrix für den Rauschanteil der Ausgangsgleichungen ry.
  • S ist im allgemeinen nicht bekannt und wird dann als Einheitsmatrix angesetzt und später optimiert.
Übungsbeispiel
  • Der Umgang mit einem Kalman Filter soll an einem konkreten Beispiel geübt werden.
  • Ein invertierendes Pendel wird durch einen mit einem Ende Q auf einer Ebene reibungsfrei gleitenden Stab modelliert.
  • Eine Antriebskraft FA wirke in Q.
  • Sonstige Parameter:
  • Dicker homogener Stab, r=0.5m, Stabdicke d=0.05m, vergl. Kapitel 9.1.
Neue Variante zur Modellierung eines invertierenden Pendels.

Bild 14-4: Neue Variante zur Modellierung eines invertierenden Pendels.

Aufgabe 1 (Vorbereitungen)
  • Modellieren Sie obiges System über die Newton-Euler-Gleichungen bzgl. des Schwerpunktes.
  • Verwenden Sie folgenden Zustandsvektor: y=[φ, ω, x, v].
  • Linearisieren Sie das System anschließend um y=0 folgendermaßen: cosφ=1, sinφ=φ, quadratische Zustandsgrößen werden Null gesetzt.
  • Bestimmen Sie dann mit Scilab nach Riccati die optimierte Rückführungsmatrix R eines passenden Zustandsreglers und damit auch die Matrix P (vergl. Kapitel 8 unten und Code 9-3 in Kapitel 9: XX entspricht P).
  • Simulieren Sie das linearisierte System für y0=[0.1,0,0.1,0] für t=[0,10] mit Δt=0.01s.
  • Es sollte folgender Verlauf heruaskommen:
Plot zur Simulation des linearisierten Riccati-geregelten invertierenden Pendels.

Bild 14-5: Plot zur Simulation des linearisierten Riccati-geregelten invertierenden Pendels.

Aufgabe 2 (Vorbereitungen)
  • Setzen Sie nun das Schema Bild 21-2, Formel 21-1, Formel 21-2 mit Scilab um, wobei Sie das reale System über die Simulation des nichtlinearen Systems abbilden.
  • Es gibt im Simulationsprogramm also 8 Zustandsgleichungen.
  • Die ersten vier sollen das reale System sein, die nachfolgenden vier das Beobachtersystem.
  • Zunächst soll es keine Störungen geben und keine Unterschiede in den AB zwischen realem und simuliertem System.
  • Vorlage für Aufgabe 1:
//Bestimmung der Rückführmatrix für einen Riccati-Zustandsregler
//Danach Simulation des linearisierten Systems.
r=0.5;
d=0.05;
m=1.0;
J=0.25*m*((d/2)^2)+(1/12)*m*((2*r)^2);
g=9.81;

//Systemmatrix:
AA=[????];

//Eingangsmatrix / Rückführungsmatrix
BB=[????];

//Durchgangsmatrix:
DD=[0;0;0;0];

//Ausgangsmatrix:
CC=[1,0,0,0; 0,1,0,0; 0,0,1,0; 0,0,0,1];

GG = syslin('c',AA,BB,CC,DD);
[PP,FF] = gcare(GG);

//Matrix P:
PP

RR = BB'*PP

global r,d,m,J,g,  AA,BB,DD,CC,PP,RR;

function f = rechteSeite(t,y)
    f = (????)*y
endfunction


t = 0:0.01:10;
y0 = [0.1,0,0.1,0]';
t0 = 0;
y  = ode(y0,t0,t,rechteSeite);

plot(t,y(1,:),'red',t,y(3,:),'blu',);
a = gca();
a.x_label.text = 't/s';
a.y_label.text = 'y';
a.title.text = 'Riccati-optimierter Zustandsregler für lin. inv. Pendel';
hl=legend(['phi';'x'],a=1);

Code 14-1: Vorlage für Aufgabe 1.