FixPosition Vision-RTK 2 - Page 85

To convert the ECEF orientation of the VRTK2 into this local frame (ENU) coordinates,
let’s assume q
ecef→body
represents the orientation of the sensor in ECEF coordinates,
which can be extracted from the pose output of the FP_A-ODOMETRY message. Then,
the rotation from ECEF coordinates to the local frame of reference (ENU) can be com-
puted using the current position of the sensor on the sphere <x,y,z>.
The Fixposition GNSS Transformation Lib(fixposition_gnss_tf) contains several useful
functions for these space operations. For example, the function TfEnuEcef() takes an
ECEF position coordinate and returns a rotation matrix that transforms from the ECEF
plane to the ENU plane. Let’s call this rotation matrix R
enu→ecef
. As the orientation output
of the VRTK2 sensor is represented using a quaternion, we need to first convert the
rotation matrix to a quaternion q
enu→ecef
. Thus, the rotation of the VRTK2 sensor in ENU
coordinates can be computed as the multiplication of q
enu→ecef
times q
ecef→body
. For a
more in-depth explanation, please refer to the following:
• Transformations/ between/ ECEF/ and/ ENU/ coordinates
• Geographic/ coordinate/ conversion
Extract heading:
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 an 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.
To convert the reference frame from ECEF to ENU, we apply the following transformation:
R
body→enu
= R
ecef→enu
· R
body→ecef
(C.1)
where R
body→ecef
is the orientation of the sensor in the ECEF frame and R
ecef→enu
is the
orientation of the ECEF frame in the ENU coordinate system (a local tangential plane).
To convert q
body→enu
into Euler angles, it is possible to use either a rotation matrix or
quaternions. For a rotation matrix, we apply the following equations:
α = tan
−1
µ
R
21
R
11
¶
, (C.2)
β = tan
−1



−R
31
q
1 − R
2
31



, (C.3)
γ = tan
−1
µ
R
32
R
33
¶
, (C.4)
where
R =


R
11
R
12
R
13
R
21
R
22
R
23
R
31
R
32
R
33


(C.5)
To convert a quaternion into yaw, pitch, and roll angles (ZYX order), the following equation
can be used:
α = tan
−1
µ
2q
x
q
y
+ 2q
w
q
z
q
w
q
w
+ q
x
q
x
− q
y
q
y
− q
z
q
z
¶
, (C.6)
β = tan
−1
¡
−2q
x
q
z
+ 2q
w
q
y
¢
, (C.7)
Vision-RTK 2 | Fixposition Positioning Sensor 79