Converting 9 degrees of freedom data to a quaternion or rotator

Development Programming & Scripting C++
1

I’m attempting to add support for a custom piece of hardware that has an IMU with 9 degrees of freedom, in which I’m trying to convert to a rotator or quaternion. I decided to try implementing the Madgwick’s orientation filter to convert the accelerometer, magnetometer, and gyroscope readings into Unreal world space.

Overall the result is very jittery. Pitch and roll seem mostly okay, but yaw doesn’t appear to function. I’m honestly not sure I’m implementing the algorithm correctly, because it works with a sample of frequencies. For my implementation, I’m calculating the data on every tick and using the DeltaTime as the frequency.

Any ideas? Or does Unreal have some functionality like this already available? I’m also not certain I’m calculating the bias correctly either…

Here is the function I implemented:



FRotator MyClass::GetDeviceRotation(float DeltaTime)
{
    SensorData data;
    FQuat q;
    FRotator rot;
    if (GetData(device, OUT &data) == SUCCESS)
    {
        // Get accelerometer values
        float ax = data.imu_acc[2] * -MilimeterScale; // Scale value is 0.00001f
        float ay = data.imu_acc[0] * MilimeterScale;
        float az = data.imu_acc[1] * MilimeterScale;

        // Get gyro values
        float gx = FMath::DegreesToRadians(data.imu_gyro[0]); // Device gives gyro values in degrees
        float gy = FMath::DegreesToRadians(data.imu_gyro[1]);
        float gz = FMath::DegreesToRadians(data.imu_gyro[2]);

        // Get magnetometer values
        float mx = data.imu_mag[2] * -Sixense_MilimeterScale;
        float my = data.imu_mag[0] * Sixense_MilimeterScale;
        float mz = data.imu_mag[1] * Sixense_MilimeterScale;

        float q1 = q.W, q2 = q.X, q3 = q.Y, q4 = q.Z;   // short name local variable for readability
        float norm;
        float hx, hy, _2bx, _2bz;
        float s1, s2, s3, s4;
        float qDot1, qDot2, qDot3, qDot4;

        float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (shown as 3 deg/s)
        float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta

        // Auxiliary variables to avoid repeated arithmetic
        float _2q1mx;
        float _2q1my;
        float _2q1mz;
        float _2q2mx;
        float _4bx;
        float _4bz;
        float _2q1 = 2.f * q1;
        float _2q2 = 2.f * q2;
        float _2q3 = 2.f * q3;
        float _2q4 = 2.f * q4;
        float _2q1q3 = 2.f * q1 * q3;
        float _2q3q4 = 2.f * q3 * q4;
        float q1q1 = q1 * q1;
        float q1q2 = q1 * q2;
        float q1q3 = q1 * q3;
        float q1q4 = q1 * q4;
        float q2q2 = q2 * q2;
        float q2q3 = q2 * q3;
        float q2q4 = q2 * q4;
        float q3q3 = q3 * q3;
        float q3q4 = q3 * q4;
        float q4q4 = q4 * q4;

        // Normalise accelerometer measurement
        norm = (float)FGenericPlatformMath::Sqrt(ax * ax + ay * ay + az * az);
        if (norm == 0.f) return rot; // handle NaN
        norm = 1 / norm;        // use reciprocal for division
        ax *= norm;
        ay *= norm;
        az *= norm;

        // Normalise magnetometer measurement
        norm = (float)FGenericPlatformMath::Sqrt(mx * mx + my * my + mz * mz);
        if (norm == 0.f) return rot; // handle NaN
        norm = 1 / norm;        // use reciprocal for division
        mx *= norm;
        my *= norm;
        mz *= norm;

        // Reference direction of Earth's magnetic field
        _2q1mx = 2.f * q1 * mx;
        _2q1my = 2.f * q1 * my;
        _2q1mz = 2.f * q1 * mz;
        _2q2mx = 2.f * q2 * mx;
        hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
        hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
        _2bx = (float)FGenericPlatformMath::Sqrt(hx * hx + hy * hy);
        _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
        _4bx = 2.f * _2bx;
        _4bz = 2.f * _2bz;

        // Gradient decent algorithm corrective step
        s1 = -_2q3 * (2.f * q2q4 - _2q1q3 - ax) + _2q2 * (2.f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
        s2 = _2q4 * (2.f * q2q4 - _2q1q3 - ax) + _2q1 * (2.f * q1q2 + _2q3q4 - ay) - 4.f * q2 * (1 - 2.f * q2q2 - 2.f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
        s3 = -_2q1 * (2.f * q2q4 - _2q1q3 - ax) + _2q4 * (2.f * q1q2 + _2q3q4 - ay) - 4.f * q3 * (1 - 2.f * q2q2 - 2.f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
        s4 = _2q2 * (2.f * q2q4 - _2q1q3 - ax) + _2q3 * (2.f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
        norm = 1.f / (float)FGenericPlatformMath::Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
        s1 *= norm;
        s2 *= norm;
        s3 *= norm;
        s4 *= norm;

        // Compute rate of change of quaternion
        qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
        qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
        qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
        qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

        // Integrate to yield quaternion
        q1 += qDot1 * DeltaTime;
        q2 += qDot2 * DeltaTime;
        q3 += qDot3 * DeltaTime;
        q4 += qDot4 * DeltaTime;
        norm = 1.f / (float)FGenericPlatformMath::Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
        q.W = q1 * norm;
        q.X = q2 * norm;
        q.Y = q3 * norm;
        q.Z = q4 * norm;

        rot = FRotator(q);
    }
    return rot;
}

And here’s a small sample of the data for reference:



delta time    : 0.008333
accelerometer : 2098, -110, 20
gyroscope     : -21, 0, 2
magnetometer  : 220, -188, -152

delta time    : 0.008333
accelerometer : 2098, -112, 24
gyroscope     : -21, 0, 2
magnetometer  : 225, -189, -170

delta time    : 0.008334
accelerometer : 2093, -109, 14
gyroscope     : -21, 0, 2
magnetometer  : 225, -189, -170

delta time    : 0.008334
accelerometer : 2089, -111, 26
gyroscope     : -21, 0, 2
magnetometer  : 225, -189, -168

delta time    : 0.008333
accelerometer : 2100, -103, 23
gyroscope     : -21, 0, 2
magnetometer  : 213, -181, -157

delta time    : 0.008333
accelerometer : 2099, -102, 21
gyroscope     : -21, 0, 3
magnetometer  : 213, -181, -157

delta time    : 0.008333
accelerometer : 2094, -96, 23
gyroscope     : -21, 0, 4
magnetometer  : 223, -189, -158

delta time    : 0.008333
accelerometer : 2086, -106, 22
gyroscope     : -21, 0, 4
magnetometer  : 223, -185, -158

delta time    : 0.008334
accelerometer : 2099, -103, 15
gyroscope     : -21, 0, 4
magnetometer  : 223, -185, -158

delta time    : 0.008333
accelerometer : 2097, -106, 31
gyroscope     : -21, 0, 4
magnetometer  : 216, -185, -156

delta time    : 0.008334
accelerometer : 2080, -113, 12
gyroscope     : -21, 0, 4
magnetometer  : 221, -192, -158
2

why is nobody answering these gyro questions ? is it a poisonous topic? I also have some questions.

3

…maybe because nobody knows the answer? It’s a pretty specific question.

4

You have to run a lerp instead of simply replacing rotator inside tick:

5

You likely need reference values to check your math. I suggest creating one with all accelerometer values zero, and all magnometer values at zero and see if things make sense. You should consider commenting out the gradient descent algorithm, because it is way too big and one typo can mess everything up. Good luck!