Implementierung eines TDOA-Algorithmus - Multilateration

[Adrian B. (adrianbergmann)]

Hallo miteinander!

Ich hoffe, DSP ist das richtige Subforum für mein Anliegen, wenn nicht, 
bitte ich höflichst um Verzeihung und Verschiebung an geeigneten Ort.

Ich plane, einen µC zu programmieren, der die Quelle lauter Geräusche 
lokalisieren soll. Er ist mit 5 Mikrofonen ausgestattet, die sich in 
bekannten Positionen befinden, deren Werte konstant gesampelt und ein 
paar ms lang gespeichert werden.
Wenn eines der Mikrofone einen ausreichend hohen Pegel liefert, schlägt 
ein Komparator um, der einen Interrupt am µC abfeuert. Dieser stoppt 
daraufhin das Sampeln und kreuzkorelliert die fünf gespeicherten 
Mikrofonsequenzen. Daraus erhoffe ich mir, die Differenz der Laufzeiten 
für das laute Schallereignis zu bekommen. Aus diesen lässt sich ja dann 
der Ort des Schallereignisses berechnen.
Dazu wollte ich den hier auf Wikipedia beschriebenen Algorithmus für 
TDOA-Multilateration benutzen: 
https://en.wikipedia.org/wiki/Multilateration

Um schonmal grob abschätzen zu können, wie sich Distanzen der Mikros 
untereinander sowie die Samplerate auf die Genauigkeit auswirken, wollte 
ich den TDOA-Algorithmus erstmal in Matlab ausprobieren. Das Ergebnis 
ist leider derart falsch, dass ich sicher bin, irgendwo einen groben 
Fehler eingebaut zu haben.

Zu dem ganzen habe ich jetzt drei Fragen:
1. Macht mein Plan mit der Kreuzkorellation überhaupt Sinn oder ist da 
ein grundsätzlicher Logikfehler drin? Wenn ich annehme, dass an den 
Mikrofoneingängen sonst nur weisses Rauschen/leises Hintergrundgemurmel 
stattfindet, müsste bei einem deutlich lauteren Schallereignis die KKF 
doch genau die Laufzeitunterschiede dieses Ereignisses liefern.
2. Ich bin nicht sicher, ob ich den Wiki-Artikel überhaupt richtig 
verstanden habe. Im Text liest es sich so, als würden mir vier Mikrofone 
reichen, um das Ereignis in drei Dimensionen eindeutig bestimmen zu 
können.
Aber mit Blick auf die Formel (7) brauche ich doch mindestens 5 
Mikrofone, damit ich auf drei lineare inhomogene Gleichungen komme und 
das ganze eindeutig bestimmt werden kann.
3. Anbei noch mein Matlab-Code. Vielleicht hat jemand die Muße, 
drüberzukucken und erkennt, was mein offensichtlicher Fehler war. Der 
eigentliche Algorithmus kommt erst im letzten Drittel. Da ich noch nicht 
mal den Gauss zum Lösen selber geschrieben habe(*) und die 
Multilateration gerade mal rund 5 Codezeilen beträgt, gehe ich stark von 
einem Verständnisfehler meinerseits aus.
Was davor im Code passiert, ist die Simulation von 
Laufzeitunterschieden, gemessen in Samples. Ergebnisse dafür sehen auf 
den ersten Blick schlüssig aus, habe es der Vollständigkeit aber drin 
gelassen.


Vielen Dank im Voraus!

(*) Zeilenskalierung und Pivotisierung vor linsolve habe ich 
ausprobiert, hat das Ergebnis aber auch nicht nennenswert verbessert.

clear all; close all;

%initialise vectors
runtimect = zeros(1, 5);
runtimedt = zeros(1, 5);
mic = zeros(5, 3);

Solution_Matrix = zeros(3,3);
Solution_Vector = zeros(1, 3);

%x,y,z positions of receivers in m
mic(1,:) = [0 0 0];
mic(2,:) = [1 0 0];
mic(3,:) = [0 1 0];
mic(4,:) = [0 0 1];
mic(5,:) = [1 1 0];

%x, y, z position of pistol in m
pistol = [ 0.5 0.5 0.5];

%x, y, z position of sound emitter in m
emitter = [5 2 0.2];

%sound velocity in m/s, sample rate in samples/s
sound_vel = 343;
samp_rate = 11025;

for i=1:5,
    %calculate distance from emitter to receiver i
    runtimect(i) = norm(mic(i,:) - emitter);

    %translate the distance to samples and truncate values
    runtimedt(i) = floor(runtimect(i)*samp_rate/sound_vel);
end

% Here begins µC multilateration magic

%calculate the TDOA relative to receiver 1 at position 0,0,0... Well,
%technically it is a SDOA - Sample Difference Of Arrival
TDOA = runtimedt - runtimedt(1);

% calculate elements of solution matrix according to Wikipedia
% Multilateration article
for i = 3:5,
    Solution_Matrix(i-2,1) = ( 2 * mic(i,1)/TDOA(i))-(2*mic(2,1)/TDOA(2));
    Solution_Matrix(i-2,2) = ( 2 * mic(i,2)/TDOA(i))-(2*mic(2,2)/TDOA(2));
    Solution_Matrix(i-2,3) = ( 2 * mic(i,3)/TDOA(i))-(2*mic(2,3)/TDOA(2));
    Solution_Vector(i-2) = -(TDOA(i)-TDOA(2)-((norm(mic(i,:))^2)/TDOA(i))+((norm(mic(2,:))^2)/TDOA(2)));
end

X = linsolve(Solution_Matrix, Solution_Vector.');

% original Emitter pos: 5, 2, 0.2
% X pos: ~ 4801, 1535, -175
% not even close...

[Adrian B. (adrianbergmann)]

Manchmal muss man Dinge einfach erstmal laut als Frage an andere 
formulieren, um zu erkennen, dass man ein Depp ist.
Meine Annahme, dass ich einfach statt der Zeitdifferenz die 
Sampledifferenz nehmen kann, da das ja nur ein skalierender Faktor in 
der Lösung wäre, war falsch. Kann man eigentlich auf dem ersten Blick in 
der Zeile sehen, in der Solution_Vector berechnet wird. Keine Ahnung, 
warum ich das nicht mal hinterfragt hab.

Für Ideen zu Frage 1 und 2 wäre ich dennoch dankbar. Ich würde das ganze 
wirklich gerne mit nur vier Mikrofonen realisieren können, da ich 
fürchte, dass ich bei 5 die Samplerate unangenehm niedrig ansetzen muss.

[Markus F. (Gast)]

Adrian B. schrieb:
> Meine Annahme, dass ich einfach statt der Zeitdifferenz die
> Sampledifferenz nehmen kann, da das ja nur ein skalierender Faktor in
> der Lösung wäre, war falsch.

Wieso ist die falsch? Zeit und Samples sind doch fest korrelliert?