kramann.info
© Guido Kramann

Login: Passwort:










14 Das Kalmanfilter

14 The Kalman filter (EN google-translate)

14 Filtr Kalmana (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:

In connection with state controllers with observers, the Kalman filter has proven itself. After its inventor Rudolf Emil Kalman, it is an optimal estimator, based on past noisy condition measurements and a model that is as accurate as possible filter current states and predict future ones. As a reminder, a representation of a state controller (see Chapter 8), a state controller with observer (see Chapter 9) and a State controller with observer and Kalman filter:

W połączeniu ze sterownikami stanu z obserwatorami sprawdził się filtr Kalmana. Po wynalezieniu Rudolfa Emila Kalmana jest to optymalny estymator, na podstawie ostatnich hałaśliwych pomiarów stanu i modelu, który jest tak dokładny, jak to tylko możliwe filtruj bieżące stany i przewiduj przyszłe. Dla przypomnienia, reprezentacja kontrolera państwowego (patrz Rozdział 8), kontroler stanu z obserwatorem (patrz Rozdział 9) i Kontroler stanu z obserwatorem i filtrem Kalmana:

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

The Kalman filter was put into practice for the first time during the Apollo program, s. following articl

Filtr Kalmana został zastosowany po raz pierwszy w programie Apollo. następujący artyku

"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
practice session
sesja praktyka
  • 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)
Task 1 (Preparations)
Zadanie 1 (przygotowania)
  • 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)
Task 2 (Preparations)
Zadanie 2 (przygotowania)
  • 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.