Quaternion NED to ENU conversion seems incorrect

[SanderVanDijk-StreetDrone]

The output of the quaternion based EKF filter is converted from NED to ENU convention here:

sbg_ros2_driver/src/message_wrapper.cpp

Lines 646 to 652 in f6c476a

	if (m_use_enu_)
	{
	ekf_quat_message.quaternion.x = ref_log_ekf_quat.quaternion[1];
	ekf_quat_message.quaternion.y = -ref_log_ekf_quat.quaternion[2];
	ekf_quat_message.quaternion.z = -ref_log_ekf_quat.quaternion[3];
	ekf_quat_message.quaternion.w = ref_log_ekf_quat.quaternion[0];
	}

It simply negates the y and z components. Compare this to the conversion of the Euler angle based output here:

sbg_ros2_driver/src/message_wrapper.cpp

Lines 569 to 574 in f6c476a

	if (m_use_enu_)
	{
	ekf_euler_message.angle.x = ref_log_ekf_euler.euler[0];
	ekf_euler_message.angle.y = -ref_log_ekf_euler.euler[1];
	ekf_euler_message.angle.z = wrapAngle2Pi((SBG_PI_F / 2.0f) - ref_log_ekf_euler.euler[2]);
	}

Given an axis/angle representation of a rotation with angle $\theta$ and axis:

${\vec {u}}=(u_{x},u_{y},u_{z})=u_{x}\mathbf {i} +u_{y}\mathbf {j} +u_{z}\mathbf {k} $

the quaternion representation is:

$\mathbf {q} =\cos {\frac {\theta }{2}}+(u_{x}\mathbf {i} +u_{y}\mathbf {j} +u_{z}\mathbf {k} )\sin {\frac {\theta }{2}}$

So flipping the y and z components of the quaternion is equivalent to mirroring the axis of rotation through the x/z and the x/y planes.

For example, if the axis of rotation in NED is $[0, \frac{\sqrt{2}}{2}, \frac{\sqrt{2}}{2}]$, in ENU that is $[\frac{\sqrt{2}}{2}, 0, -\frac{\sqrt{2}}{2}]$, however, the current implementation results in $[0, -\frac{\sqrt{2}}{2}, -\frac{\sqrt{2}}{2}]$.

So from this, I think the correct implementation should be:

    ekf_quat_message.quaternion.x = ref_log_ekf_quat.quaternion[2];
    ekf_quat_message.quaternion.y = ref_log_ekf_quat.quaternion[1];
    ekf_quat_message.quaternion.z = -ref_log_ekf_quat.quaternion[3];
    ekf_quat_message.quaternion.w = ref_log_ekf_quat.quaternion[0];

[SanderVanDijk-StreetDrone]

This discussion in an issue for a driver for a different device is related:

ros-drivers/microstrain_mips#4

They indeed swap x and y and negate z. However, the argument in the issue is that that cannot be right, because a an identity quaternion [z, x, y, z] = [1, 0, 0, 0], which would indicate alignment with the respective frame, stays the same under this transformation. Meaning that the device would be aligned with both the NED and the ENU frame.

It seems that after the issue was created, they did add a parameter to transform using a proper quaternion multiplication:

https://github.com/ros-drivers/microstrain_mips/blob/0fdb75b2da13d226bf38ef664ae4d9c9d9840163/src/microstrain_3dm.cpp#L3320-L3336

[SanderVanDijk-StreetDrone]

The convention has been there in the ROS 1 driver as well, and looking at the README it is intended:

https://github.com/SBG-Systems/sbg_ros_driver#frame-convention

There is a PR with somebody putting in a fix, by what looks like a quaternion multiplication similar to the previous comment, though for the IMU data rather than for the odometry:

SBG-Systems/sbg_ros_driver#81

[SanderVanDijk-StreetDrone]

The following code base for 'field robotics' also performs a swap of x and y and negation of z:

https://github.com/FroboLab/frobomind/blob/389f7e6b14434811e7836bf260295c8bdcf522ca/fmSensors/global_sensing/imu/vectornav_vn100/src/vectornav_vn100_node.cpp#L113-L135

[SanderVanDijk-StreetDrone]

OxTS converts RPY measurement from their device to a quaternion, then (left) multiplies that with a quaternion that represents a 90 degree yaw and 180 degree roll:

https://github.com/OxfordTechnicalSolutions/oxts_ros2_driver/blob/25d4e005f3b3c985d00f4f417a7c0ea27cb6f714/oxts_ins/src/conversions/wrapper.cpp#L44-L57

There is also an issue with some very long derivations about how the orientations are or aren't correct:

OxfordTechnicalSolutions/oxts_ros2_driver#7

[SanderVanDijk-StreetDrone]

Interestingly, the ROS 1 version of the SBG driver has a closed but unmerged PR, that also flips x and y and negates z:

https://github.com/SBG-Systems/sbg_ros_driver/pull/58/files#diff-8109b4f810b2fa296fe48e243ee06da35f66bf9dea8b84a30c149d0a5cd6ff31R435-R455

Specific section:

https://github.com/SBG-Systems/sbg_ros_driver/blob/6e78d40e0f70ffcb6a01622b82742adbddc80aa1/src/message_wrapper.cpp#L435-L445

It looks however like a bunch of the changes from there still made it into the driver. The following commit changed the quaternion transformation from initially swapping x and y and negating z, to negating y and z:

SBG-Systems/sbg_ros_driver@1d57b9f