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
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
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
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?
>>>>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
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
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
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.
Bitte melde dich an um einen Beitrag zu schreiben. Anmeldung ist kostenlos und dauert nur eine Minute.
Bestehender Account
Schon ein Account bei Google/GoogleMail? Keine Anmeldung erforderlich!
Mit Google-Account einloggen
Mit Google-Account einloggen
Noch kein Account? Hier anmelden.