Kalman-Filter zur Bestimmung der Geschwindigkeit aus den LIDAR-Daten

→ zurück zum Hauptartikel: Geschwindigkeitsermittlung

Bearbeiten Sie nachfolgende Aufgaben bis zum Abgabetermin und stellen Sie Ihre Lösung bis zum Abgabetermin Prof. Göbel vor. Gehen Sie systematisch in den in SDE vermittelten Schritten

Theorie
Konzept
Modellierung
Umsetzung
Testing

vor.

Die erstellten Dateien wurden, wenn nicht anders angegeben, im Ordner MTR_SDE_Praktikum/trunk/Daten/Kalman-Filter zur Bestimmung der Geschwindigkeit aus den LIDAR-Daten gespeichert.
LIDAR steht im gesamten Artikel für den auf dem AMR des SDE-Praktikums verbauten Laser-Scanner mit einem Rundumlaser.

Konzept

Als Aufgabenstellung ist anhand der LIDAR-Messwerte die Egogeschwindigkeit des Fahrzeugs in Längsrichtung mittels Kalman-Filter zu schätzen.

Aufzeichnung

Zeichnen Sie im Praktikum mit Hilfe des Teams Deitel/Groß eine Geradeausfahrt auf ein stehendes Ziel (z.B. Karton) auf. Diese Aufzeichnung sollte auch die aus den Hallsensoren ermittelte Geschwindigkeit und Position enthalten.

Die Aufzeichnung geschieht mit dem AMR des SDE-Praktikums gesteuert über dSPACE ControlDesk®. Das genaue Vorgehen bei einer Messung mit ControlDesk® mit Hilfe eines selbst konfigurierten Recorders wird in dem Artikel Messungen mit dSPACE ControlDesk beschrieben.
Es wird eine einfache Geradeausfahrt mit relativ langsamer Geschwindigkeit auf ein stehendes Hindernis aufgezeichnet. Als Hindernis werden nacheinander ein Rollcontainer und ein Papierkarton verwendet. Jede Art von Messung wird mehrmals wiederholt, um die Messdaten dadurch zu validieren. Da die Kommunikation zwischen dem LIDAR und der dSPACE-Autobox zwischendurch abbricht, sind ebenfalls mehrere Messungen erforderlich. Aus den Aufzeichungsdaten kann dann für die Auswertung ein geeigneter Datensatz ausgewählt werden.
Folgende Daten werden aufgenommen:

Streckenmessung Hallsensoren SenVx_sx_K_f64
Geschwindigkeit Hallsensoren SenVx_vx_K_f64
Winkel der Objekte Signal_Object_angle als Array [5x1]
Tiefe der Objekte Signal_Object_depth als Array [5x1]
Wahrscheinlichkeit der Objekte Signal_Object_plausibility als Array [5x1]
Geschwindigkeit der Objekte Signal_Object_speed als Array [5x1]
Breite der Objekte Signal_Object_width als Array [5x1]
X-Koordinate der Objekte Signal_Object_x als Array [5x1]
Y-Koordinate der Objekte Signal_Object_y als Array [5x1]
Parameter der Objekte Signal_Object_params als Array [5x1]
Zähler der Objekte Signal_Object_count

Die Messdaten werden als mat-File exportiert und können in Matlab eingelesen werden.
Die Mess-Dateien als exportierte mat-Files liegen in im SVN-Archiv des SDE-Pratikums unter MTR_SDE_Praktikum/trunk/Software/CaroloCupFahrzeug/dSPACE/Carolo Cup Fahrzeug/Inbetriebnahme/Measurement Data/. Es sind die Dateien lid001.mat bis lid015.mat.

Schnittstelle zu Matlab

Lesen Sie die Messwerte in Matlab (nicht Simulink) ein.

Da die Qualität und Verwendbarkeit der Messdaten untersucht werden muss, werden zunächst alle Messungen geladen. Dies geschieht mit dem load-Befehl in Matlab. Hier ein Beispiel:

% Datei mit Pfad und Dateinamen festlegen
Datei=([Pfad,Name]);
messung = load(Datei);

Signalnamen

Um eine schnelle Übersicht der getriggerten Signale zu bekommen, können die Signale in eine separate char-Variable gespeichert werden. Dabei muss berücksichtigt werden, dass die Messdaten in einer struct-Variabel gespeichert sind.

%% Namen der Signale extrahieren
% enthaltene Namen aus einer Messung (hier lid001) auslesen
for k=1:45
    namen_Signale.cell(k) = cellstr(lid001.Y(1,k).Name);
end

% Signalnamen an Matlab-Konvention anpassen, entfernen eckiger Klammern
for ii=1:45
    namen_Signale.cell(ii)=strrep(namen_Signale.cell(ii), '[',);
    namen_Signale.cell(ii)=strrep(namen_Signale.cell(ii), ']',);
end

% in char konvertieren
namen_Signale=char(namen_Signale.cell);

Diese beiden Befehle sind in dem Script NamenAuslesen.m zusammengefasst.

Visualisierung

Von den aufgenommenen Messungen können, wie oben angedeutet, evtl. nicht alle verwendet werden. Deswegen werden die einzelnen Signale der Messungen in separaten Plots visualisiert. Hier ist das beispielhaft für die Messung lid008.mat gezeigt.

for ii=1:length(lid008.Y)
    figure
    plot(lid008.X.Data,lid008.Y(1,ii).Data)
    xlabel('t [s]')
    ylabel(strrep(lid008.Y(1,ii).Name,'_','\_'))
    grid on, box on
end

Diese Befehlsfolge befindet sich im Script PlotData.m.
Dadurch können die nicht verwendbaren Messungen schnell identifiziert werden, da ohne genaue Betrachtung der Daten ein erster Eindruck möglich ist. Desweiteren kann für die jeweilige Messung der ungefähre zeitliche Rahmen bestimmt werden. Dies kann mit dem Cursor z. Bsp. im Plot der Streckenmessung herausgefunden werden, vgl. Abbildung Cursor im Matlab-Plot.

Es stellte sich heraus, dass nur folgende Messungen von Null verschiedene Daten enthalten

Messung	Messzeitraum in s
lid004.mat	23.5
lid005.mat	14.8
lid007.mat	27.75
lid008.mat	16.7

Messungen laden

Für das Laden der Messdaten wurde eine separate Funktion MessungLaden.m geschrieben. Nachdem die Variable für die Daten als leere struct-Variable angelegt wurde, wird der Benutzer in einem Dialogfenster nach der einzulesenden Datei gefragt.

%% Messung extrahieren aus Mat-Datei
% Mat-Datei laden
% Benutzer bekommt Pfad-, Dateinamen- und Dateiformat-Vorschlag
[Name,Pfad]=uigetfile({'*.m;*.fig;*.mat;*.slx;*.mdl','MATLAB Files (*.m,*.fig,*.mat,*.slx,*.mdl)';
   '*.m',  'Code files (*.m)';'*.fig','Figures (*.fig)';'*.mat','MAT-files (*.mat)'; ...
   '*.mdl;*.slx','Models (*.slx, *.mdl)';'*.*',  'All Files (*.*)'},'Dateiauswahl',...
          '..\..\Software\CaroloCupFahrzeug\dSPACE\Carolo Cup Fahrzeug\Inbetriebnahme\Measurement Data\lid004.mat');
Datei=([Pfad,Name]);
messung = load(Datei);

Mit der Funktion fieldnames lassen sich in struct-Variablen die Namen der Ebenen auslesen. In der ersten Ebene lässt sich hier somit der ursprüngliche Name des mat-Files abspeichern.

% Messungsnamen auslesen
name_messung = fieldnames(messung);

Im Weiteren wird die Struktur der Messung in die erste Ebene der Variable verschoben. Sollte das nicht möglich sein, wird eine Fehlermeldung ausgegeben.

% in erste Ebene der Struktur verschieben
if length(name_messung) == 1
    messung = messung.(name_messung{1});
else % Fehlermeldung, falls nicht möglich
    fprintf('Messstruktur nicht lesbar, bitte Aufbau der Struktur überprüfen. In der ersten Ebene muss die Messdatei mit seinem Namen stehen');
end

Die Signale werden nun in die zuvor erstellte Variable geschrieben.

% Ergebnisstruktur nur mit den Signalen füllen
Messdaten.(messung.Y(ii).Name) = messung.Y(ii).Data';

Rohdatenverarbeitung

Extrahieren Sie aus LIDAR-Messwerten das stehende Objekt. Sie dürfen gern den Algorithmus aus der Vorlesung verwenden.

Übersichts-Plot

Für die Auswahl aus den vier oben genannten Messungen wurde ein Übersichts-Plot für die relevanten Signale realisiert. Beispielhaft für die Messung Lid007 ist dieser in der Abbildung Übersichts-Plot der Messung Lid007 zu sehen. Der Plot wird mit der Funktion PlotMessung.m aufgerufen.

In dem Fensternamen wurde zur besseren Übersicht der Name der Messung integriert. Dieses lässt sich mit dem Attribut Name in der figure-Anweisung erreichen. Hier ein Beispiel:

figure('Name',['Plot der Messung ' char(name_messung)])

Der Plot besteht aus mehreren Subplots mit jeweils einem Signal über der Zeit. Die Achsen sind beschriftet mit einem Schlagwort und der Einheit des Signals. Die Gitternetzlinien werden angezeigt. Der Code dafür sieht zum Beispiel so aus:

subplot(2,4,1)
hold on
plot(Messdaten.t,Messdaten.SenVx_vx_K_f64, 'b', 'LineWidth', 2)
xlabel('t [s]')
ylabel('Geschwindigkeit v [m/s]')
grid on

Es ist festzustellen, dass die y-Koordinate der Objektdaten in Fahrtrichtung des Fahrzeuges zeigt. Die x-Koordinate zeigt dementsprechend in Fahrtrichtung nach rechts. Die Längenangaben in den Objektdaten sind in Millimeter mm, die Winkel in Radiant rad. Die Werte für die Wahrscheinlichkeit sind in Prozent % angegeben.

Plot der Fahrt

Im weiteren Schritt soll ein erster Eindruck der Bewegung des Fahrzeugs auf das Objekt zu geschaffen werden. Dafür wird in einem Plot das Objekt als Rechteck statisch dargestellt. Im Plot ist der Nullpunkte der Ordinate die fahrzeugzugewandte Seite des Objektes. Die Dimension des Hindernisses ist beispielhaft gewählt mit einer Breite von 0,6 m und einer Tiefe von 0,4 m.

% Objekt alles statisches Rechteck
rectangle('Position',[-0.30,0,0.6,0.4])

Damit das Bild im Plot nicht verzehrt wird, sind die Skalierungen der beiden Koordinaten-Achsen gleich zu wählen.

% Skalierung der Achsen gleichmäßig --> keine Verzerrung
axis equal

Bei der Betrachtung der Signale fällt auf, das die Objektwerte aus der LIDAR-Messung nicht für jeden Zeitstempel aktualisiert werden. Dies ist auch in der sprungförmigen Darstellung der Signale in dem Plot zu sehen, vgl. Abbildung Übersichts-Plot der Messung Lid007. Möglicherweise liegt hier ein Performance-Problem der Datenübertragung zwischen LIDAR und CPU auf dem AMR vor, da anzunehmen ist, dass die Abtastrate des LIDARs deutlich höher ist. Aus diesem Grund werden die Wertepaare, die als Wegpunkte des Fahrzeugs angesehen werden können, in einer zuvor angelegten Matrix position geschrieben. Diese werden dabei von Millimeter in Meter umgerechnet. Die y-Koordinaten werden mit -1 multipliziert, sodass der abnehmende Abstand zum Objekt, das bei 0 steht, visualisiert werden kann.

% Matrix für Wegpunkte vorbereiten, da sich nicht zu jedem Zeitstempel Koordinaten ändern
position=zeros(2,2);
n=1;

% Durchlauf über alle Zeitstempel
for ii=1:length(Messdaten.Signal_Object_x0)
    
    [...]

        position(n,1)=Messdaten.Signal_Object_x0(ii)/1000;
        position(n,2)=-Messdaten.Signal_Object_y0(ii)/1000;
        n=n+1;
end

Sind zwei zeitlich folgende Wertepaare identisch, werden diese nicht gespeichert. Leere Signalwerte werden im Schleifendurchlauf über die Messwerte übersprungen. Mit dem Befehl contiune wird der aktuelle Schleifendurchlauf beendet und der nächste begonnen.

% leere Signale überspringen
if (Messdaten.Signal_Object_x0(ii)==0) || (Messdaten.Signal_Object_x0(ii)==-inf)
   continue
end

Durch diese Maßnahmen kann die Durchlaufzeit des gesamten Skriptes deutlich reduziert werden, da nicht alle Werte für den Plot verwendet werden.
Der Plot der Wegpunkte erfolgt mit roten Kreisen, die Wegstrecke wird durch eine rote Linie dargestellt. Es werden einfach alle Wertpaare der zuvor erstellten Matrix durchlaufen. Dabei muss darauf geachtet werden, dass der Index nicht größer als die Matrix-Dimension wird.

% Durchlauf über alle Wegpunkte
for m=1:length(position)
    
    % Plot der Wegpunkte als rote Kreise
    plot(position(m,1),position(m,2),'ro')

    % Plot der Strecke zwischen zwei Wegpunkten als blaue Linie
    if m<length(position)
       line([position(m,1) position(m+1,1)],[position(m,2) position(m+1,2)])
    end
end

Die Funktion PlotFahrt.m erzeugt diesen Plot, vgl. Abbildung Plot der Fahrt auf das Objekt zu.

Die Aufgabe lautet, das Objekt aus den Messdaten zu extrahieren, da sich aber das Fahrzeug auf das Objekt zubewegt, wurde hier diese Darstellung gewählt. In der Weiterverarbeitung soll es ja auch um die Geschwindigkeit des Fahrzeugs und nicht um die des Objektes gehen.

Modellierung

Erstellen Sie ein System- und ein Messmodell.

Als Systemmodell ist ein Ruck-Null-Modell zu wählen.

Der Zustandsvektor lautet $\underline{x}={x \choose \dot{x}}$

Nutzen Sie ein Kalman-Filter-Tracking, um die Geschwindigkeit und Position des Egofahrzeugs kontinuierlich aus den stehenden Objekten zu bestimmen. Hierzu dürfen keine Toolboxen verwendet werden.

Für das Kalman-Filter sind folgende Schritte notwendig:

Initialisierung
Prädiktion
Schätzung

In diesen wird auch hier vorgegangen.

Initialisierung

Es werden ein System- und ein Messmodell benötigt.

Systemmodell

Als Systemmodell soll ein Ruck-Null-Modell zum Einsatz kommen. Dieses meint ein Bewegungsmodell, bei dem sich die Beschleunigung nicht ändert. Hier ändert sich die Geschwindigkeit zum Ende der Fahrt (vgl. Abbildung Übersichts-Plot der Messung Lid007). Richtungsänderungen kommen nur durch äußere Einwirkungen zustande, es wurde nicht aktiv gelenkt. Somit liegt hier für das System eine gleichmäßig beschleunigte Bewegung vor. Die Beschleunigung lässt sich mit 0.1 m/s² annehmen.
Das allgemeine Systemmodell lautet x_k+1=Ax_k+w_k . Da das Systemrauschen w_k als Null angenommen wird, lässt sich das Systemmodell hier vereinfachen zu x_k+1=Ax_k mit A={1 0,1*t;0 1} .

Messmodell

Das Messmodell beschreibt den Zusammenhang zweier aufeinanderfolgender Messwerte. Das allgemeine Messmodell lautet z_k+1=Cx_k+v_k. Das Messmodell lässt sich hier vereinfachen zu z_k+1=Cx_k+R mit C={1 0} und R als Skalar.

Rauschen

Das Systemrauschen wird als sehr gering angenommen. Die Rauschmatrix Q wird festgelegt mit Q={0,001 0;0 00,1} . Die Werte auf der Hauptdiagonalen sind die Kovarianzquadrate von x und v. Die Werte der Nebendiagonalen beschreiben die Kovarianz zwischen Strecke und Geschwindigkeit, diese ist hier nicht vorhanden.
Das Messrauschen ist relativ groß, wird als Kovarianzquadrat mit 10 festgelegt.

Startwerte

Die initialen Werte werden wie folgt festgelegt:

Strecke x₀=0 m
Geschwindigkeit v₀=0,11 m/s
initiale Fehlerkovarianzmatrix P={1 0;0 1}

Diese Fehlerkovarianzmatrix beschreibt, wie genau die Schätzung war. Da sich diese an dem Plot orientiert, wird sie mit relativ hoher Sicherheit angenommen.
Die Implementierung der Initialisierung in Matlabcode wird hier nur an einem Beispiel vorgestellt.

% initiale Fehlerkovarianzmatrix
P = eye(2);

Die Funktion eye erzeugt eine Diagonalmatrix mit der angegebenen Dimension.

Prädiktion

Die zugrundeliegenden Formeln werden hier nicht weiter erklärt. Diese sind zum Beispiel in Kalman-Filter für Einsteiger einzusehen. Es soll hier nur die Implementierung vorgestellt werden.

%% Prädiktion  
xp = A*x;  
Pp = A*P*A' + Q;

Schätzung

%% Schätzung
K = Pp*C'*inv(C*Pp*C' + R);

x = xp + K*(z - C*xp);
P = Pp - K*C*Pp;   

% Zustandsschätzung  
pos = x(1);
vel = x(2);

Das gesamte Kalman-Filter ist in der Funktion Kalman.m implementiert.

Ergebnisdarstellung

Stellen Sie die Schätzung in je einem Diagramm $x(t)$ und $\dot{x}(t)$ der Messung gegenüber und diskutieren Sie diese.

Plausibilisieren Sie die Strecke und die Geschwindigkeit über die mit dem Hallsensor gemessenen Referenzdaten.

Für die Ergebnisdarstellung wurde ein Skript erstellt StartKalman.m. Dieses bereitet die Messdaten vor, ruft das Kalman-Filter auf und stellt die Ergebnisse dar. Es werden die Daten auf diejenigen Zeitstempel begrenzt, die Daten des LIDAR enthalten.

% Anzahl leerer Werte zu Beginn bestimmen
n=0;
for ii=1:length(ObjektX)
    if (Messdaten.Signal_Object_x0(ii)==0) || (Messdaten.Signal_Object_x0(ii)==-inf)
        n=n+1;
    end
end 

% leere Werte entfernen
ObjektX=ObjektX((n+1):length(ObjektX));

Desweiteren werden die Abstandswerte in eine Streckenmessung umgerechnet. Die Änderung dieser ist dann die gemessene Geschwindigkeit, es stehen nämlich von der LIDAR-Messung kein Geschwindigkeitswerte zur Verfügung. Die mit den Hallsignalen gemessenen Geschwindigkeits- und Streckenwerte werden erst später für die vergleichende Darstellung verwendet.

Streckenmessung

Die Schätzwerte des Kalman-Filters und die Objektdaten werden in der Abbildung Streckenmessung den aus den Messwerten der Hallsensoren errechneten Werten gegenübergestellt.

Es fällt auf, dass die Streckenmessung am Ende bei einem konstanten Messwert stehen bleibt, die Fahrt des Fahrzeugs wurde angehalten. Die LIDAR-Daten realisieren aber weiter eine deutliche Änderung. Dies kann an dem schon angesprochenen Performance-Problem liegen. Die Kalman-Schätzung folgt den Messwerten überhaupt nicht, dies liegt an der sprungartigen Änderung der Objektdaten. Das Systemmodell passt nicht. Wird die mögliche Beschleunigung deutlich angehoben auf 10000 m/s², folgt das Filter den Messwerten deutlich stärker, vgl. Abbildung Streckenmessung mit großem a.

Das Filter lässt sich somit nicht sinnvoll für die Streckenmessung anwenden.

Geschwindigkeitsmessung

Die Schätzwerte des Kalman-Filters und die Objektdaten für die Geschwindigkeit werden in der Abbildung Geschwindigkeitsmessung den Messwerten der Hallsensoren gegenübergestellt.

Es ist deutlich ersichtlich, dass die Objektdaten sehr sprunghaft sind. Aus diesem Grund sind die Geschwindigkeitswerte völlig unrealistisch. Um die übrigen Signale besser erkennen zu können, wurde in den Plot hinein gezoomt. Obwohl bereits die hohe Beschleunigung von 10000 m/s² angenommen wurde, folgt das Kalman-Filter nicht den Objektdaten, es handelt sich ja hier um ein Ruck-Null-Modell als Systemmodell. Für diese Messwerte lässt sich also das Kalman-Filter nicht sinnvoll einsetzen, es müsste eine viel größere Abtastrate des LIDARs geben.
Außerdem fällt auf, dass die LIDAR-Messwerte nicht mit denen der Hall-Signale übereinpassen.

Code Review

Machen Sie für Ihren Quelltext ein Code Review und dokumentieren Sie dieses in der Vorlage (\Anforderungsmanagement\Testverfahren).

Modultest

Führen Sie für Ihre Quellen einen Komponententest durch und dokumentieren Sie diesen entsprechend der Vorlesung Reliability Engineering. Simulieren Sie hierzu die Eingangsdaten, stellen Sie die Ergebnisse dar und diskutieren Sie diese.

Systemtest

Prüfen Sie, ob Ihr Modell negative Auswirkungen auf das Gesamtsystem (EPA, BSF) hat. Führen Sie Ihre Ergebnisse Herrn Prof. Göbel vor und übernehmen Sie nach der Abnahme das Modell in den Hauptzweig (trunk).

Dokumentation

Dokumentieren Sie alle Daten in SVN und die Ergebnisse in diesem Artikel. Berücksichtigen Sie dabei die Kriterien für wissenschaftliches Arbeiten sowie die Anforderungsunterlagen von SDE (z.B. Schnittstellendokumentation.docx, Namenskonventionen.pdf, Lastenheft_AutonomesFahrzeug.docx).

Die erstellten Dateien wurden im Ordner MTR_SDE_Praktikum/trunk/Daten/Kalman-Filter zur Bestimmung der Geschwindigkeit aus den LIDAR-Daten gespeichert.

Fazit

Leider ließ sich die Aufgabe nicht zufriedenstellend bearbeiten, weil die LIDAR-Messung noch nicht so recht funktioniert. Aus diesem Grund wurde der erstellte Quellcode auch nicht getestet und in das Modell CCF integriert. Es stellt sich dem Autor überhaupt die Frage, welches Ziel mit einem solchen Vorhaben erreicht werden soll, da der LIDAR eine dynamische Umgebung mit einer Unsicherheit tragt. Die Messung über Hallsensoren ist für die Geschwindigkeit da deutlich verlässlicher, auch wenn das LIDAR-Tracking mit einem Kalman-Filter verarbeitet werden würde.

Ausblick

Die LIDAR-Messung muss 10 Messwerte pro Sekunde liefern, die sich bei dynamischer Umgebung von vorherigem Zeitschritt unterscheiden.
Die Geschwindigkeitswerte aus der LIDAR-Messung (speed) müssen mit Inhalt gefüllt werden.
Die Messwerte sollten in SI-Einheiten ausgegeben werden, zum Beispiel Meter statt Millimeter.

→ zurück zum Hauptartikel: Geschwindigkeitsermittlung → zurück zum Hauptartikel: Praktikum SDE