Sensorfusion mit Kalman

Matthias Gast #3225272
Lesenswert?
• ▲
▼ 
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
Antwort Zitieren Bearbeiten Thread verschieben Thread sperren Anmeldepflicht aktivieren Anpinnen Thread löschen Thread mit anderem zusammenführen Beitrag melden
Matthias Gast #3225477
Lesenswert?
• ▲
▼ 
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?
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Detlef _. #3225578
Lesenswert?
• ▲
▼ 
>>>>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
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Matthias Gast #3229013
Lesenswert?
• ▲
▼ 
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
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Matthias Gast #3233439
Lesenswert?
• ▲
▼ 
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.

1
% Test für das simpelste SS-Model Acc und einem hypothetischen GPS (v,s)

2
% mit Bias

3
clc;

4
clear all;

5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

6
% allgemeine Parameter

7
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

8
dt = 0.01;   % time step

9
pic = 1;

10
end_of_time = 1000 - dt; % time vector length 

11
hv = size(0:dt:end_of_time);

12
x_est_out = zeros(4,hv(2)); % Ausgabe-vektor der estimated Zustände

13


14
sigma_a = 3.6;

15
sigma_v = 10.1;

16
sigma_s = 10.5;

17
sigma_ba = 500;

18


19
var_a   = sigma_a*sigma_a;

20
var_v   = sigma_v*sigma_v;

21
var_s   = sigma_s*sigma_s;

22
var_ba  = sigma_ba*sigma_ba;

23


24
bias_s = 0.0; 

25
bias_v = 0.0; 

26
bias_a = -0.5;

27
alpha  = 2000;

28


29
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

30
% Kalman-Modell (diskret)

31
%

32
%  | s_k+1  |   | 1  dt  dt²/2  -dt²/2 | | s_k  |   

33
%  |        |   |                      | |      |   

34
%  | v_k+1  |   | 0  1     dt     -dt  | | v_k  |   

35
%  |        | = |                      |*|      | + noise

36
%  | a_k+1  |   | 0  0      1      0   | | a_k  |   

37
%  |        |   |                      | |      |  

38
%  | ba_k+1 |   | 0  0      0      1   | | ba_k |   

39
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

40
A = [1 dt dt*dt*0.5 -dt*dt*0.5; 0 1 dt -dt; 0 0 1 0; 0 0 0 1];

41
B = [0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0];

42
C = [0 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 0];   % ba eigentlich nicht direkt messbar

43
%gensys = ss(A,B,C,0,dt) %discrete

44
gensys = ss(A,B,C,0)

45


46
% Prüfung Beobachtbarkeit

47
Ob = obsv(gensys);

48
unob = length(A)-rank(Ob) % The model is observable if Ob has full rank n.

49
% Prüfung Steuerbarkeit

50
Co = ctrb(gensys);

51
unco = length(A)-rank(Ob)

52
% figure(1);

53
% step(gensys);

54


55
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

56
% erzeuge Zeit- und Eingangsvektoren mittels idealem Modell

57
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

58
time = 0:dt:end_of_time;

59
tmp  = size(time);

60
s_gps = zeros(1,tmp(2));

61
v_gps = zeros(1,tmp(2));

62
a = zeros(1,tmp(2));

63
n = 1;

64


65
for i = 0:dt:end_of_time

66
    if i < 4

67
        a(n) = 0;

68
    elseif i < 20

69
        a(n) = 0.2;

70
    elseif i < 40

71
        a(n) = 0;

72
    elseif i < 50

73
        a(n) = -0.34;

74
    elseif i < 200 

75
        a(n) = 0.02;

76
    elseif i < 500

77
        a(n) = -0.015;

78
    elseif i < 600

79
        a(n) = 0.02;

80
    else  

81
        a(n) = -0.001;

82
    end   

83
    

84
    n = n + 1;

85
end

86


87
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

88
% perfekte Simulation

89
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

90
u_sim = a; % input für perfekte Sim

91
sA = [0 1; 0 0];

92
sB = [0 1]';

93
sC = [1 0 ; 0 1];

94
%sGensys = ss(sA,sB,sC,0,dt) %discrete

95
sGensys = ss(sA,sB,sC,0);

96
%figure(pic);

97
%pic = pic+1;

98
%lsim(sGensys,u_sim,time);

99
[orgin_y,time] = lsim(sGensys,u_sim,time);

100


101
figure(pic);

102
pic = pic+1;

103
plot(time,u_sim,'y');

104
title('Perfekte Simulation');

105
grid on;

106
hold on;

107
plot(time,orgin_y(1:hv(2)),'b');

108
plot(time,orgin_y(hv(2)+1:2*hv(2)),'r');

109
hold off;

110


111
s_gps = orgin_y(1:hv(2));   % Zuweisung der perfekten Daten

112
v_gps = orgin_y(hv(2)+1:2*hv(2));

113
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

114
% Verrauschen der Daten

115
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

116
noise_a = sigma_a.*randn(hv(2),1);

117
noise_v = sigma_v.*randn(hv(2),1);

118
noise_s = sigma_s.*randn(hv(2),1);

119
noisy_a = zeros(1,hv(2));

120
noisy_v = zeros(1,hv(2));

121
noisy_s = zeros(1,hv(2));

122


123
n=1;

124
for i = 1:dt:end_of_time

125
    noisy_a(n) = a(n) + noise_a(n);

126
    noisy_v(n) = v_gps(n) + noise_v(n);

127
    noisy_s(n) = s_gps(n) + noise_s(n);

128
    n=n+1;

129
end

130


131
noisy_meas = [noisy_s;noisy_v;noisy_a];

132


133
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

134
% verrauschte Simulation: a = input

135
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

136
[sim_noisy_y,time] = lsim(sGensys,noisy_a,time);

137


138
figure(pic);

139
pic = pic+1;

140
plot(time,noisy_a,'y');

141
title('Verrauschte Simulation');

142
grid on;

143
hold on;

144
plot(time,sim_noisy_y(1:hv(2)),'b');

145
plot(time,sim_noisy_y(hv(2)+1:2*hv(2)),'r');

146
hold off;

147


148
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

149
% Bias zur Messung

150
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

151
noisy_bias_s   = noisy_s + bias_s*ones(1,tmp(2));

152
noisy_bias_v   = noisy_v + bias_v*ones(1,tmp(2));

153
noisy_bias_a   = noisy_a + bias_a*ones(1,tmp(2));

154
noisy_biased_meas = [noisy_bias_s; noisy_bias_v; noisy_bias_a]';

155


156


157
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

158
% Kalmanformeln

159
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

160
I = eye(4);

161
P = alpha*I;

162
P_pred = P;

163
x_pred = ones(4,1);

164


165
% Q = E{e_wk e_wk^T}

166
q = [dt*dt/2 0 0 0; 0 dt 0 0; 0 0 1 0; 0 0 0 0]; 

167
Q = (q*(var_a)*q');

168
% R = E{e_vk e_vk^T}

169
R = [var_s 0 0 0; 0 var_v 0 0; 0 0 var_a 0; 0 0 0 var_ba];

170


171
% spaltenweise Nummerierung der Matrixinhalte. fortlaufend. 

172
% zB 2.Spalte von y

173
% yy = y(n*3-2:n*3)' 

174


175
x_pred_buf_s  = zeros(1,hv(2));

176
x_pred_buf_v  = zeros(1,hv(2));

177
x_pred_buf_a  = zeros(1,hv(2));

178
x_pred_buf_ba = zeros(1,hv(2));

179
n = 1;

180
for i = 0:dt:end_of_time

181
    

182
    % verschiedenen Messungen aktiv. C-Matrix auch anpassen dann...

183
    %measurement = [noisy_biased_meas(n) noisy_biased_meas(n+hv(2)) noisy_biased_meas(n+2*hv(2)) 0]';

184
    %measurement = [noisy_biased_meas(n) 0 noisy_biased_meas(n+2*hv(2)) 0]';

185
    measurement = [0 noisy_biased_meas(n+hv(2)) noisy_biased_meas(n+2*hv(2)) 0]';

186
    

187
    K = (P_pred*C')/(C*P_pred*C' + R);              % Kalman Gain

188
    x_est = x_pred + K*(measurement - C*x_pred);    % Zustandsschätzung

189
    P = (I - K*C)*P_pred;                           % neue Kovarianzmatrix  

190
    x_pred = A*x_est;                               % Prädiktion    

191
    P_pred = A*P*A' + Q;

192
    

193
    x_pred_buf_s(n)  = x_pred(1);

194
    x_pred_buf_v(n)  = x_pred(2);

195
    x_pred_buf_a(n)  = x_pred(3);

196
    x_pred_buf_ba(n) = x_pred(4);

197
    

198
    x_est_out(4*n-3:4*n) = x_est;   % schreibe in Ausgabevektor 3*length(hv(2)) 

199
    n = n + 1;

200
end

201


202
figure(pic);

203
pic = pic+1;

204
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');

205
title('X_P_R_E_D');

206
grid on;

207


208
% gemeinsamer Vergleich

209
figure(pic);

210
pic = pic+1;

211
plot(time,x_est_out,'b',time,orgin_y,'y',time,sim_noisy_y,'r');

212
title('Vergleich: geschätzte(b) vs orginale Zustände(y) vs noisy(r)');

213
grid on;

214


215
% getrennte Ausgabe der Zustandsvariablen

216
x_est_help = x_est_out';

217
figure(pic);

218
pic = pic+1;

219
plot(time,noisy_bias_s(1:hv(2)),'r',time,x_est_help(1:hv(2)),'b',time,orgin_y(1:hv(2)),'y');

220
title('Vergleich: geschätzte(b) vs orginale(y) vs rauschende(r) Strecke');

221
grid on;

222


223
figure(pic);

224
pic = pic+1;

225
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');

226
title('Vergleich: geschätzte(b) vs orginale(y) vs rauschende(r) Geschwindigkeit');

227
grid on;

228


229
figure(pic);

230
pic = pic+1;

231
%plot(time,x_est_help(2*hv(2)+1:3*hv(2)),'b',time,a,'y');

232
plot(time,noisy_bias_a,'r',time,x_est_help(2*hv(2)+1:3*hv(2)),'b',time,a,'y');

233
title('Vergleich: geschätzte(b) vs orginale(y) vs rauschende(r) Beschleunigung');

234
grid on;

235


236
x_pred_buf_ba(hv(2))

237
% EOF


LG, Matthias
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Pfannenfranz Gast #3272088
Lesenswert?
• ▲
▼ 
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.
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Thread beobachten

Antwort schreiben

Bitte melde dich an, um einen Beitrag zu schreiben.

Bestehender Account

oder

Mit Google-Account einloggen

Noch kein Account?

Die Registrierung ist kostenlos und dauert nur eine Minute.

Jetzt registrieren