Ok here's the problem... we have a rectangular area ABCD and somewhere in that area (point X) there is an event that produces a sound (e.g. a ping pong ball strikes the surface). This results in sound waves travelling outwards to the corners where they are picked up by sound detectors.
The first sound to be picked up at each sensor arrives at a time that obviously depends on that sensor's distance from the original sound. Lets call the times tA, tB, tC, tD. (Each sensor will pick up reverberations and echoes after the original sound, but its the first "edge" of the sound we're interested in)
Now since we don't know when the original sound happens (we only detect it when it got to the closest sensor) we don't actually know the real values of tA, tB, tC, tD but we get relative times from the first sensor. In this case X is closest to D, the sound reached D first. The time we read at each sensor A,B,C is relative to tD. Graphically this can be shown like this (each red line is reduced by distance XD)
Now we can use these reduced distances to define circles based on the points A,B,C and D (radius at D is initially zero)
As pointer out by Arduino forum member Necromancer on http://arduino.cc/forum/index.php?topic=52583.0 we can find X geometrically by progressively increasing the radii of the circles at A,B,C and D by the same amounts until all the circles intersect at a single point. At this point we have added back the unknown distance XD and the circles intersect at point X as shown below.
The problem is that this iterative calculation of circle intersections is processing-heavy and would probably take too long to solve on a microcontroller to be responsive enough for my application. However I started thinking about getting a "head start" by doing some simple calculation to get the initial increment where all the circles intersect for the first time, and go forward from there.
Here's what I mean.... point Q is a point on the line AC which is equal distances from the points where the initial circles around A and C cross AC.
The actual coordinates of Q aren't needed, we just need to know how much to increase the radii of the circles around A and C so that they intersect at the first time (which will happen at Q)
The calculation is simply the length of AC minus the radii of the two circles, all divided by two. If we calculate this value and add it to the radii of the circles they will meet at Q.
If we do the same calculation for lines AB, BC, CD, AD, AC, BD we'll get 6 different values that we should increase circle radii by make them intersect. We can just take the largest of all the 6 values and apply that as the base value by which the radii of all the circles should be increased to get to the initial point where all the circles intersect.
This will not neccessarily be point X... we might need to interatively expand the circles a bit more to make them all intersect at the same point. However we got a big head start and so far we've just done simple arithmetic.
I made a simple program where I could click with the mouse to simulate the values arriving at the 4 sensors., then applied the above calculations... in many cases the results are very close to the final point X. For example:
In other places the accuracy is not so spot-on, but it looks like some kind of simple averaging of the points of intersection might give a position good enough for what I want, and without having to iteratively apply complex calculations (like square roots), so it should be pretty fast.
Only three sensors are strictly required for multilateration, however I found that adding the fourth sensor made a massive difference to the accuracy of the above calculation.
The next step is to calculate the points of intersection and give it a try with some averaging, to see if I can avoid the iterative method. However I'm not a mathemetician, so if anyone has a better idea, please let me know!
Update: OK, I gave the averaging thing a try. This clip shows my test program using these calculations to track the mouse pointer. The tracking is not perfect and there are a couple of places (vertical midpoint of the area, towards left and right side) where it is worst, but I think this is good enough (especially given that my sensors won't be perfect!).
The program displays the fours circles calculated as above. The final calculated position is displayed by the red crosshairs
The process is as follows:
1. Firstly I need to simulate the sensor inputs, so I take the position of the mouse pointer, calculate distance to each corner and then subtract the minimum distance from all the others. The results (tA, tB, tC, tD) are representative of time or arrival info from real sensors.
2. For each edge, and the two diagonals I get the length of the line (area width, height or diagonal distance) and subtract the relative arrival times for the points at each end.
offset1 = (width - tA - tB)/2
offset2 = (width - tC - tD)/2
offset3 = (height - tA - tD)/2
offset4 = (height - tB - tC)/2
offset5 = (diagonal - tA - tC)/2
offset6 = (diagonal - tB - tD)/2
Now take the maximum of these 6 values:
offset = max(offset1,offset2,offset3,offset4,offset5,offset6)
now calculate the circle radius by adding the offset to each time.
rA = tA + offset
rB = tB + offset
rC = tC + offset
rD = tD + offset
now calculate the intersection points of all pairs of circles. I used the C code example from here http://paulbourke.net/geometry/2circle/
There are six pairs of circles, AB, AC, AD, BC, BD, CD. Not all may intersect (ignore pairs which do not intersect). Otherwise we get 2 insection points (which may be identical if circles just touch) for each pair of circles. Lets say that for each pair of circles we can get two intersection points P and P'
For each pair of points P and P', one will be closest to our target point and the other should be discarded. The way I did this was to calculate the average of all the points, then go back through the list selecting the point from each pair that was closest to the calculated average point and then averaging just these "closest points".
ie. Take the first "rough" average of all points P and P' - lets call is (Xave, Yave), then recalculate the average position using either P or P' from each pair based on the condition:
if (Xp - Xave)^2 + (Yp - Yave)^2 > (Xp' - Xave)^2 + (Yp' - Yave)^2
then use point P'
else use point P
The resulting average (X , Y) is the final calculated point.
Better accuracy would be got by interatively increasing rA, rB, rC and rD and recalculating the intersection points until they are at their closest to each other. However I don't think I need this - the sensor input is unlikely to be so accurate it would benefit from this.... and I think it would be computationally expensive due to calling sqrt( ) many times and therefore slow.
Once again I'm no mathematician and I'd be grateful for any advice here!
