IMU alignment methods

Question

I have an IMU that is outputting the following for its measurements:

accelx= 0.000909228 (g's)
accely= -0.000786797 (g's)
accelz= -0.999432 (g's)
rotx= 0.000375827 (radians/second)
roty= -0.000894705 (radians/second)
rotz= -0.000896965 (radians/second)

I would like to calculate the roll, pitch and yaw and after that the orientation matrix of the body frame relative to the NED frame.

So I do

roll = atan2(-accely,-accelz);
pitch =atan2(-accelx/sqrt(pow(accely,2)+pow(accelz,2)));
sinyaw = -rotycos(roll)+rotzsin(roll);
cosyaw = rotxcos(pitch)+rotysin(roll)sin(pitch)+rotzcos(roll)*sin(pitch);
yaw = atan2(sinyaw,cosyaw);

and I get:

roll = 0.000787244
pitch = -0.000909744
yaw = 1.17206

in radians.

However the IMU is also outputting what it calculates for roll, pitch and yaw.

From the IMU, I get:

roll: -0.00261682
pitch: -0.00310018
yaw: 2.45783

Why is there is a mismatch between my roll, pitch and yaw and that of the IMU's?

Additionally, I found this formula for the initial orientation matrix.

Which way of calculating the orientation matrix is more correct. R1(roll)*R2(pitch)*R3(yaw), or the form above?

Answer 1

The roll and pitch angles that you calculate using the accelerometer measurements will only be correct if (1) the IMU is non-accelerating (e.g., stationary), and (2) the accelerometer measurements are perfect. Thus, they can only be used to initialize the tilt (roll and pitch) of the IMU, not to calculate roll and pitch during acceleration. An external measurement of yaw angle is required to initialize the yaw angle.

See these answers:

• What information an IMU gives to a drone?
• Multicopter: What are Euler angles used for?

for some background.

Say the accelerometer measurements are $f_x$, $f_y$, and $f_z$; the gyro measurements are $\omega_x$, $\omega_y$, and $\omega_z$, and the magmetometer measuremenst are $b_x$, $b_y$, and $b_z$. The roll angle ($\phi$) and pitch ($\theta$) angle can be initialized if the IMU is not accelerating using \begin{eqnarray} \phi_0 &=& \tan^{-1}\left(f_y/f_z\right) \\ \theta_0 &=& \tan^{-1}\left(-f_x/\sqrt{f_y^2+f_z^2}\right) \end{eqnarray} The yaw angle ($\psi$) can be initialized using the magnetometer measurements. Given the roll and pitch angles, the magnetic heading ($\psi_m$) can be calculated from $b_x$, $b_y$, and $b_z$. Given the magnetic declination at the system location, the true heading (or initial yaw angle, $\psi_o$) can be calculated.

Once the IMU is initalized in an unaccelerated state, the gyro measurements can be used to calculate the rates of change of the Euler angles while the IMU is moving: \begin{eqnarray} \dot\phi &=& \omega_x +\tan\theta\sin\phi\,\omega_y +\tan\theta\cos\phi\,\omega_z \\ \dot\theta &=& \cos\phi\,\omega_y -\sin\phi\,\omega_z \\ \dot\psi &=& \sec\theta\sin\phi\,\omega_y +\sec\theta\cos\phi\,\omega_z \end{eqnarray} The rates of change of the Euler angles are then numerically integrated to propagate the Euler angles. The coordinate transformation matrix at each instant of time can then be obtained from the Euler angles.

This will work if your IMU never pitches up or down to $\pm90^\circ$. In that case it will be better to calculate and propagate quaternions instead of Euler angles. (Euler angles can always be calculated from the quaternions.)

Alas, the gyros are not perfect. Say that the gyros have bias errors, then these bias errors will also be integrated with time to result in the Euler angled "drifting". For this reason, an extended Kalman filter is often used to calculate the orientation of the IMU, aided by other measurements (magnetometer, accelerometer, and a GPS, for example). But that's another topic :)