MATLAB-Beispiel Alpha-Beta-Filter

Dozent:	Prof. Dr.-Ing. Schneider
Modul	Multisensorsysteme (Wahlpflichtprofil „Systems Design Engineering“), Wintersemester
Modulbezeichnung:	MTR-B-2-7.10
Modulverantwortung:	Ulrich Schneider
Lehrveranstaltung:	Praktikum Multisensorsysteme
Lektion:	5 - Alpha-Beta-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-Filter sollen die Position und die Geschwindigkeit des Roboters geschätzt werden.

Gegeben sind:

• Abtastzeit: $T_s=0,1\mathrm{s}$
• Wahre Geschwindigkeit: $v=0,8\mathrm{m}/\mathrm{s}$
• Standardabweichung des Messrauschens: ${\sigma }_z=0,15\mathrm{m}$
• Anfangsposition: $x_0=0\mathrm{m}$

Bearbeiten Sie folgende Aufgaben:

1. Erzeugen Sie simulierte Positionsmessungen mit additivem, normalverteiltem Messrauschen.
2. Implementieren Sie die Prädiktion und Korrektur des Alpha-Beta-Filters.
3. Stellen Sie die wahre Position, die Messwerte und die geschätzte Position grafisch dar.
4. Untersuchen Sie die Parameterkombinationen $(\alpha ,\beta )=(0,2;0,05)$, $(0,6;0,2)$ und $(0,9;0,5)$.
5. Berechnen Sie den RMSE der Positionsschätzung und diskutieren Sie den Einfluss der Parameter.

Lösung

startAlphaBetaFilter.m 

clear; close all; clc;

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

dt = 0.1;               % Abtastzeit in s
t = 0:dt:20;            % Zeitvektor in s
N = numel(t);           % Anzahl Elemente

vTrue = 0.8;            % Wahre Geschwindigkeit in m/s
sigmaZ = 0.15;          % Standardabweichung in m

xTrue = vTrue * t;      % Wahre Position in m
z = xTrue + sigmaZ * randn(size(t)); % Verrauschte Messung in m

%% Parameterkombinationen [alpha, beta]
params = [
    0.2  0.05
    0.6  0.20
    0.9  0.50];

nParams = size(params, 1);

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



%% Alpha-Beta-Filter
for j = 1:nParams
    AlphaBetaFilter(); % (Re-)Init
    for k=1:N
        alpha = params(j, 1);
        beta  = params(j, 2);

        xEst = AlphaBetaFilter(z(k),dt,alpha,beta);

        xHat(j,k) = xEst(1);
        vHat(j,k) = xEst(2);

        % RMSE der Positionsschaetzung
        rmse(j) = sqrt(mean((xTrue - xHat(j,:)).^2));
    end
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 in s');
ylabel('Position x in m');
title('Alpha-Beta-Filter: Positionsschaetzung');
legend('Wahre Position', 'Messung', ...
    'alpha=0.2, beta=0.05', ...
    'alpha=0.6, beta=0.2', ...
    'alpha=0.9, beta=0.5', ...
    'Location', 'northwest');
print(gcf, '-djpeg', 'DemoAlphaBetaFilter_Pos');

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

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

grid on;
xlabel('Zeit t in s');
ylabel('Geschwindigkeit v in m/s');
title('Alpha-Beta-Filter: Geschwindigkeitsschaetzung');
legend('Wahre Geschwindigkeit', ...
    'alpha=0.2, beta=0.05', ...
    'alpha=0.6, beta=0.2', ...
    'alpha=0.9, beta=0.5', ...
    'Location', 'best');
print(gcf, '-djpeg', 'DemoAlphaBetaFilter_Vel');

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

disp(Ergebnis);

AlphaBetaFilter.m 

%-----------------------------------------------------------------------
function xEst = AlphaBetaFilter(z,dt,alpha,beta)
%-----------------------------------------------------------------------
persistent x v 
if nargin < 2 % Persistente Variablen löschen
    % Reset persistent variable
    x = [];
    disp('Reset Kalman-Filter')
else


    if isempty(x)
        % Nach Möglichkeit Vorwissen einbauen
        x = z; % Position in m
        v = 0; % Geschwindigkeit in m/s
        disp('Init AlphaBetaFilter')
    else

    % Prädiktion
    xp= x+dt*v; % Position in m
    vp= v;      % Geschwindigkeit in m/s - Konstante Geschwindigkeit

    % Residuum/Innovation
    r = z-xp;   % in m

    % Schätzung
    x = xp+alpha*r;   % Position in m
    v = vp+beta*r/dt; % Geschwindigkeit in m/s
    end
    % Rückgabe
    xEst = [x;v];   % Zustandsvektor
end
end

Parametrierung

Die Parameter $\alpha$ und $\beta$ bestimmen die Korrektur der geschätzten Zustandsgrößen anhand der Messinnovation.

Parameterkombination	Erwartete Tendenz	Mögliche Auswirkung
$\alpha =0,2,\beta =0,05$	Vorsichtige Korrektur	Stärkere Glättung, möglicherweise langsamere Anpassung an Bewegungsänderungen
$\alpha =0,6,\beta =0,2$	Mittlere Korrektur	Kompromiss zwischen Glättung und dynamischer Nachführung
$\alpha =0,9,\beta =0,5$	Starke Korrektur	Schnellere Anpassung, möglicherweise stärkere Übernahme des Messrauschens

Die beschriebenen Wirkungen sind qualitative Erwartungen. Das tatsächliche Verhalten hängt von der Parameterkombination, der Abtastzeit und dem Bewegungsprofil ab.

Auswertung

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

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

Ein kleinerer RMSE bedeutet für die untersuchten Messdaten einen geringeren mittleren quadratischen Positionsfehler. Er bewertet jedoch nicht allein das Einschwingverhalten oder die Qualität der Geschwindigkeitsschätzung. Daher sind zusätzlich die dargestellten Positions- und Geschwindigkeitsverläufe zu untersuchen.

Parameterkombination	RMSE in m	Interpretation
$\alpha =0,2,\beta =0,05$	0.080615	Die Positionsschätzung wird pro Messung nur wenig korrigiert. Das Filter kann Messrauschen glätten, reagiert aber langsamer auf tatsächliche Änderungen der Bewegung.
$\alpha =0,6,\beta =0,2$	0.10366	Die Messwerte werden stärker berücksichtigt. Diese Kombination stellt einen Vergleich zwischen Glättung und Anpassungsgeschwindigkeit dar.
$\alpha =0,9,\beta =0,5$	0.13506	Die Position wird stark an die aktuelle Messung angepasst. Dadurch kann die Schätzung dem Messsignal eng folgen, übernimmt aber tendenziell mehr Messrauschen.

Die Parameterkombinationen sind Versuchsparameter und keine allgemeingültigen Empfehlungen. Die geeignete Einstellung ist anhand repräsentativer Messdaten zu bestimmen.

Abb. 01: Alpha-Beta-Filter: Positionsschätzung

Abb. 02: Alpha-Beta-Filter: Geschwindigkeitsschaetzung

Erweiterungsaufgabe

Erweitern Sie die Simulation so, dass der Roboter ab $t=10\mathrm{s}$ beschleunigt. Untersuchen Sie, wie schnell das Alpha-Beta-Filter auf die Änderung reagiert und wie sich der Positionsfehler verändert.

Diskutieren Sie, weshalb ein Alpha-Beta-Filter mit einem Modell konstanter Geschwindigkeit bei beschleunigter Bewegung einen Modellfehler aufweist.

---

→ zurück zum Hauptartikel: MTR Multisensorsysteme: Alpha-Beta-Filter