Sensorfusion mit Kalman

[Matthias (Gast)]

Hallo.

Wie macht man die Sensorfusion mit einem Kalman richtig?
Wenn ich zB zu Beginn ein einfaches lineares System habe und nehmen wir 
an eine eindimensionale Bewegung, 3 Sensorquellen bestehend aus 
Streckenmessung, Geschwindigkeit und Beschleunigung (alles generiert, 
gleiche Datenrate...), die halt unterschiedlich verrauscht sind. Wie 
genau sieht dann das Modell aus? Ich verstehe nicht ganz wo, wie und 
wann die einzelnen Quellen verschmolzen werden?

Mich verwirrt, dass Inputs und Zustandvektor sich überschneiden. Damit 
meine ich zB Input Beschleunigung wird integriert und dann habe ich bspw 
eine Geschwindigkeit vom GPS. Ich kann die Geschwindigkeit aber nicht 
als Input nehmen, weil ich ja im Zustandsvektor auch die Geschwindigkeit 
habe.

(ds dv) = A*(s v) + B*(a)

Kann mir jemand einen Tipp geben, wie ich das Modell für eine Fusion 
verwenden kann? Ich finde nirgends ein einfaches Bsp, das mir das klar 
macht.

LG, Matthias

[Matthias (Gast)]

...transponiert-Zeichen vergessen...

(ds dv)' = A*(s v)' + B*(a)

[Detlef _. (detlef_a)]

Du willst ein System zweiter Ordnung schätzen: Beschleunigung integriert 
zu Geschwindigkeit integriert zu Strecke?
Dann sind doch deine Beschleunigungs-,Geschwindigkeits- und 
Streckensensoren (Meß-)Ausgänge und keine Inputs?

Oder wie ist das gemeint?

Cheers
Detlef

[Jan S. (jannemann)]

Im Buch von Dan Simon - Kalman Filtering Embedded Systems Programming 
S.72-79. gehts wenn ich mich nicht irre um die Modellierung eines Autos.
Zumindest hatten wir eine Vorlesung in der ein Automodelliert wurde. Das 
Beispiel stammte aus dem Buch.
http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf

[wieOskar (Gast)]

Vielleicht hilft das:
 http://www.ai.mit.edu/~murphyk/Software/kalman.html

[Matthias (Gast)]

Danke für die Links. Die kannte ich eh auch schon.

@Detlef:
Genau das stört mich und versteh ich nicht irgendwie, dass zB die 
Beschleunigung ein Input für mein System ist. Dann steht im geschätzten 
Zustandsvektor die Geschwindigkeit, die durch Integration der 
Beschleunigung erhalten wird und die Strecke durch Integration der 
geschätzten Geschwindigkeit. Messwerte verwende ich dann (theoretisch) 
Geschwindigkeit und Position von GPS. Wo passiert die Fusion? Wenn man 
einfach aufintegriert driftet man auch irgendwohin. Genau das soll ja 
durch die Fusion verhindert werden.

In den ganzen Beispielen wie auch zB in dem einen Link 
(http://www.cs.ubc.ca/~murphyk/Software/Kalman/kalman.html) gibts gar 
keine Inputs. Heißt das, dass ich jeden "input" als Rauschen betrachte 
und dann zB folgenden Zustandsvektor hätte (s v a)' ? Da fehlt mir auch 
die Fusion. Passiert diese dann automatisch in der Schätzung durch die 
Kalman-Gewichtung zwischen Mess- und Modellwert? Habe ich zuviel Magic 
vom Kalman erwartet?

Oder muss man da wesentlich mehr machen, also wirklich jeden Sensor mit 
anderen korrigieren damit was brauchbares herauskommt und nichts 
driftet?

[Detlef _. (detlef_a)]

>>>>In den ganzen Beispielen wie auch zB in dem einen Link
(http://www.cs.ubc.ca/~murphyk/Software/Kalman/kalman.html) gibts gar
keine Inputs.

Doch, der Wikipedia Artikel betrachtet inputs: x(k+1)=F*x+B*u+w
Die lassen die inputs weg, weil sich mit inputs nix ändert ausser dem 
Prädiktionsschritt.

>>>> Heißt das, dass ich jeden "input" als Rauschen betrachte
und dann zB folgenden Zustandsvektor hätte (s v a)' ?

Ja, so ist es. Deine Zustandgröße a hat Rauschen als input, v und s 
nicht mehr.

>>> Messwerte verwende ich dann (theoretisch) Geschwindigkeit und Position von 
GPS.

Das ist redundant, das GPS berechnet v auch nur als Änderung von s. Nimm 
einfach s als Messwert für den Schätzer.

>>Habe ich zuviel Magic vom Kalman erwartet?
Naja, steht auf besseren Füßen als eine Zigeunerin mit Kristallkugel. 
Das klappt ganz gut wenn man weiß was man tut und/oder bißchen probiert. 
Wichtig ist das Spielen mit dem Meß- und Eingangsrauschen. Kleines 
Meßrauschen oder kleines Eingagsrauschen bewertet die Meßwerte hoch, 
ansonsten 'verläßt' sich Kalman mit der Prädiktion auf sein eigenes 
Modell und macht dicht für die Meßwerte. Ich habe gute Erfahrungen mit 
'Kalman'-Filtern gemacht, die mit konstanten Verstärkungen arbeiteten, 
das ganze war also ein Beobachter. Die konstanten Verstärkungen hatte 
ich offline mit mitgeschnittenen Meßwerten und angenommen Eingangs- und 
Meßrauschen ermittelt.

Matlab Code für Kalman gibts hier:
Beitrag "Amplitude und Phase einer festen Frequenz bestimmen."
Für einen zusätzlichen Eingagsvektor u ändert sich nur
xds=A*xd;
zu
xds=A*xd+B*u;

HTH
Cheers
Detlef

[Matthias (Gast)]

Ok, danke für die Antworten!

Ich hab das jetzt mal so aufgebaut. also mit (s v a)' als 
Zustandsvektor. Schaut noch nicht so toll aus: "Schwingt" bzw erzeugt 
Dreiecksmuster. Vermutlich ist noch etwas falsch eingestellt. Sollte ich 
etwas brauchbares zusammenbringen und mich besser auskennen, werde ich 
es posten...

LG, Matthias

[Matthias (Gast)]

Hallo.

Ich denke soweit passt die Fusion im einfachen Beispiel einmal. Eh 
einfach, LOL...

Vll sind die Rauschmatrizen noch nicht 100% richtig, aber das Ergebnis 
schaut recht gut aus. Ich habe verschiedene Messungen und Schätzungen 
mit verschiedenen Rauschstärken getestet. Man muss dazu die C-Matrix und 
die Zuweisung des Vektors "measurement" anpassen.

% Test für das simpelste SS-Model Acc und einem hypothetischen GPS (v,s)
% mit Bias
clc;
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% allgemeine Parameter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 0.01;   % time step
pic = 1;
end_of_time = 1000 - dt; % time vector length 
hv = size(0:dt:end_of_time);
x_est_out = zeros(4,hv(2)); % Ausgabe-vektor der estimated Zustände

sigma_a = 3.6;
sigma_v = 10.1;
sigma_s = 10.5;
sigma_ba = 500;

var_a   = sigma_a*sigma_a;
var_v   = sigma_v*sigma_v;
var_s   = sigma_s*sigma_s;
var_ba  = sigma_ba*sigma_ba;

bias_s = 0.0; 
bias_v = 0.0; 
bias_a = -0.5;
alpha  = 2000;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kalman-Modell (diskret)
%
%  | s_k+1  |   | 1  dt  dt²/2  -dt²/2 | | s_k  |   
%  |        |   |                      | |      |   
%  | v_k+1  |   | 0  1     dt     -dt  | | v_k  |   
%  |        | = |                      |*|      | + noise
%  | a_k+1  |   | 0  0      1      0   | | a_k  |   
%  |        |   |                      | |      |  
%  | ba_k+1 |   | 0  0      0      1   | | ba_k |   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A = [1 dt dt*dt*0.5 -dt*dt*0.5; 0 1 dt -dt; 0 0 1 0; 0 0 0 1];
B = [0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0];
C = [0 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 0];   % ba eigentlich nicht direkt messbar
%gensys = ss(A,B,C,0,dt) %discrete
gensys = ss(A,B,C,0)

% Prüfung Beobachtbarkeit
Ob = obsv(gensys);
unob = length(A)-rank(Ob) % The model is observable if Ob has full rank n.
% Prüfung Steuerbarkeit
Co = ctrb(gensys);
unco = length(A)-rank(Ob)
% figure(1);
% step(gensys);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% erzeuge Zeit- und Eingangsvektoren mittels idealem Modell
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
time = 0:dt:end_of_time;
tmp  = size(time);
s_gps = zeros(1,tmp(2));
v_gps = zeros(1,tmp(2));
a = zeros(1,tmp(2));
n = 1;

for i = 0:dt:end_of_time
    if i < 4
        a(n) = 0;
    elseif i < 20
        a(n) = 0.2;
    elseif i < 40
        a(n) = 0;
    elseif i < 50
        a(n) = -0.34;
    elseif i < 200 
        a(n) = 0.02;
    elseif i < 500
        a(n) = -0.015;
    elseif i < 600
        a(n) = 0.02;
    else  
        a(n) = -0.001;
    end   
    
    n = n + 1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% perfekte Simulation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
u_sim = a; % input für perfekte Sim
sA = [0 1; 0 0];
sB = [0 1]';
sC = [1 0 ; 0 1];
%sGensys = ss(sA,sB,sC,0,dt) %discrete
sGensys = ss(sA,sB,sC,0);
%figure(pic);
%pic = pic+1;
%lsim(sGensys,u_sim,time);
[orgin_y,time] = lsim(sGensys,u_sim,time);

figure(pic);
pic = pic+1;
plot(time,u_sim,'y');
title('Perfekte Simulation');
grid on;
hold on;
plot(time,orgin_y(1:hv(2)),'b');
plot(time,orgin_y(hv(2)+1:2*hv(2)),'r');
hold off;

s_gps = orgin_y(1:hv(2));   % Zuweisung der perfekten Daten
v_gps = orgin_y(hv(2)+1:2*hv(2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Verrauschen der Daten
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
noise_a = sigma_a.*randn(hv(2),1);
noise_v = sigma_v.*randn(hv(2),1);
noise_s = sigma_s.*randn(hv(2),1);
noisy_a = zeros(1,hv(2));
noisy_v = zeros(1,hv(2));
noisy_s = zeros(1,hv(2));

n=1;
for i = 1:dt:end_of_time
    noisy_a(n) = a(n) + noise_a(n);
    noisy_v(n) = v_gps(n) + noise_v(n);
    noisy_s(n) = s_gps(n) + noise_s(n);
    n=n+1;
end

noisy_meas = [noisy_s;noisy_v;noisy_a];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% verrauschte Simulation: a = input
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[sim_noisy_y,time] = lsim(sGensys,noisy_a,time);

figure(pic);
pic = pic+1;
plot(time,noisy_a,'y');
title('Verrauschte Simulation');
grid on;
hold on;
plot(time,sim_noisy_y(1:hv(2)),'b');
plot(time,sim_noisy_y(hv(2)+1:2*hv(2)),'r');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Bias zur Messung
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
noisy_bias_s   = noisy_s + bias_s*ones(1,tmp(2));
noisy_bias_v   = noisy_v + bias_v*ones(1,tmp(2));
noisy_bias_a   = noisy_a + bias_a*ones(1,tmp(2));
noisy_biased_meas = [noisy_bias_s; noisy_bias_v; noisy_bias_a]';


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kalmanformeln
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
I = eye(4);
P = alpha*I;
P_pred = P;
x_pred = ones(4,1);

% Q = E{e_wk e_wk^T}
q = [dt*dt/2 0 0 0; 0 dt 0 0; 0 0 1 0; 0 0 0 0]; 
Q = (q*(var_a)*q');
% R = E{e_vk e_vk^T}
R = [var_s 0 0 0; 0 var_v 0 0; 0 0 var_a 0; 0 0 0 var_ba];

% spaltenweise Nummerierung der Matrixinhalte. fortlaufend. 
% zB 2.Spalte von y
% yy = y(n*3-2:n*3)' 

x_pred_buf_s  = zeros(1,hv(2));
x_pred_buf_v  = zeros(1,hv(2));
x_pred_buf_a  = zeros(1,hv(2));
x_pred_buf_ba = zeros(1,hv(2));
n = 1;
for i = 0:dt:end_of_time
    
    % verschiedenen Messungen aktiv. C-Matrix auch anpassen dann...
    %measurement = [noisy_biased_meas(n) noisy_biased_meas(n+hv(2)) noisy_biased_meas(n+2*hv(2)) 0]';
    %measurement = [noisy_biased_meas(n) 0 noisy_biased_meas(n+2*hv(2)) 0]';
    measurement = [0 noisy_biased_meas(n+hv(2)) noisy_biased_meas(n+2*hv(2)) 0]';
    
    K = (P_pred*C')/(C*P_pred*C' + R);              % Kalman Gain
    x_est = x_pred + K*(measurement - C*x_pred);    % Zustandsschätzung
    P = (I - K*C)*P_pred;                           % neue Kovarianzmatrix  
    x_pred = A*x_est;                               % Prädiktion    
    P_pred = A*P*A' + Q;
    
    x_pred_buf_s(n)  = x_pred(1);
    x_pred_buf_v(n)  = x_pred(2);
    x_pred_buf_a(n)  = x_pred(3);
    x_pred_buf_ba(n) = x_pred(4);
    
    x_est_out(4*n-3:4*n) = x_est;   % schreibe in Ausgabevektor 3*length(hv(2)) 
    n = n + 1;
end

figure(pic);
pic = pic+1;
plot(time,x_pred_buf_v,'y',time,x_pred_buf_s,'b',time,x_pred_buf_a,'r',time,x_pred_buf_ba,'k');
title('X_P_R_E_D');
grid on;

% gemeinsamer Vergleich
figure(pic);
pic = pic+1;
plot(time,x_est_out,'b',time,orgin_y,'y',time,sim_noisy_y,'r');
title('Vergleich: geschätzte(b) vs orginale Zustände(y) vs noisy(r)');
grid on;

% getrennte Ausgabe der Zustandsvariablen
x_est_help = x_est_out';
figure(pic);
pic = pic+1;
plot(time,noisy_bias_s(1:hv(2)),'r',time,x_est_help(1:hv(2)),'b',time,orgin_y(1:hv(2)),'y');
title('Vergleich: geschätzte(b) vs orginale(y) vs rauschende(r) Strecke');
grid on;

figure(pic);
pic = pic+1;
plot(time,x_est_help(hv(2)+1:2*hv(2)),'b',time,orgin_y(hv(2)+1:2*hv(2)),'y',time,sim_noisy_y(hv(2)+1:2*hv(2)),'r');
title('Vergleich: geschätzte(b) vs orginale(y) vs rauschende(r) Geschwindigkeit');
grid on;

figure(pic);
pic = pic+1;
%plot(time,x_est_help(2*hv(2)+1:3*hv(2)),'b',time,a,'y');
plot(time,noisy_bias_a,'r',time,x_est_help(2*hv(2)+1:3*hv(2)),'b',time,a,'y');
title('Vergleich: geschätzte(b) vs orginale(y) vs rauschende(r) Beschleunigung');
grid on;

x_pred_buf_ba(hv(2))
% EOF

LG, Matthias

[Pfannenfranz (Gast)]

Matthias schrieb:
> Mich verwirrt, dass Inputs und Zustandvektor sich überschneiden. Damit
> meine ich zB Input Beschleunigung wird integriert und dann habe ich bspw
> eine Geschwindigkeit vom GPS. Ich kann die Geschwindigkeit aber nicht
> als Input nehmen, weil ich ja im Zustandsvektor auch die Geschwindigkeit
> habe.

Etwas spät, aber vielleicht hilft's ja noch:
1. Unterscheiden zwischen verschiedenen "Inputs": üblicherweise "u" wird 
der Steuereingang benannt, "z" Messungen. "u" ist oft Null.

2. Eine Messgröße (z.B. Geschwindigkeit) kann es auch als Systemzustand 
geben. Die Messmatrix, meist "H" gibt an wie die Messungen sich auf den 
Systemzustand auswirken.

3. Sensorfusion findet "ganz automatisch" statt, wenn man die Messungen 
der verschiedenen Sensoren parallel anwendet. Beispiel: ein Sensor misst 
die Geschwindigkeit, einer eine Beschleunigung, welche noch dazu einen 
konstanten Offset hat. Dafür ist beim Geschwindigkeits-Sensor die 
Geschwindigkeit sehr verrauscht. Wenn man nun diese beiden Messungen für 
das System verwendet berechnet der Kalmanfilter den Offset des 
Beschleunigungssensor, und man erhält dauerhaft präzisere 
Geschwindigkeitsmessungen als nur mit einem Sensor.