I have to know where a multi-rotor is, in a rectangular room, via 6 lasers, 2 on each axis.
The problem is like this:
- Room : square => 10 meters by 10 meters
- 6 positions of the lasers : Fixed on the frame
- 6 orientations of the lasers : Fixed on the frame
- The 6 measurements of the lasers
- The quaternion from the IMU of my flight controller (PixHawk).
- The origin is centered on the gravity center of the multi-rotor and defined as if the walls are perpendicular to each axes (the normal of the wall in X is (-1,0,0))
- Position in 3D (X,Y,Z)
- Angular position (quaternion)
Since I got the angular position of the multi-rotor, I rotated the laser positions and orientations via the quaternion, then extrapolate via the 6 measurements and I got the 3 walls. (orientations of the walls are trivial, then only one point is enough to determine its position.
Badly, I noticed that the yaw (rotation about z) measurement from the PixHawk is unreliable. Then I should measure the yaw from the lasers, but I do not success to do it. Event if the 2D problem is easy, I am lost in 3D.
Does someone know if it [Algorithm to know XYZ position and quaternion from 6 measurments] exists somewhere ? Or what is the right way to go on this problem ?
The question : How could I get the yaw from 2 measurements from 2 lasers which I know the original position, orientation and the pitch and roll.
NOTE : Green pointers are the origin position, Red pointers are the "final" position, but could be rotated around the red circle (due to yaw).