Announcement

Collapse
No announcement yet.

Converting 9 degrees of freedom data to a quaternion or rotator

Collapse
X
 
  • Filter
  • Time
  • Show
Clear All
new posts

    Converting 9 degrees of freedom data to a quaternion or rotator

    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:

    Code:
    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:
    Code:
    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.

    Comment


      #3
      Originally posted by aerotekk3000 View Post
      why is nobody answering these gyro questions ? is it a poisonous topic? I also have some questions.
      ...maybe because nobody knows the answer? It's a pretty specific question.

      Comment


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

        https://api.unrealengine.com/INT/API...p/1/index.html
        | Savior | USQLite | FSM | Object Pool | Sound Occlusion | Property Transfer | Magic Nodes | MORE |

        Comment


          #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!

          Comment

          Working...
          X