Rotation in 3D space using Quaternion

Question

System Configuration

I have two simulation systems (which I am going to call Sim-A and Sim-B) where I am going to spawn an entity created in Sim-A inside Sim-B.

Expected outcome

The entity created in Sim-A appears in Sim-B in the right position with the correct orientation.

System Input

The position of the entity created in Sim-A received in Lat/Long/Alt format (it assumes WGS-84 spheroid) and the orientation in Euler angles.

The Euler angles would help me to transform an entity from its world coordinate system to entity coordinate system by applying the Euler angles in the order of heading, pitch and yaw. Below is how it is documented.

" The order of rotation is first, rotate about z by the angle psi (), then about the new y (y') by angle theta (T), then about the newest x (x'') by the angle phi (I). The positive direction of rotation about an axis is defined as clockwise when viewed toward the positive direction along the axis of rotation."

However, in Sim-B this rotation is defined in Quaternion and it assumes a perfect sphere for the world (not WGS-84). As a result, I have to convert Euler to Quaternion before sending the information to Sim-B.

I have achieved this by applying ZYX conversion (page A-11) given in [here][2].

Challenge

When I pass the position and orientation of an entity created in Sim-A to Sim-B ( Sim-B uses ECEF system, so I do another conversion to go from LLA to ECEF), it appears in the correct LLA position but its orientation is incorrect (shown in the figure.).

This can be fixed by adding some value to the pitch and roll but it is not mathematically sound. Also, these values might change (haven't tried) if you spawn the entity in a different location of the earth due to its shape.

So, I would like to know the right way of correcting this issue (i.e. how to make the entity to stand upright). Assume that I can get the Up, Front and Right vector of the entity when it is spawned in Sim-B.

Answer 1

If you can obtain the Up, Front and Right vector, you have a full orthonormal basis to reconstruct the quaternion, you don't need to worry about Euler angles, just forget the Euler angles and build the quaternion directly from the Orthonormal Basis (x axis = -Right, y axis = Up, z axis = Front).

The simplest way to do this is to build a rotation matrix from the basis and then construct the quaternion from the rotation matrix, should be straight forward and should always give you the expected result.

Here's how to do the first step: http://renderdan.blogspot.cz/2006/05/rotation-matrix-from-axis-vectors.html

Here's how to do the second: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/

Hope it helps.