Quaternion angle - Opengl rendering

Question

I have been using a 6dof LSM6DS0 IMU unit (with accelerometer and gyroscope). I am trying to calculate the angle of rotation around all the three axes and Render a 3D cube using opengl to immitate the rotations being done with IMU, like the one in this link. I have previously posted a question here and with the help of the members, I got my solution.

I used Madgwick's quaternion method and rendered a cube, but Now I have been facing some issues in the result I got,

The quaternion gets updated for every accelerometer and gyroscope value(even for small changes when the IMU is completely static). This causes the cube rendered to rotate when the IMU unit is static. Fix : I compared the current IMU values with previous values and update the quaternion only when there is notable difference. The issues, I am facing currently are,

1. After a few rotation, when I bring back the IMU to its original position, I am not getting the same initial rendered result, there has been a big change. Why is this happening, what can be done to nullify this effect?

2. When the IMU is rotated faster, the rendering is not proper?

Any help could be really appreciated. Thanks in advance.

Answer 1

From my experience in the past with things like this, your first problem is because of accumulated error. You can't simply "throw away" sensor data like that and expect it work out. This is because the sensors you have - a gyro that measures $\omega$ only gives you the rate of change of $\theta$. True, you're using the accelerometer to correct for this, but there will be some error that creeps in (even if you use an EKF)

What I would suggest for smooth movement is to take the raw gyro values, and interpolate using them. Do not render the cube with the instantaneous gyro reading. Rather, have the gyro reading be an "ideal target value" that you smoothly approach (by using something like exponential smoothing)

One nice way to smooth data like this is to use a damped spring system. Use your current state as the "current angle" of the particle, with the "target angle" being the raw gyro value. Propogate the system using the equations of an ideal, critically damped spring:

$$ \delta \phi = \theta_{current} - \theta_{ideal} \\ \alpha_{t} = -\delta\phi - \omega \\ \omega_{t} = \omega_{t - 1} + \alpha_t . \small \Delta t \\ \theta_{current_{t}} = \theta_{current_{t - 1}} + \omega_t.\Delta t + \frac{1}{2}\alpha_t^2 $$

Here, $\delta \phi = \theta_{current} - \theta_{ideal}$ where $\theta_{current}$ is the current angle of the cube, $\theta_{ideal}$ is the ideal position the cube must have (which is the raw gyro value). $\alpha$ is the angular acceleration, $\omega$ the angular velocity, $\theta$ is the angular position.

You will realize that I am simply propogating the spring equation using Euler integration. IF you wish, you can use a better / more numerically stable algorithm such as RK4

Answer 2

I wanted to add a comment by I do not have sufficient points to do so. I do not know the model of the IMU, but from a quick search seems to be a rather expensive one, so I assume it is fairly accurate (please feel free to contradict me). The fact that the problem is coupled to a software of which you have not shared any line, makes answering more difficult.

From what you are describing you have an fairly large integration error. What integration method are you using?

The highest is the order of the integration the most accurate the estimation, however it becomes more computationally expensive. You can also use a filtering state estimation method such as the Extended Kalman Filter. But you have to use an IMU model and integration method that are suitable. The task is non-trivial and it falls in the category of the state estimation problem. A couple of references are Savage for a classical approach or a modern integration is exposed as part of the overall estimation algorithm by Leutenegger. You have to focus to the attitude estimation part. This is everything I can tell from all the information you gave us.

Cheers

Andrea