It's been a while since my last log, but progress is being made. The big problem has been how to convert from point locations & lighthouse angles into a known position of the tracked object in space. The first part of that problem involves figuring out where the lighthouses are in relation to the tracked object. The libsurvive project is also solving much the same problem, so I've been working in the context of that project.
There are a number of ways to solve this problem, and it's not clear at this point which solution will work the best. The approach I'm taking is as follows:
- Given any pair of points (sensors), and the angle they make relative to the lighthouse, the set of possible locations of the lighthouse is defined by a torus.
- Given a set of tori, look for the spot where all of the tori converge in one point, and that will be the location of the lighthouse
Now, that sounds simple enough. But I'm not aware of any simple solution for finding the converging location for multiple tori, particularly when there is noise in the signal so they don't all converge perfectly.
So, here's my solution:
For each torus, generate a point cloud. Then, for one of the tori, for each point on the torus, compute a least-squares distance to the nearest point on each other torus. Whichever point has the lowest least-squares distance will be a good estimate of the location of the lighthouse.
Now, you need to refine that initial guess to get a higher precision estimate of the lighthouse location. Right now I am using the same approach as above, except just rendering part of the tori at higher resolution. It looks something like this:
In this image, you can see that there are so many tori being calculated that it's hard to distinguish each one. But you can also see the area that looks denser. The red "star" indicates the location of the initial guess, and as a result a second pass was done just in that area, with each tori being rendered in much higher resolution. Looking very closely, you can even see that a third pass was done in the center of that patch to eek out the most precision possible using this approach. While this seems to work fairly well, it's also somewhat memory intensive, and very CPU intensive.
So, I'm planning a different approach for refining the initial estimate that avoids a point cloud altogether by employing a gradient descent. Basically, it involves looking at the 3 dimensional gradient of the least-squares distance to each torus, and iteratively moving the "best estimate" point along this gradient until it reaches a local minimum (and ideally this will also be the global minimum, too).