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:

Inputs :

  • 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))

Output :

  • 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).


  • $\begingroup$ We can easily get the orientation via MavLink from the PixHawk. Badly, the yaw, heading, orientation about Z is not stable due to the indoor application, while pitch and roll are stable. I am editing the question to add the setup $\endgroup$ Commented Feb 27, 2016 at 15:43

1 Answer 1


Solution : Is there another solution without prerotating vectors ?

I finally got a solution, and here it is.

Python, ROS geometry library, numpy

My actual code/maths in short :

1) Rotate the position & orientation of lasers by roll & pitch. The axes='sxyz' means : Static axis, apply roll, pitch, yaw.

quaternion_matrix creates a 4x4 transformation matrix from the quaternion.

laser = (1,1,1,0) # laser position
orientation = (1,0,0,0) # laser orientation

roll, pitch, _ = list(euler_from_quaternion(q, axes='sxyz'))
q = quaternion_from_euler(roll, pitch, 0, axes="sxyz")
laser = numpy.dot(quaternion_matrix(q), laser)
orientation = numpy.dot(quaternion_matrix(q), orientation)

2) Algebric solution : Rotation around Z in function of yaw

Rotation around Z

laser       = [-sin(a)*laser[1] + cos(t)*laser[0], 
                cos(t)*laser[1] + sin(t)*laser[0],

orientation = [-sin(a)*orientation[1] + cos(t)*orientation[0], 
                cos(t)*orientation[1] + sin(t)*orientation[0],

3) Algebric solution : Extrapolation from the measurments in function of yaw

Important notice : Since the rotation do not scale vectors, the denominator of the K factor is a constant. Then, we can simplify it by precompute length of the orientation vector.

M = 100 # distance
K = sqrt(M^2 / (orientation[0]^2 + orientation[01]^2 + orientation[1]^2))
PointOnWall = [ K * orientation[0] + laser[0],
                K * orientation[1] + laser[1],
                K * orientation[2] + laser[2]]

4) Algebric solution : From this, on two laser, get walls.

The two "PointOnWall" equations should gives enough data to get the yaw. Knowing this is a (-1,0,0) normale, I can find 2 planes from the two points :

Wall equation

5) Algebric solution : Measure the YAW.

One plane in the other (Via XMaxima), we got :

Tan equation

def getYaw(position1, orientation1, measure1, position2, orientation2, measure2):
    length1 = length(orientation1)
    length2 = length(orientation2)
    k1 = measure1/length1
    k2 = measure2/length2
    numerator   = -k2*orientation2[0] + k1*orientation1[0] + position1[0] - position2[0]
    denominator = -k2*orientation2[1] + k1*orientation1[1] + position1[1] - position2[1]
    return atan(numerator/denominator)

As expected, roll & pitch DO NOT interfere, since the positions and orientations are prerotated.

  • $\begingroup$ Is this an answer to your own question, or more detail for the question? $\endgroup$
    – Ben
    Commented Feb 28, 2016 at 0:05
  • $\begingroup$ Incomplete answer :/ Mainly to show how I am doing right now, maybe I am doing it wrong. $\endgroup$ Commented Feb 28, 2016 at 1:10
  • $\begingroup$ I finally got the complete solution :D $\endgroup$ Commented Feb 28, 2016 at 14:53

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.