Posts Tagged‘C’

Einleitung

Für ein Vorstellungsgespräch bei meinem jetzigen Arbeitgeber habe ich unter anderem nach den gängigsten Filtern zur Beschreibung von Bewegungen im Raum gesucht. Dabei bin ich auf dieses 4D Kalman Filter gestoßen. Dort werden die mathematischen Zusammenhänge und die Implementierung des Filters in Matlab beleuchtet. Da ich bei meiner aktuellen Arbeitstelle allerdings mit C-Code arbeite, habe ich mir zum Ziel gesetzt, den Algorithmus in C umzusetzen.

Mein Vorschlag ist daher keine Kritik oder Neuentwicklung des besprochenen Filters an sich, sondern ist vielmehr als Ergänzung für Entwickler gedacht, die lieber in C arbeiten.

Der komplette Source-Code befindet sich hier. Abhängigkeiten zu externen (nicht-Standard-) Bibliotheken wurden gezielt vermieden, allerdings wurde an zwei Stellen externen Code mit in das Projekt eingebaut und entsprechend erwähnt. Dieser Filter kann durch einfache Änderungen als Basis für einen echten eingebetteten Filter hergenommen werden.

Ich werde hier Schritt für Schritt durch die wichtigsten Stellen des Codes gehen und an geeigneten Stellen auf den originalen Algorithmus verweisen.

Sollten programmiertechnische Fehler auffallen oder gar Copyright-Probleme entdeckt werden, bitte ich sehr darum, sich bei mir zu melden oder selbst über Github aktiv zu werden. Der Code wurde auf meiner Maschine mit meinem Compiler getestet, ein plattformübergreifendes Funktionieren kann nicht garantiert werden. Ich bin aber gerne für konstruktive Kritik offen!

Source Code

Inhalt von KalmanFilter.c

Zeilen 1 – 6

Benötigte Includes, hauptsächlich für Ein- und Ausgabe und zu selbstgeschriebener KalmanFilter.h-Datei.

Zeilen 15 – 26

Erzeugen von Zufallsvariablen. Benutzt wird die Box Muller Transformation zur Erzeugung Gauß-verteilter Zufallszahlen (entnommen von hier). Dies entfällt bei einem echten System.

Zeilen 29 – 37

Matrizen werden entsprechend dem Beispiel mit Anfangsdaten befüllt.

Zeilen 40 – 57

In diesen Matrizen werden Zwischenergebnisse gespeichert. Diese werden hier mit Nullen initialisiert.

Zeilen 59 – 62

Es wird eine Ergebnis-Datei geöffnet und der Gnuplot initialisiert.

Zeilen 64 – 112

Hauptschleife, die den Algorithmus implementiert. Kann im Falle einer echten Anwendung durch eine while-Schleife ersetzt werden, in der zyklisch Werte abgefragt und weiterverarbeitet werden. Das Abfragen der Werte kann in

Zeilen 78 + 79

geschehen.

Vor jedem Rechenschritt steht der Teil des originalen Algorithmus, der darunter berechnet wird. In

Zeilen 105 – 109

werden Zwischenschritte der Berechnung ausgegeben. Hier kann man zu Debug-Zwecken auch andere Werte verwenden.

Zeilen 115 – 121

Housekeeping

Zeilen 125 bis Ende

Der Rest des C-Files implementiert die nötigen Algorithmen. Bei den meisten handelt es sich um einfache Matrix-Multiplikationen oder Ausgabefunktionen. Die einzig kompliziertere Rechnung ist die der Matrix-Inversen (entnommen von hier). Durch die Ausgabe im gnuplot und als Textdatei, kann der Entwickler schnell und einfach feststellen, dass er richtig entwickelt hat. Im Falle dieses Algorithmus habe ich so recht einfach meinen Output mit dem des Originaloutputs vergleichen können.

Inhalt von KalmanFilter.h

Enthält

Ab Zeile 10

hauptsächlich Funktionsdeklarationen der im vorhergehenden Teil beschriebenen Funktionen.

Ab Zeile 86

werden nützliche Abkürzungen definiert, die den Hauptcode etwas aufgeräumter wirken lassen sollen. Der Schlüssel #define weist den Compiler an, den nachfolgenden String VOR dem Compilieren durch den darauffolgenden String zu ersetzen.

Beispiel:

#define WRITEFLOATARRAYTOFILE(f, arr) int i; for ( i= 0; i < SIZEOFARRAY(arr); i++) fprintf(f, "%f\n", arr[i]);

Schreibt die Werte eines Float-Arrays (arr) in eine Textdatei (f), mit jeweils einem Zeilenvorschub (\n) unter Zuhilfenahme eines weiteren hilfreichen Shortcuts SIZEOFARRAY (da die Größe eines Arrays in C nicht ohne Umwege zu erhalten ist). Im Code ist diese Funktionalität durch die Abkürzung WRITEFLOATARRAYTOFILE(f, arr) zu erreichen.

Inhalt von CleanMakeAndRun.bat

Dieses Skript entfernt zuvor gebaute Objekte und Executables und baut den Source Code neu. Außerdem wird er nach dem Bauen ausgeführt und das eventuell schon vorhandene gnuplot Fenster vor dem Ausführen geschlossen. So kann nach Änderungen am Code im gleichen Fenster neu gecleanet, gebaut und ausgeführt werden; alles mit einem Tastendruck.

Wie das funktioniert, wird im folgenden Video dargestellt.

Inhalt von Makefile

Eigentliche Anweisungen an den Compiler. Muss bei Hinzufügen, Ändern oder Löschen von Source-Code angepasst werden.

Inhalt von Kalman.m

Leicht angepasster Original-Algorithmus von hier.

Abhängigkeiten

Das Programm muss mit einem geeigneten C-Compiler compiliert werden. Ich selbst benutze MinGW auf Windows 10. Falls gnuplot zur Anzeige der Daten genutzt werden soll, ist die Installation von gnuplot erforderlich.

Vergleich der Output-Daten

Wie besprochen, handelt es sich im hier vorgeschlagenen Beitrag um eine Code-Portierung von Matlab auf C. Die folgenden Bilder zeigen, dass beide Algorithmen sich mit gleich parametriertem Filter in etwa gleich verhalten. Da es sich in beiden Fällen um “echte” Zufallszahlen handelt, kann nie der exakt gleiche Output erwartet werden.

Folgende Abbildung zeigt den Output des originalen Matlab-Algorithmus.

Der vorgeschlagene Ansatz in C liefert folgenden Output: