问题
I have 3 receivers (A, B and C), and some signal producing source (let's say sound or light) with an unknown location. Given the locations of A,B and C, and the time at which each receiver "heard" the signal, I'd like to determine the direction of the source.
I understand there are ways to do so with TDoA multilateration/trilateration, however I'm having trouble implementing the calculation. There isn't a lot of clear, detailed information on this out there for those entirely new to the subject. What is out there is vague, more theoretical, or a bit too esoteric for me.
Some similar posts on SO (but not quite what I'm after): TDOA multilateration to locate a sound source Trilateration of a signal using Time Difference(TDOA)
This is also interesting, but assumes we have some boundaries: Multiliteration implementation with inaccurate distance data
@Dave also commented an excellent and fairly accessible resource https://sites.tufts.edu/eeseniordesignhandbook/files/2017/05/FireBrick_OKeefe_F1.pdf, but it falls short of going into enough depth that one might be able to actually implement this in code (at least, for someone without deep knowledge of regression, finding the intersection of the resulting hyperbolas, etc).
[EDIT]: I should add that I can assume the 3 sensors and the source are on the surface of the Earth, and the effects of the curvature of the Earth are negligible (i.e. we can work in 2-dimensions).
回答1:
Interesting problem. I am too lazy to derive equations for algebraic solution. Instead why not fit the result?
So simply fit the 2D (or higher) position using any fitting method capable of finding local solution (using optimization/minimization of some error value). When I use mine simple approximation search to fit the position the results looks pretty good.
The algorithm is:
iterate through "all" positions on your range
of coarse not all the heuristics of the fitting will reduce the problem a lot.
on each tested position compute the delta times that would be measured
simple time of travel from tested position into your receiver stations.
normalize all delta times so the start from zero
so substract the smallest time of arrival from all the recivers times. The same do for the real measured times. This assures the times does not involve relative offset.
compute the difference between real measured times and computed ones
simple abs difference is enough. Use this value as fitting parameter (optimization).
Here small C++ example doing this using my approx class from link above:
//---------------------------------------------------------------------------
// TDoA Time Difference of Arrival
//---------------------------------------------------------------------------
const int n=3;
double recv[n][3]; // (x,y) [m] receiver position,[s] time of arrival of signal
double pos0[2]; // (x,y) [m] object's real position
double pos [2]; // (x,y) [m] object's estimated position
double v=340.0; // [m/s] speed of signal
double err=0.0; // [m] error between real and estimated position
//---------------------------------------------------------------------------
void compute()
{
int i;
double x,y,a,da,t0;
//---------------------------------------------------------
// init positions
da=2.0*M_PI/(n);
for (a=0.0,i=0;i<n;i++,a+=da)
{
recv[i][0]=256.0+(220.0*cos(a));
recv[i][1]=256.0+(220.0*sin(a));
}
pos0[0]=300.0;
pos0[1]=220.0;
// simulate measurement
t0=123.5; // some start time
for (i=0;i<n;i++)
{
x=recv[i][0]-pos0[0];
y=recv[i][1]-pos0[1];
a=sqrt((x*x)+(y*y)); // distance to receiver
recv[i][2]=t0+(a/v); // start time + time of travel
}
//---------------------------------------------------------
// normalize times into deltas from zero
a=recv[0][2]; for (i=1;i<n;i++) if (a>recv[i][2]) a=recv[i][2];
for (i=0;i<n;i++) recv[i][2]-=a;
// fit position
int N=6;
approx ax,ay;
double e,dt[n];
// min, max,step,recursions,&error
for (ax.init( 0.0,512.0, 32.0 ,N, &e);!ax.done;ax.step())
for (ay.init( 0.0,512.0, 32.0 ,N, &e);!ay.done;ay.step())
{
// simulate measurement -> dt[]
for (i=0;i<n;i++)
{
x=recv[i][0]-ax.a;
y=recv[i][1]-ay.a;
a=sqrt((x*x)+(y*y)); // distance to receiver
dt[i]=a/v; // time of travel
}
// normalize times dt[] into deltas from zero
a=dt[0]; for (i=1;i<n;i++) if (a>dt[i]) a=dt[i];
for (i=0;i<n;i++) dt[i]-=a;
// error
e=0.0; for (i=0;i<n;i++) e+=fabs(recv[i][2]-dt[i]);
}
pos[0]=ax.aa;
pos[1]=ay.aa;
//---------------------------------------------------------
// compute error
x=pos[0]-pos0[0];
y=pos[1]-pos0[1];
err=sqrt((x*x)+(y*y)); // [m]
}
//---------------------------------------------------------------------------
Here preview:
Blue dots are the receivers, Red dot is real position of object and yellow cross is its estimated position. The area is 512x512 m and I fit it with initial step 32 m and 6 recursions leading to error ~0.005 m
I am more than happy with the result... You can change the number of receivers n without any real change to source or algorithm. I initiated the receiver positions as uniformly distributed on circle but the positions might be any other (not all on single line of coarse)
回答2:
The simplest (but not fastest) approach would be to solve the equations with gradient descent.
I'm assuming that we know
- the positions of the receivers, A, B, and C, which do not lie on the same line;
- the pseudorange of the unknown source X to each of A, B, and C.
Intuitively, we simulate a physical system with three ideal springs configured like so, where the equilibrium length of each spring is the corresponding pseudorange.
A
|
X
/ \
B C
The springs push when the distance is too small and pull when it's too large. The approximate resting place of X should be a reasonable estimate (though depending on your application you might want to do additional validation).
Here's some sample Python code using complex numbers as 2D vectors that should be easy to transliterate.
import random
def distance(p, q):
return abs(p - q)
# Force exerted by an ideal spring between variable y and fixed q of equilibrium
# length dxq.
def force(y, q, dxq):
dyq = distance(y, q)
return (dxq - dyq) * (y - q) / dyq
# Trilateration via gradient descent.
def trilaterate(
a, dxa, b, dxb, c, dxc, *, max_iterations=1000000, gamma=0.001, precision=1e-12
):
# Use the centroid of the receivers as the initial estimate.
y = (a + b + c) / 3
for i in range(max_iterations):
f = force(y, a, dxa) + force(y, b, dxb) + force(y, c, dxc)
y += gamma * f
if abs(f) <= precision:
return y
return None
def random_point():
return complex(random.random(), random.random())
def test_error():
a = random_point()
b = random_point()
c = random_point()
x = random_point()
y = trilaterate(a, distance(x, a), b, distance(x, b), c, distance(x, c))
return distance(x, y)
if __name__ == "__main__":
print(test_error())
来源：https://stackoverflow.com/questions/63707712/how-to-localize-a-signal-given-the-location-of-three-receivers-and-the-times-at