Ok, here a numerical solution for your problem. Couldn't find an elegant solution that resolves this complex system (but still, it looks like it is possible). 8)
Some facts: you need 4 sensors, as one does only determine the first time impact, so you actually have only 3 data values, and as someone maybe remembers, with 3 values you can determine a triangle, not with 2. The following algorithm proves that (generating a lot of solutions if you don't consider the 3rd value, and they are all ok ).
The algorithm is based on this idea.
Not knowing the time to first impact I assume that I know it anyway, I calculate for a guessed value what x y should be, knowing two delta times (easy application of law of cosines). The third delta time cross checks the initial guess of t. Varying the guess of t you find the minimum error.
This small program has also a routine for testing some points on a grid.
What you need to do:
- ordering of impacts and mirroring the result values for the 4 different quadrants.
- feed the sound data to the guessing routine with the sound correction constant (speed of sound + temperature.
- it is highly optimizable (i think in 20 steps you can obtain an error of 0.01%, not sure, if I have time I will try that, it doesn't look necessary)
#include <stdlib.h>
#include <math.h>
/* quick and dirty code, not written on the arduino
*/
double a=1000,b=700; // this is all ugly, better create a class for the whole thingy
/* equations system is:
x=(p^2-t^2-a^2)/(-2*a), // cosine law, p and q the t1+t and p=t2+t
y=(q^2-t^2-b^2)/(-2*b)
*/
#define SQR(x) ((x)*(x))
void getxy(double t, double p, double q, double *x, double *y)
{
*x=(SQR(q)-SQR(t)-SQR(a))/(-2*a);
*y=(SQR(p)-SQR(t)-SQR(b))/(-2*b);
}
void findXY(double t1, double t2, double t3, double eps, double *xres, double *yres)
{
double t,x,y;
double mineps=sqrt(a*a+b*b);
/* this can be highly optimized !!!
the minimum t depends on the maximum time on the diagonal of the opposite sensor (or something like that)
*/
for (t=0; t < sqrt(a*a+b*b); t+=0.1)
{
getxy(t,t1+t,t2+t,&x,&y);
double t3e=sqrt(SQR(a-x)+SQR(b-y)); // cross check with opposite point (latest arrival)
// printf("t=%f x=%f y=%f t3=%f eps=%f\n ", t,x,y, t3e, fabs(t3+t-t3e));
if (fabs(t3+t-t3e) < mineps)
{
/* you can optimize this too, once we have a local minimum for 3 values (epsilon[0]>epsilon[1] < epsilon[2]), we can refine the search (t+=0.01 starting with t-0.1 until t+0.1) or exit
*/
mineps=fabs(t3+t-t3e);
*xres=x;
*yres=y;
}
}
}
void test(double x, double y)
{
double t1=sqrt(SQR(x)+SQR(y));
double t2=sqrt(SQR(x)+SQR(b-y));
double t3=sqrt(SQR(a-x)+SQR(y));
double t4=sqrt(SQR(a-x)+SQR(b-y));
printf("%.1f %.1f %.1f %.1f: ", t1,t2,t3,t4);
// printf("%f x ", (SQR(t2)-SQR(t1)-SQR(b))/(-2*b));
// printf("%f\n", (SQR(t3)-SQR(t1)-SQR(a))/(-2*a));
t2-=t1;
t3-=t1;
t4-=t1;
t1=0;
printf("t1=%.1f t2=%.1f t3=%.1f ", t2,t3,t4);
findXY(t2,t3,t4,0.01,&x,&y);
printf("~ {%.2f;%.2f}\n",x,y);
}
void testgrid()
{
for (double y=0; y <= b/2; y+=50.0)
{
for (double x=0; x <= a/2; x+=50.0)
{
test(x,y);
}
}
}
int main(int argc, char *argv[])
{
testgrid();
}