silo
Posting GPT answer as it is the most concise:
Sure! Here's the modified version of my first answer with inline formulas:
Given a quaternion ( q = (q_w, q_x, q_y, q_z) ), you can convert it to roll, pitch, and yaw (Euler angles) using the following equations:
(can't render latex so I'll add python code)
import math
def quaternion_to_euler(q_w, q_x, q_y, q_z):
# Roll (x-axis rotation)
roll = math.atan2(2 * (q_w * q_x + q_y * q_z), 1 - 2 * (q_x**2 + q_y**2))
# Pitch (y-axis rotation)
pitch = math.asin(2 * (q_w * q_y - q_z * q_x))
# Yaw (z-axis rotation)
yaw = math.atan2(2 * (q_w * q_z + q_x * q_y), 1 - 2 * (q_y**2 + q_z**2))
return roll, pitch, yaw
As for the declination issue; I assume you are operating in a small scale (relative to earth size) so you can use a constant declination theta which can be computed once from the difference between the RTK and the magnetometer (or by checking the declination online for your specific location). Then correct the heading by that angle theta.
Thoughts?
Thanks,
Jaka