How use raw Gryoscope Data °/s for calculating 3D rotation?

Question

My question may seem trivial, but the more I read about it - the more confused I get... I have started a little project where I want to roughly track the movements of a rotating object. (A basketball to be precise) I have a 3-axis accelerometer (low-pass-filtered) and a 3-axis gyroscope measuring °/s. I know about the issues of a gyro, but as the measurements will only be several seconds and the angles tend to be huge - I don't care about drift and gimbal right now.

My Gyro gives me the rotation speed of all 3 axis. As I want to integrate the acceleration twice to get the position at each timestep, I wanted to convert the sensors coordinate-system into an earthbound system. For the first try, I want to keep things simple, so I decided to go with the big standard rotation matrix. But as my results are horrible I wonder if this is the right way to do so. If I understood correctly - the matrix is simply 3 matrices multiplied in a certain order. As rotation of a basketball doesn't have any "natural" order, this may not be a good idea. My sensor measures 3 angular velocitys at once. If I throw them into my system "step by step" it will not be correct since my second matrix calculates the rotation around the "new y-axis" , but my sensor actually measured an angular velocity around the "old y-axis". Is that correct so far?

So how can I correctly calculate the 3D rotation? Do I need to go for quaternoins? but how do I get one from 3 different rotations? And don't I have the same issue here again?

I start with a unity-matrix ((1, 0, 0)(0, 1, 0)(0, 0, 1)) multiplied with the acceleration vector to give me the first movement. Then I want use the Rotation matrix to find out, where the next acceleration is really heading so I can simply add the accelerations together.

But right now I am just too confused to find a proper way.

Any suggestions? btw. sorry for my poor english, I am tired and (obviously) not a native speaker ;)

Thanks, Alex

Millrace

Answer 1

Short answer

Yes, go for quaternions and use a first order linearization of the rotation to calculate how orientation changes. This reduces to the following pseudocode:

    float pose_initial[4]; // quaternion describing original orientation
    float g_x, g_y, g_z; // gyro rates
    float dt; // time step. The smaller the better.

    // quaternion with "pose increment", calculated from the first-order 
    // linearization of continuous rotation formula
    delta_quat = {1, 0.5*dt*g_x, 0.5*dt*g_y, 0.5*dt*g_z};
    // final orientation at start time + dt
    pose_final = quaternion_hamilton_product(pose_initial, delta_quat);

This solution is used in PixHawk's EKF navigation filter (it is open source, check out formulation here). It is simple, cheap, stable and accurate enough.

Unit matrix (describing a "null" rotation) is equivalent to quaternion [1 0 0 0]. You can get the quaternion describing other poses using a suitable conversion formula (for example, if you have Euler angles you can go for this one).

Notes:

• Quaternions following [w, i, j, k] notation.
• These equations assume angular speeds in SI units, this is, radians per second.

Long answer

A gyroscope describes the rotational speed of an object as a decomposition in three rotational speeds around the orthogonal local axes XYZ. However, you could equivalently describe the rotational speed as a single rate around a certain axis --either in reference system that is local to the rotated body or in a global one.

The three rotational speeds affect the body simultaneously, continously changing the rotation axis.

Here we have the problem of switching from the continuous-time real world to a simpler discrete-time formulation that can be easily solved using a computer. When discretizing, we are always going to introduce errors. Some approaches will lead to bigger errors, while others will be notably more accurate.

Your approach of concatenating three simultaneous rotations around orthogonal axes work reasonably well with small integration steps (let's say smaller than 1/1000 s, although it depends on the application), so that you are simulating the continuous change of rotation axis. However, this is computationally expensive, and error grows as you make time steps bigger.

As an alternative to first-order linearization, you can calculate pose increments as a small delta of angular speed gradient (also using quaternion representation):

    quat_gyro = {0, g_x, g_y, g_z};
    q_grad    = 0.5 * quaternion_product(pose_initial, quat_gyro);
    // Important to normalize result to get unit quaternion!
    pose_final = quaternion_normalize(pose_initial + q_grad*dt); 

This technique is used in Madgwick rotation filter (here an implementation), and works pretty fine for me.

Concentrated

Sorry for my poor math. Is this quaternion_product Hamilton product? Thanks. – Bonnard

Tested and yes, it is Hamilton product. – Bonnard