played with the math today and couldn't figure out a nice formula, so I tried another approach, simulation.
When an impact is made one knows three time differences (these are simulated in the code below). In the simulation I try (not) all points of the grid:
- if this point is the point of impact would it (re)create the observation made?
Based upon a grid of 1000x1000 the first approach (brute force) worked well but too slow. The current version starts with the middle of the grid and searches for the points with a lower error around it. This repeats until no better point is found (and the error == 0).
The timing improved dramatically, brute force took several minutes while the current algorithm is in order of 100 msec on my 328 - at least as far as I tested.
Comments and improvements are as allways welcome.
- update -
patched a bug;
timing now averaging around ~100 msec (sometimes one pixel off)
//
// FILE: impactSearch.pde
// AUTHOR: Rob Tillaart
// DATE: 2011-02-21
//
// PURPOSE: determine impact location based upon arrival time of signal at 4 sensors
//
#define SQR(x) ((x)*(x))
// THE GRID = 1000 x 1000;
// p[] = point A, B, C, D
const long p[8] = { 0,0, 0,1000, 1000,0, 1000,1000 };
int best_x = 0; // point to be found
int best_y = 0; // idem
float best_SE = 0; // Square Error
unsigned long start = 0; // timer
void setup()
{
Serial.begin(115200);
Serial.println("Start...");
}
void loop()
{
// simulation random impact near point A then B then c then D
// as the grid is 1000x1000 the max location is 500-500 as otherwise
// it would be closer to B, C or D
long px = random(499);
long py = random(499);
Serial.print(px);
Serial.print(",");
Serial.print(py);
Serial.print("\t\t");
// The distances to the 4 mikes are calculated
// and the value of TA is subtracted as there the sound arrives first.
// Note distance is time * speed of sound
// the values here are calculated
// one could add random noise for the simulation
float TA = sqrt(SQR(px) + SQR(py));
float TB = sqrt(SQR(px) + SQR(1000L - py)) - TA;
float TC = sqrt(SQR(1000L-px) + SQR(py)) - TA;
float TD = sqrt(SQR(1000L-px) + SQR(1000L - py)) - TA;
start = millis();
impactSearch(TA, TB, TC, TD);
Serial.print(millis() - start); // how fast
Serial.print("\t");
// print delta + error
Serial.print(best_x - px);
Serial.print(",");
Serial.print(best_y - py);
Serial.print("\t");
Serial.println(best_SE);
}
// impactSearch detemines the point of impact by searching
// the point with a smart trial and error method
// in essence:
// It first takes the middle and tests if that was the point of
// impact what would be the timing and compares that to the factual
// times. Then it does the same for the points around it and
// moves from point to point decreasing the error.
// if there is no better point, the point of impact has been found
//
// This code uses an optimization by doing big steps in the beginning
// and decreasing the stepsize until 0 in the end
//
// Some optimizations are under investigation
// e.g. 3 mikes seems to be enough, but when noise is added
// a fourth mike reduces the overall error I think.
void impactSearch(float t1, float t2, float t3, float t4)
{
// start in the middle
// in fact (250,250) is a better point to start as this
// is the middle of the grid
best_x = 500;
best_y = 500;
best_SE = 100000000L;
int step = 16;
boolean found = false; // point not found yet
while (false == found)
{
boolean decreaseStep = true;
for (int x = best_x-step; x <= best_x+step; x+=step)
for (int y = best_y-step; y <= best_y+step; y+=step)
{
// optimization, same point is never better
if (y== best_y && x==best_x) continue;
// determine what would be the arrival time
// for point x,y
// DA = distance to point A - etc
float DA = sqrt( SQR(p[0]-x) + SQR(p[1]-y) );
float DB = sqrt( SQR(p[2]-x) + SQR(p[3]-y) );
float DC = sqrt( SQR(p[4]-x) + SQR(p[5]-y) );
float DD = sqrt( SQR(p[6]-x) + SQR(p[7]-y) );
// use square error when compared to the real arrival times.
float se = SQR(DA + t2 - DB);
se += SQR(DA + t3 - DC);
se += SQR(DA + t4 - DD);
// remember the one with the smallest error
if (se < best_SE)
{
decreaseStep = false;
best_SE = se;
best_x = x;
best_y = y;
}
}
// if no better point found decrease the search area
// by decreaing the step size
if (decreaseStep) step--; // was step = step/2;
found = (step == 0);
}
}