How to extract heading from VRTK2

The FP_A-ODOMETRY message contains the position and orientation of the VRTK2 sensor in the ECEF coordinate system. For mathematical stability, the sensor employs quaternions to represent these rotations. Nonetheless, in case the user requires a Euler angles representation using Roll-Pitch-Yaw angles, it is necessary to first convert the orientation of the sensor into a local tangential coordinate system such as ENU or NED.

Instructions

To convert the reference frame from ECEF to ENU, we apply the following transformation: 

where, is the orientation of the sensor in the ECEF frame and is the orientation of the ECEF frame in the ENU coordinate system (a local tangential plane). To convert into Euler angles, it is possible to use either a rotation matrix or quaternions. For a rotation matrix we apply the following equations: 

where

To convert a quaternion into yaw, pitch and roll angles (ZYX order), the following equation can be used: 

where . In this context, the Yaw angle with respect to the ENU (East-North-Up) frame represents the heading from East to North. To get the heading of the sensor from North in a clockwise direction, we need to compute 90° - yaw. Alternatively, the user can calculate the rotation in NED (North-East-Down) coordinates and then extract the Roll-Pitch-Yaw angles with respect to NED, where yaw would be directly the heading in the common sense.

The function EcefPoseToEnuEul(), in the Fixposition GNSS Transformation Lib, receives the pose of the sensor in ECEF coordinates and return the orientation of the robot in Yaw-Pitch-Roll angles using the equations described above. 

Related articles