|
|
| Zeile 18: |
Zeile 18: |
| * <code>SenKam_StoplinieFlag_bit</code> | | * <code>SenKam_StoplinieFlag_bit</code> |
|
| |
|
|
| |
| == Geschwindigkeitsaufzeichnung ==
| |
| Zeichnen Sie eine Rohdatensequenz von mindestens 30 s auf. Diese sollte nachfolgende Phasen umfassen
| |
| # Stillstand
| |
| # Beschleunigung
| |
| # Konstante Geschwindigkeit
| |
| # Verzögerung
| |
| # Stillstand
| |
| Rohdaten sind die Flanken, die vom Hallsensor in der DS1104 ankommen.
| |
|
| |
| == Schnittstelle zu Matlab ==
| |
| Lesen Sie die aufgezeichneten Daten sequentiell in Matlab (nicht Simulink) ein.
| |
|
| |
| == Modellierung ==
| |
| Erstellen Sie ein System und Messmodell. Als Systemmodell ist ein Ruck-Null-Modell zu wählen.
| |
|
| |
| Der Zustandsvektor lautet
| |
| <math>\underline{x}={x \choose \dot{x}} </math>
| |
|
| |
| == Kalman Filter ==
| |
| Implementieren Sie basierend auf der Vorlesung ein Kalman-Filter zur Schätzung des Zustandsvektors. Hierzu dürfen keine Toolboxen verwendet werden.
| |
|
| |
| == Ergebnisdarstellung ==
| |
| Stellen Sie Messung und Schätzung in je einem Diagramm <math>x(t)</math> und <math>\dot{x}(t)</math> dar. Stellen Sie die Schätzwerte den Messwerten des Filters gegenüber.
| |
|
| |
| == Code Review ==
| |
| Machen Sie für Herrn Petersen 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 ==
| |
| Legen Sie einen Entwicklungszweig (Branch) an und führen Sie einen Systemtest auf dem Fahrzeug durch. Vergleichen Sie das bisherige Filter mit dem Ergebnis des Kalman-Filters.
| |
|
| |
| == 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).
| |
|
| |
| == Ergebnis ==
| |
| === Konzept ===
| |
| In der Abbildung 1 ist das Konzept zur Implementierung eines Kalman-Filters dargestellt. Dieses Konzept basiert auf dem Konzept zum Projekt "Carrera CV" <ref>[http://www.mi.hs-rm.de/~schwan/Projects/CG/CarreraCV/doku/index.htm | Projekt "Carrera CV"]</ref> und zeigt den groben Programmablaufplan und kann hier eingesehen werden<ref>[http://193.175.248.171/wiki/index.php/Datei:PAP-Kalman.png | Konzept zum Kalman-Filter]</ref>.
| |
|
| |
| === Einlesen in MatLab ===
| |
| Zum Einlesen der daten aus der Aufnahme wurde diese als ".mat"-Datei exportiert. Über das nachstehende Script können die Daten aus der Aufnahme in MatLab eingelesen werden. Gleichzeitig konvertiert das Script die Hall-Signal zum Geschwindigkeitssignal.
| |
| <source lang="matlab">
| |
| function [HALLA, HALLB, HALLC] = extract()
| |
| %% EXTRACT DATA
| |
| data = load('HAll-Sensoren.mat');
| |
| TIME = double(data.HAll_Sensoren.X.Data');
| |
| HALL = [TIME double(data.HAll_Sensoren.Y(1).Data').*2^2+double(data.HAll_Sensoren.Y(2).Data').*2^1+double(data.HAll_Sensoren.Y(3).Data').*2^0];
| |
|
| |
| %% INTERPRET DATA
| |
| [~,move] = ismember(HALL(:,2),[5 4 6 2 3 1]);
| |
| change = [find((move(1:end-1)-move(2:end))~=0) ;size(HALL,1)];
| |
|
| |
| %% SPEED CALCULATION
| |
| v_tmp = 30./(change(2:end)-change(1:end-1)).*0.05;
| |
| v = zeros(numel(change)-1,1);
| |
| for k=1:numel(change)-1
| |
| v(change(k):change(k+1),1) = v_tmp(k);
| |
| end
| |
| end
| |
| </source>
| |
|
| |
| === Kalman-Filter ===
| |
|
| |
| Die Implementierung des Kalman-Filters basiert auf dem Kalman-Filter von Student Dave<ref>[http://studentdavestutorials.weebly.com|Student Dave - 1D Kalman-Filte]</ref>.
| |
|
| |
| <source lang="matlab">
| |
| function [vel_estimate,acc_estimate,pos] = stepKalman(v)
| |
| %****************************************************************
| |
| % Funktion : Kalman-Filter *
| |
| % *
| |
| % Implementation : MATLAB 2013b *
| |
| % *
| |
| % Author : StudentDave/ Michael A. Goodrich *
| |
| % http://studentdavestutorials.weebly.com *
| |
| % *
| |
| % Bemerkung : for licensing and usage questions *
| |
| % email scienceguy5000 at gmail. com *
| |
| % *
| |
| % This code is adapted and modified to fit *
| |
| % for the usage. *
| |
| % *
| |
| %***************************************************************/
| |
|
| |
| close all
| |
| %% define our meta-variables (i.e. how long and often we will sample)
| |
| dt = .005; %timestep
| |
| duration = numel(v)*dt-dt; %measurement time
| |
|
| |
| %% Define update equations (Coefficent matrices): A physics based model for where we expect the Quail to be [state transition (state + velocity)] + [input control (acceleration)]
| |
| A = [1 dt; 0 1] ; % state transition matrix
| |
| B = [dt^2/2; dt]; %input control matrix
| |
| C = [1 0]; % measurement matrix
| |
|
| |
| %% define main variables
| |
| u = 0; % define acceleration magnitude
| |
| Q= [0; 0]; %initized state
| |
| Q_estimate = Q; %x_estimate of initial location estimation
| |
| Accel_noise_mag = 1; %process noise
| |
| Vel_noise_mag = 0.1; %measurement noise
| |
| Ez = Vel_noise_mag^2;% Ez convert the measurement noise (stdv) into covariance matrix
| |
| Ex = Accel_noise_mag^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % Ex convert the process noise (stdv) into covariance matrix
| |
| P = Ex; % estimate of initial position variance (covariance matrix)
| |
| %% initize result variables
| |
| % Initialize for speed
| |
| Q_vel_meas = v(2:end-1);
| |
|
| |
|
| |
| %% MAGIC MAGIC
| |
| %initize estimation variables
| |
| vel_estimate = []; % Quail position estimate
| |
| acc_estimate = []; % Quail velocity estimate
| |
| pos = [0];
| |
| for t = 1:length(Q_vel_meas)
| |
| % Predict next state with the last state and predicted motion.
| |
| Q_estimate = A * Q_estimate + B * u;
| |
| %predict next covariance
| |
| P = A * P * A' + Ex;
| |
| % predicted measurement covariance
| |
| % Kalman Gain
| |
| K = P*C'*inv(C*P*C'+Ez);
| |
| % Update the state estimate.
| |
| Q_estimate = Q_estimate + K * (Q_vel_meas(t) - C * Q_estimate);
| |
| % update covariance estimation.
| |
| P = (eye(2)-K*C)*P;
| |
| %Store for plotting
| |
| vel_estimate = [vel_estimate; Q_estimate(1)];
| |
| acc_estimate = [acc_estimate; Q_estimate(2)];
| |
| pos = [pos; pos(end)+Q_estimate(2)/2*dt^2+Q_estimate(1)*dt];
| |
| end
| |
|
| |
| end
| |
| </source>
| |
| ===Ergebnisdarstellung ===
| |
| Die nachfolgende Grafik zeigt die Hall-Signale überlappend. Zur besseren Darstellung wurde das Signal des Hall-Sensors B mit 2 addiert und der Hall-Sensor C mit 4. Die Kombinatorik der Sensoren gibt an, wie der aktuelle Status des Motors ist. Aufgetragen ist jeweils der Status S zum Zeitpunkt x.
| |
| [[Datei:Hall-Signal.png|center|800px]]
| |
| Das Ergebnis der Kalman-Filterung sieht folgend aus. Hierbei ist die Geschwindigkeit je Zeitpunkt aufgetragen.
| |
| [[Datei:Result_Kalman-V.png|center|500px]]
| |
|
| |
| === Modultest ===
| |
| Eingangssignal ist ein Geschwindigkeitssignal, welches in den Zeitsteps von 1:200 eine Beschleunigung erfährt, danach folgt eine konstante Geschwindigkeit für 600 Zeitsteps, anschließend erfolgt eine Verzögerung bis zum Stillstand über 200 Zeitsteps. Das Ergebnis des Kalman Filters ist nachfolgend abgebildet:
| |
| [[Datei:Simulation_Kalman.png|center|500px]]
| |
| Das Ergebnis zeigt deutlich, dass der Kalman-Filter anfällig ist gegen abrupte Änderungen nach langen konstantem Verhalten. Jedoch schwingt sich dieser ohne Überschwingen auf den neuen Wert recht schnell ein.
| |
| ===Systemtest===
| |
| Nachfolgend ist der Vergleich zwischen der aktuellen Filterung via PT1 und der Filterung via Kalman-Filter dargestellt. Die Datengrundlage ist für beide Filter gleich. Das Diagramm zeigt den Kalman-Filter in rot und den PT1-Filter in blau.
| |
| Es ist deutlich zu erkennen, dass der PT1-Filter verzögert, während der Kalman-Filter einem stärkeren Schwanken unterliegt. Gleichfalls überschwingt der Kalman-Filter bei abrupter Änderung der Geschwindigkeit. Das Schwanken verstärkt sich, je länger vorher eine konstante Geschwindigkeit gefahren wurde.
| |
| [[Datei:Kalman_vs_PT1.png|center|500px]]
| |
| ===Dokumentenablage===
| |
| Alle Dokumente lassen sich im SVN unter ".\Teams\Geschwindigkeit_per_Interrupt\Sonderaufgabe" einsehen.
| |
|
| |
| === Einzelnachweis ===
| |
| <references/>
| |
|
| |
|
| ---- | | ---- |
| → zurück zum Hauptartikel: [[Geschwindigkeitsermittlung|Geschwindigkeitsermittlung]] | | → zurück zum Hauptartikel: [[Geschwindigkeitsermittlung|Geschwindigkeitsermittlung]] |