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$ Feb 27 '16 at 15:43

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
    Feb 28 '16 at 0:05
  • $\begingroup$ Incomplete answer :/ Mainly to show how I am doing right now, maybe I am doing it wrong. $\endgroup$ Feb 28 '16 at 1:10
  • $\begingroup$ I finally got the complete solution :D $\endgroup$ Feb 28 '16 at 14:53

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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