Dozent:	Prof. Dr.-Ing. Schneider
Modul	Multisensorsysteme (Wahlpflichtprofil „Systems Design Engineering“), Wintersemester
Modulbezeichnung:	MTR-B-2-7.10
Modulverantwortung:	Ulrich Schneider
Lehrveranstaltung:	Praktikum Multisensorsysteme
Lektionen:	6 - Alpha-Beta-Gamma-Filter

Aufgabenstellung

Ein autonomer mobiler Roboter fährt entlang einer geraden Strecke. Seine Position wird durch ein Lokalisierungssystem gemessen. Die Messwerte enthalten Rauschen. Mit einem Alpha-Beta-Gamma-Filter sollen die Position, die Geschwindigkeit und die Beschleunigung des Roboters geschätzt werden.

Im Gegensatz zum Alpha-Beta-Filter berücksichtigt das Alpha-Beta-Gamma-Filter zusätzlich die Beschleunigung im Bewegungsmodell.

Gegeben sind:

Abtastzeit: $T_{s} = 0, 1 s$
Anfangsgeschwindigkeit: $v_{0} = 0, 5 m / s$
Beschleunigung: $a = 0, 1 m / s^{2}$
Standardabweichung des Messrauschens: $σ_{z} = 0, 15 m$
Anfangsposition: $x_{0} = 0 m$

Bearbeiten Sie folgende Aufgaben:

Erzeugen Sie simulierte Positionsmessungen mit additivem, normalverteiltem Messrauschen.
Implementieren Sie die Prädiktion und Korrektur des Alpha-Beta-Gamma-Filters.
Stellen Sie die wahre Position, die Messwerte und die geschätzte Position grafisch dar.
Stellen Sie die wahre und geschätzte Geschwindigkeit sowie die wahre und geschätzte Beschleunigung dar.
Untersuchen Sie die Parameterkombinationen $(α, β, γ) = (0, 4; 0, 1; 0, 02)$ , $(0, 7; 0, 2; 0, 05)$ und $(0, 9; 0, 4; 0, 15)$ .
Berechnen Sie den RMSE der Positionsschätzung und diskutieren Sie den Einfluss der Parameter.

Lösung

clear; close all; clc;

%% Simulation des mobilen Roboters
rng(1);                    % Reproduzierbare Messdaten

Ts = 0.1;                  % Abtastzeit [s]
t = 0:Ts:20;               % Zeitvektor [s]
N = numel(t);

x0 = 0;                   % Anfangsposition [m]
v0 = 0.5;                 % Anfangsgeschwindigkeit [m/s]
aTrueValue = 0.1;         % Beschleunigung [m/s^2]
sigmaZ = 0.15;            % Standardabweichung [m]

% Wahre Bewegung mit konstanter Beschleunigung
xTrue = x0 + v0*t + 0.5*aTrueValue*t.^2;
vTrue = v0 + aTrueValue*t;
aTrue = aTrueValue * ones(size(t));

% Verrauschte Positionsmessung
z = xTrue + sigmaZ * randn(size(t));

%% Parameterkombinationen [alpha, beta, gamma]
params = [
    0.4  0.1  0.02
    0.7  0.2  0.05
    0.9  0.4  0.15
];

nParams = size(params, 1);

xHat = zeros(nParams, N);
vHat = zeros(nParams, N);
aHat = zeros(nParams, N);
rmse = zeros(nParams, 1);

%% Alpha-Beta-Gamma-Filter
for j = 1:nParams

    alpha = params(j, 1);
    beta  = params(j, 2);
    gamma = params(j, 3);

    % Initialisierung
    xHat(j,1) = z(1);
    vHat(j,1) = v0;
    aHat(j,1) = 0;

    for k = 2:N

        % 1. Praediktion
        xPred = xHat(j,k-1) ...
            + Ts*vHat(j,k-1) ...
            + 0.5*Ts^2*aHat(j,k-1);

        vPred = vHat(j,k-1) ...
            + Ts*aHat(j,k-1);

        aPred = aHat(j,k-1);

        % 2. Innovation
        r = z(k) - xPred;

        % 3. Korrektur
        xHat(j,k) = xPred + alpha*r;

        vHat(j,k) = vPred ...
            + (beta/Ts)*r;

        aHat(j,k) = aPred ...
            + (gamma/(2*Ts^2))*r;

    end

    % RMSE der Positionsschaetzung
    rmse(j) = sqrt(mean((xTrue - xHat(j,:)).^2));
end

%% Positionsverlauf
figure;
plot(t, xTrue, 'k-', 'LineWidth', 2);
hold on;
plot(t, z, '.', 'Color', [0.6 0.6 0.6]);

for j = 1:nParams
    plot(t, xHat(j,:), 'LineWidth', 1.3);
end

grid on;
xlabel('Zeit t [s]');
ylabel('Position x [m]');
title('Alpha-Beta-Gamma-Filter: Positionsschaetzung');
legend('Wahre Position', 'Messung', ...
    'alpha=0.4, beta=0.1, gamma=0.02', ...
    'alpha=0.7, beta=0.2, gamma=0.05', ...
    'alpha=0.9, beta=0.4, gamma=0.15', ...
    'Location', 'northwest');

%% Geschwindigkeitsverlauf
figure;
plot(t, vTrue, 'k--', 'LineWidth', 2);
hold on;

for j = 1:nParams
    plot(t, vHat(j,:), 'LineWidth', 1.3);
end

grid on;
xlabel('Zeit t [s]');
ylabel('Geschwindigkeit v [m/s]');
title('Alpha-Beta-Gamma-Filter: Geschwindigkeitsschaetzung');
legend('Wahre Geschwindigkeit', ...
    'alpha=0.4, beta=0.1, gamma=0.02', ...
    'alpha=0.7, beta=0.2, gamma=0.05', ...
    'alpha=0.9, beta=0.4, gamma=0.15', ...
    'Location', 'best');

%% Beschleunigungsverlauf
figure;
plot(t, aTrue, 'k--', 'LineWidth', 2);
hold on;

for j = 1:nParams
    plot(t, aHat(j,:), 'LineWidth', 1.3);
end

grid on;
xlabel('Zeit t [s]');
ylabel('Beschleunigung a [m/s^2]');
title('Alpha-Beta-Gamma-Filter: Beschleunigungsschaetzung');
legend('Wahre Beschleunigung', ...
    'alpha=0.4, beta=0.1, gamma=0.02', ...
    'alpha=0.7, beta=0.2, gamma=0.05', ...
    'alpha=0.9, beta=0.4, gamma=0.15', ...
    'Location', 'best');

%% Vergleich anhand des RMSE
Ergebnis = table(params(:,1), params(:,2), params(:,3), rmse, ...
    'VariableNames', {'Alpha', 'Beta', 'Gamma', ...
    'RMSE_Position_m'});

disp(Ergebnis);

Parametrierung

Die Parameter $α$ , $β$ und $γ$ bestimmen die Korrektur der geschätzten Zustandsgrößen anhand der Positionsinnovation.

Parameterkombination	Erwartete Tendenz	Mögliche Auswirkung
$α = 0, 4, β = 0, 1, γ = 0, 02$	Vorsichtige Korrektur	Ruhigere Schätzung, möglicherweise langsamere Anpassung an Änderungen der Bewegung
$α = 0, 7, β = 0, 2, γ = 0, 05$	Mittlere Korrektur	Kompromiss zwischen Glättung und dynamischer Nachführung
$α = 0, 9, β = 0, 4, γ = 0, 15$	Starke Korrektur	Schnellere Anpassung, potenziell stärkere Reaktion auf Messrauschen

Die drei Parameter wirken gemeinsam. Insbesondere beeinflusst $γ$ die Beschleunigungsschätzung über den Faktor $γ / (2 T_{s}^{2})$ . Bei kleinen Abtastzeiten kann dieser Faktor groß werden. Deshalb ist die Parametrierung immer im Zusammenhang mit der Abtastzeit und dem Messrauschen zu untersuchen.

Die angegebenen Kombinationen sind Versuchsparameter und keine allgemeingültigen Empfehlungen.

Auswertung

Der Root Mean Square Error (RMSE) der Positionsschätzung wird berechnet durch

${R M S E}_{x} = \sqrt{\frac{1}{N} \sum_{k = 1}^{N} {(x (k) - \hat{x} (k))}^{2}} .$

Ein kleinerer RMSE bedeutet für den untersuchten Simulationslauf einen geringeren mittleren quadratischen Positionsfehler. Er bewertet jedoch nicht allein die Qualität der Geschwindigkeits- und Beschleunigungsschätzung. Daher sind zusätzlich die drei Zustandsverläufe zu untersuchen.

Da die Simulation mit konstanter Beschleunigung erfolgt, entspricht das Bewegungsmodell dem verwendeten Prädiktionsmodell. Abweichungen entstehen hier vor allem durch das Messrauschen und die anfängliche Einschwingphase. Bei realen Robotern können zusätzlich Modellfehler durch wechselnde Beschleunigung, Schlupf oder ungenaue Positionsmessungen auftreten.

Erweiterungsaufgabe

Erweitern Sie die Simulation so, dass der Roboter ab $t = 10 s$ eine andere Beschleunigung aufweist. Untersuchen Sie, wie schnell das Alpha-Beta-Gamma-Filter die Änderung erkennt und wie sich die Positions-, Geschwindigkeits- und Beschleunigungsfehler verändern.

Vergleichen Sie die Ergebnisse mit dem Alpha-Beta-Filter. Diskutieren Sie, in welchen Situationen die zusätzliche Schätzung der Beschleunigung für die Navigation eines autonomen mobilen Roboters vorteilhaft ist.

→ zurück zum Hauptartikel: Alpha-Beta-Gamma-Filter

MATLAB-Beispiel Alpha-Beta-Gamma-Filter

Inhaltsverzeichnis

Aufgabenstellung

Lösung

Parametrierung

Auswertung

Erweiterungsaufgabe

Navigationsmenü

MATLAB-Beispiel Alpha-Beta-Gamma-Filter

Aufgabenstellung

Lösung

Parametrierung

Auswertung

Erweiterungsaufgabe

Navigationsmenü

Suche