Euler Angles
Sources
- Roll and Pitch Angles from Accelerometer Sensors, Michael Wrona’s Blog
- 9-Axis IMU Lesson 10: Making a Tilt Compensated Compass with Arduino, Paul McWhorter
Before we begin, I’d like to warn you to check the built-in coordinate axis of your IMU sensors. If I would have done this in the beginning, I would have been able to save myself a considerable amount of time on troubleshooting the divergence of the EKF. For example, in the MPU-9250 IMU, the Accelerometer and Gyroscope coordinate axis are in alignment. However, the MPU-9250’s magnetometer (AK8963), has the X and Y-axes swapped, as well as the Z-axis being inverted, or negated. Therefore, after calibrating the Accelerometer, Gyroscope, and Magnetometer, you must re-assign the Magnetometer axis so that:
$$ M_{old} = \begin{bmatrix} M_x \\ M_y \\ M_z \end{bmatrix} \qquad \longmapsto \qquad M_{new} = \begin{bmatrix} M_y \\ M_x \\ -M_z \\ \end{bmatrix} $$
The Euler angles are defined as rotations around each of the three axes of the xyz coordinate system:
- Roll, $\phi$, is rotation about the x-axis.
- Pitch, $\theta$, is rotation about the y-axis.
- Yaw, $\psi$, is rotation about the z-axis. (Yaw is also known as Heading)
We can represent the position of an object, using a vector, based upon the magnitudes and euler angles, as shown:
The Euler angles can be determined from our real-world sensors, specifically from an Accelerometer and a Magnetometer, as shown below:
$$ \phi = \arctan \left( \frac{A_{y}}{A_{z}} \right) $$
$$ \theta = \arcsin \left( \frac{A_x}{\sqrt{A^2_x + A^2_y + A^2_z}} \right) = \arcsin \left( \frac{A_x}{g} \right) $$
Where $\boldsymbol{g}$ is the force of gravity acting upon the static sensor.
$$ \psi = atan2 \left( \frac {-M_y \cos(\phi) + M_z \sin(\phi)} {M_x \cos(\theta) + M_y \sin(\phi)\sin(\theta) + M_z \cos(\phi) \sin(\theta) } \right) + \delta $$
Where $\delta$ is the Magnetic Declination, which is dependent upon the physical location of the sensor with respect to the Earth’s Magnetic field.