kramann.info
© Guido Kramann

Das Kalmanfilter

(EN google-translate)

(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

Bild 0-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 0-2: Zustandsregler mit Beobachtermodell: Die Differenz zwischen realem Systemzustand und simuliertem Systemzustand wird auf den Eingang der simulierten Regelstrecke rückgekoppelt.

Bild 0-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 0-1: Simuliertes System beim Zustandsregler mt Beobachtermodell und Kalmanfilter als Filtermatrix L.

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

Formel 0-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 0-3: Bestimmung des Kalmanfilters.

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

$ \dot x = Ax+ Bu + r_x $

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

$ y = Cx + Du + r_y $

Formel 0-5: Zus \tan dsraumdarstellung & 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 0-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 r_y.

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 F_A wirke in Q.
Sonstige Parameter:
Dicker homogener Stab, r=0.5m, Stabdicke d=0.05m, vergl. Kapitel 9.1.

Bild 0-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 y₀=[0.1,0,0.1,0] für t=[0,10] mit Δt=0.01s.
Es sollte folgender Verlauf heruaskommen:

Bild 0-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 0-1: Vorlage für Aufgabe 1.