I'm pretty convinced at this point that the algorithm to use to find location is EPnP. The authors of the original paper have provided sample code to run the algorithm, which is awesome. But, the sample code depends on OpenCV libraries, so that's a bit of a hitch. I'll have to pull out or rewrite the parts of OpenCV that it uses in order to get this loaded on the Arduino Due.

So, here's the current plan:

1) Modify/ use the existing EPnP code while running on a desktop to derive the position and pose of the sensor board relative to the lighthouse. Values will be captured, and offline analysis will be done. i.e. This is not realtime, just proof that I can get the code to work.

2) Build a derivative EPnP library that does not rely on OpenCV. Ensure that it behaves the same as (1).

3) Load the code from (2) onto the Arduino Due, and have it calculate pose information realtime. Benchmark, and see if it's "good enough"

4) If the speed of the algorithm is (3) is too slow, iteratively attempt to optimize the algorithm, likely by trying to push as much math as possible into using integer math instead of floating point math. Not sure how easy it will be to do this with EPnP, but probably worth a shot.

5) Build support for on-device calibration of a world reference frame. I'm expecting this to have a user experience something like:

1) Place the tracked device where the origin should be & press a button. 2) Place the tracked device at least a meter away in the +Y direction & press a button. 3) Place the tracked device in the +X direction by about a meter & press a button.

At this point, the tracked object should be able to have a well defined world coordinate system, and it should know how that coordinate system maps to the Lighthouse's coordinate system. The Arduino should be able to consistently spit out the tracked object's location in the world coordinate system now.

And here's the part I really like: 6) It should spit out location data in standard NMEA sentences. That's the standard way that just about all GPS receivers report location information. Why does that matter? It means you could hook this up to any device that expects to use a GPS receiver, but instead it's using lighthouse tracking instead. For example, you could hook it up to a small drone using an off-the-shelf UAV board, and program flight paths using off-the-shelf software, without any modifications.