void SensorFusion::handleMessage(const MessageBodyFrame& msg)
{
    if (msg.Type != Message_BodyFrame)
        return;
  
    // Put the sensor readings into convenient local variables
    Vector3f angVel    = msg.RotationRate; 
    Vector3f rawAccel  = msg.Acceleration;
    Vector3f mag       = msg.MagneticField;

    // Set variables accessible through the class API
	DeltaT = msg.TimeDelta;
    AngV = msg.RotationRate;
    AngV.y *= YawMult;  // Warning: If YawMult != 1, then AngV is not true angular velocity
    A = rawAccel;

    // Allow external access to uncalibrated magnetometer values
    RawMag = mag;  

    // Apply the calibration parameters to raw mag
    if (HasMagCalibration())
    {
        mag.x += MagCalibrationMatrix.M[0][3];
        mag.y += MagCalibrationMatrix.M[1][3];
        mag.z += MagCalibrationMatrix.M[2][3];
    }

    // Provide external access to calibrated mag values
    // (if the mag is not calibrated, then the raw value is returned)
    CalMag = mag;

    float angVelLength = angVel.Length();
    float accLength    = rawAccel.Length();


    // Acceleration in the world frame (Q is current HMD orientation)
    Vector3f accWorld  = Q.Rotate(rawAccel);

    // Keep track of time
    Stage++;
    float currentTime  = Stage * DeltaT; // Assumes uniform time spacing

    // Insert current sensor data into filter history
    FRawMag.AddElement(RawMag);
    FAccW.AddElement(accWorld);
    FAngV.AddElement(angVel);

    // Update orientation Q based on gyro outputs.  This technique is
    // based on direct properties of the angular velocity vector:
    // Its direction is the current rotation axis, and its magnitude
    // is the rotation rate (rad/sec) about that axis.  Our sensor
    // sampling rate is so fast that we need not worry about integral
    // approximation error (not yet, anyway).
    if (angVelLength > 0.0f)
    {
        Vector3f     rotAxis      = angVel / angVelLength;  
        float        halfRotAngle = angVelLength * DeltaT * 0.5f;
        float        sinHRA       = sin(halfRotAngle);
        Quatf        deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle));

        Q =  Q * deltaQ;
    }
    
    // The quaternion magnitude may slowly drift due to numerical error,
    // so it is periodically normalized.
    if (Stage % 5000 == 0)
        Q.Normalize();
    
	// Maintain the uncorrected orientation for later use by predictive filtering
	QUncorrected = Q;

    // Perform tilt correction using the accelerometer data. This enables 
    // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected
    // because the rotation axis is parallel to the gravity vector.
    if (EnableGravity)
    {
        // Correcting for tilt error by using accelerometer data
        const float  gravityEpsilon = 0.4f;
        const float  angVelEpsilon  = 0.1f; // Relatively slow rotation
        const int    tiltPeriod     = 50;   // Req'd time steps of stability
        const float  maxTiltError   = 0.05f;
        const float  minTiltError   = 0.01f;

        // This condition estimates whether the only measured acceleration is due to gravity 
        // (the Rift is not linearly accelerating).  It is often wrong, but tends to average
        // out well over time.
        if ((fabs(accLength - 9.81f) < gravityEpsilon) &&
            (angVelLength < angVelEpsilon))
            TiltCondCount++;
        else
            TiltCondCount = 0;
    
        // After stable measurements have been taken over a sufficiently long period,
        // estimate the amount of tilt error and calculate the tilt axis for later correction.
        if (TiltCondCount >= tiltPeriod)
        {   // Update TiltErrorEstimate
            TiltCondCount = 0;
            // Use an average value to reduce noice (could alternatively use an LPF)
            Vector3f accWMean = FAccW.Mean();
            // Project the acceleration vector into the XZ plane
            Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z);
            // The unit normal of xzAcc will be the rotation axis for tilt correction
            Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized();
            Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f);
            // This is the amount of rotation
            float    tiltAngle = yUp.Angle(accWMean);
            // Record values if the tilt error is intolerable
            if (tiltAngle > maxTiltError) 
            {
                TiltErrorAngle = tiltAngle;
                TiltErrorAxis = tiltAxis;
            }
        }

        // This part performs the actual tilt correction as needed
        if (TiltErrorAngle > minTiltError) 
        {
            if ((TiltErrorAngle > 0.4f)&&(Stage < 8000))
            {   // Tilt completely to correct orientation
                Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q;
                TiltErrorAngle = 0.0f;
            }
            else 
            {
                //LogText("Performing tilt correction  -  Angle: %f   Axis: %f %f %f\n",
                //        TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z);
                //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f;
                // This uses agressive correction steps while your head is moving fast
                float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f);
                // Incrementally "untilt" by a small step size
                Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q;
                TiltErrorAngle += deltaTiltAngle;
            }
        }
    }

    // Yaw drift correction based on magnetometer data.  This corrects the part of the drift
    // that the accelerometer cannot handle.
    // This will only work if the magnetometer has been enabled, calibrated, and a reference
    // point has been set.
    const float maxAngVelLength = 3.0f;
    const int   magWindow = 5;
    const float yawErrorMax = 0.1f;
    const float yawErrorMin = 0.01f;
    const int   yawErrorCountLimit = 50;
    const float yawRotationStep = 0.00002f;

    if (angVelLength < maxAngVelLength)
        MagCondCount++;
    else
        MagCondCount = 0;

	YawCorrectionInProgress = false;
    if (EnableYawCorrection && MagReady && (currentTime > 2.0f) && (MagCondCount >= magWindow) &&
        (Q.Distance(MagRefQ) < MagRefDistance))
    {
        // Use rotational invariance to bring reference mag value into global frame
        Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM));
        // Bring current (averaged) mag reading into global frame
        Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean()));
        // Calculate the reference yaw in the global frame
        float gryaw = atan2(grefmag.x,grefmag.z);
        // Calculate the current yaw in the global frame
        float gyaw = atan2(gmag.x,gmag.z);
        //LogText("Yaw error estimate: %f\n",YawErrorAngle);
        // The difference between reference and current yaws is the perceived error
        YawErrorAngle = AngleDifference(gyaw,gryaw);
        // If the perceived error is large, keep count
        if ((fabs(YawErrorAngle) > yawErrorMax) && (!YawCorrectionActivated))
            YawErrorCount++;
        // After enough iterations of high perceived error, start the correction process
        if (YawErrorCount > yawErrorCountLimit)
            YawCorrectionActivated = true;
        // If the perceived error becomes small, turn off the yaw correction
        if ((fabs(YawErrorAngle) < yawErrorMin) && YawCorrectionActivated) 
        {
            YawCorrectionActivated = false;
            YawErrorCount = 0;
        }
        // Perform the actual yaw correction, due to previously detected, large yaw error
        if (YawCorrectionActivated) 
        {
			YawCorrectionInProgress = true;
            int sign = (YawErrorAngle > 0.0f) ? 1 : -1;
            // Incrementally "unyaw" by a small step size
            Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * sign) * Q;
        }
    }
}
Example #2
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();
}