0
$\begingroup$

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.

Initial orientation matrix formula

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

$\endgroup$
  • $\begingroup$ The present rol, pitch and yaw as the IMU yields can not depend on the present acceleration as the acceleration impacts only future roll, pitch and yaw values. Try to understand en.wikipedia.org/wiki/Euler_angles#Tait-Bryan_angles and use only rotx, roty, rotz for your calculation. $\endgroup$ – sirop Jul 7 '17 at 8:29
  • $\begingroup$ In the box just below the "So I do", you calculate the pitch as a function of a variable accel_x which doesn't seem to be defined. Is it a typo? $\endgroup$ – Christo Jul 7 '17 at 11:11
  • $\begingroup$ Are there magnetometer outputs available as well? $\endgroup$ – Christo Jul 7 '17 at 11:17
  • $\begingroup$ @Christo, Yes that is a typo. $\endgroup$ – rielt12 Jul 7 '17 at 11:36
  • $\begingroup$ @Christo. There are also magnetometer ouputs $\endgroup$ – rielt12 Jul 7 '17 at 11:36
1
$\begingroup$

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:

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 :)

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.