LH2 geometry

[PhileasL]

Purpose of this PR

The goal of this PR is to build a mathematical model to interpret FPGA data to an azimuth and elevation w.r.t. the lighthouse.
The full description of the mathematical model isn't about to be written here. I'll dedicate a whole paper on it as it's pure mathematics and not really informatics.

The mathematical model roughly explained without equations

Remember, the FPGA is returning iterations of LFSR. The frequency of the LFSR is 12MHz and the frequency of the rotor of the lighthouse (from which laser planes are emitted) is somewhere around 50Hz (depending on the mode of the lighthouse).

In addition with theses data, we know that laser planes are tilted with an angle of ~30° from the axis of rotation of the rotor. The start angle of the rotor is the angle representing iteration 0 of the LFSR and the phase of the system is the angle between the 2 lenses of the rotor (lenses that diffract the laser into planes).

With those data, the intersections of the 2 planes can be mathematically computed. The intersection is a line with a certain azimuth and a certain longitude in the lighthouse frame.

With several azimuth and elevation from the same laser sweep, a pose can be computed.

[PhileasL]

No real need of changing anything as it's a test and not the final mathematical model at its finest version.