[Math] Quaternion – Angle computation using accelerometer and gyroscope

3danglequaternionsrotations

I have been using a 6dof LSM6DS0 IMU unit (with accelerometer and gyroscope) and am trying to calculate the angle of rotation around all three axes. I have tried many methods but am not getting the results as expected.

Methods tried:

(i) Complementary filter approach – I am able to get the angles using the formula provided in the link Angle computation method.

But the problem is that the angles are not at all consistent and drift a lot. Moreover, when the IMU is rotated around one axis, angles calculated over other axes wobble too much.

(ii) Quaternion based angle calculation: There were plenty of resources claiming the angles are calcluated very well using quaternion approach but none had a clear explanation. I have used this method in order to update the quaternion for every values taken from the IMU unit. But the link didn't explain how to calculate the angles from a quaternion.

I have used glm math library in order to convert the quaternion to Euler angles and also have tried the formula specified in the wiki link. With this method, since pitch calculation asin returns only $-90$ to $90$ degrees I am not able to rotate the object in 3D as shown in the link.

Has anyone tried the quaternion to angle conversion before? I need to calculate the angles around all the three axis in the range $0$ to $360$ degrees or $-180$ to $180$ degrees.

Any help would be really appreciated. Thanks in advance.

Best Answer

The simplest way to obtain the relative orientation is the integrating of kinematic equations Quaternion kinematics for the error-state KF (formula 107). All the explanations about quaternions are in the book.

Gyroscope measures angular velocity $\omega$, so the relative orientation you can evaluate by integrating (in real-time) the kinematic equation $\dot{q}=\frac{1}{2}q\circ\omega$, where normalized quaternion $q$ defines orientation of the body-frame relative to the initial frame. The disadvantage of this method is the result diverges with time (because of integration errors and precision of gyroscope).

There is a better approach uses other sensors.

If you want to represent relative orientation as a sequence of rotations around 3 axis you should learn a bit about the Euler angles. Actually the second angle $\beta$ should be always in range $-\frac{\pi}{2}..\frac{\pi}{2}$ or in $0..\pi$.