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;
}
}
}
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();
}