Example #1
0
void SensorFusion::handleMessage(const MessageBodyFrame& msg)
{
    if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled())
        return;

    // Put the sensor readings into convenient local variables
    Vector3f gyro  = msg.RotationRate; 
    Vector3f accel = msg.Acceleration;
    Vector3f mag   = msg.MagneticField;

    // Insert current sensor data into filter history
    FRawMag.AddElement(mag);
    FAngV.AddElement(gyro);

    // Apply the calibration parameters to raw mag
    Vector3f calMag = MagCalibrated ? GetCalibratedMagValue(FRawMag.Mean()) : FRawMag.Mean();

    // Set variables accessible through the class API
    DeltaT = msg.TimeDelta;
    AngV   = gyro;
    A      = accel;
    RawMag = mag;  
    CalMag = calMag;

    // Keep track of time
    Stage++;
    RunningTime += DeltaT;

    // Small preprocessing
    Quatf Qinv = Q.Inverted();
    Vector3f up = Qinv.Rotate(Vector3f(0, 1, 0));

    Vector3f gyroCorrected = gyro;

    // Apply integral term
    // All the corrections are stored in the Simultaneous Orthogonal Rotations Angle representation,
    // which allows to combine and scale them by just addition and multiplication
    if (EnableGravity || EnableYawCorrection)
        gyroCorrected -= GyroOffset;

    if (EnableGravity)
    {
        const float spikeThreshold = 0.01f;
        const float gravityThreshold = 0.1f;
        float proportionalGain     = 5 * Gain; // Gain parameter should be removed in a future release
        float integralGain         = 0.0125f;

        Vector3f tiltCorrection = SensorFusion_ComputeCorrection(accel, up);

        if (Stage > 5)
        {
            // Spike detection
            float tiltAngle = up.Angle(accel);
            TiltAngleFilter.AddElement(tiltAngle);
            if (tiltAngle > TiltAngleFilter.Mean() + spikeThreshold)
                proportionalGain = integralGain = 0;
            // Acceleration detection
            const float gravity = 9.8f;
            if (fabs(accel.Length() / gravity - 1) > gravityThreshold)
                integralGain = 0;
        }
        else // Apply full correction at the startup
        {
            proportionalGain = 1 / DeltaT;
            integralGain = 0;
        }

        gyroCorrected += (tiltCorrection * proportionalGain);
        GyroOffset -= (tiltCorrection * integralGain * DeltaT);
    }

    if (EnableYawCorrection && MagCalibrated && RunningTime > 2.0f)
    {
        const float maxMagRefDist = 0.1f;
        const float maxTiltError = 0.05f;
        float proportionalGain   = 0.01f;
        float integralGain       = 0.0005f;

        // Update the reference point if needed
        if (MagRefIdx < 0 || calMag.Distance(MagRefsInBodyFrame[MagRefIdx]) > maxMagRefDist)
        {
            // Delete a bad point
            if (MagRefIdx >= 0 && MagRefScore < 0)
            {
                MagNumReferences--;
                MagRefsInBodyFrame[MagRefIdx] = MagRefsInBodyFrame[MagNumReferences];
                MagRefsInWorldFrame[MagRefIdx] = MagRefsInWorldFrame[MagNumReferences];
            }
            // Find a new one
            MagRefIdx = -1;
            MagRefScore = 1000;
            float bestDist = maxMagRefDist;
            for (int i = 0; i < MagNumReferences; i++)
            {
                float dist = calMag.Distance(MagRefsInBodyFrame[i]);
                if (bestDist > dist)
                {
                    bestDist = dist;
                    MagRefIdx = i;
                }
            }
            // Create one if needed
            if (MagRefIdx < 0 && MagNumReferences < MagMaxReferences)
            {
                MagRefIdx = MagNumReferences;
                MagRefsInBodyFrame[MagRefIdx] = calMag;
                MagRefsInWorldFrame[MagRefIdx] = Q.Rotate(calMag).Normalized();
                MagNumReferences++;
            }
        }

        if (MagRefIdx >= 0)
        {
            Vector3f magEstimated = Qinv.Rotate(MagRefsInWorldFrame[MagRefIdx]);
            Vector3f magMeasured  = calMag.Normalized();

            // Correction is computed in the horizontal plane (in the world frame)
            Vector3f yawCorrection = SensorFusion_ComputeCorrection(magMeasured.ProjectToPlane(up), 
                                                                    magEstimated.ProjectToPlane(up));

            if (fabs(up.Dot(magEstimated - magMeasured)) < maxTiltError)
            {
                MagRefScore += 2;
            }
            else // If the vertical angle is wrong, decrease the score
            {
                MagRefScore -= 1;
                proportionalGain = integralGain = 0;
            }
            gyroCorrected += (yawCorrection * proportionalGain);
            GyroOffset -= (yawCorrection * integralGain * DeltaT);
        }
    }

    // Update the orientation quaternion based on the corrected angular velocity vector
    Q = Q * Quatf(gyroCorrected, gyroCorrected.Length() * DeltaT);

    // The quaternion magnitude may slowly drift due to numerical error,
    // so it is periodically normalized.
    if (Stage % 500 == 0)
        Q.Normalize();
}