OVR_SensorFusion.cpp
Go to the documentation of this file.
00001 /************************************************************************************
00002 
00003 Filename    :   OVR_SensorFusion.cpp
00004 Content     :   Methods that determine head orientation from sensor data over time
00005 Created     :   October 9, 2012
00006 Authors     :   Michael Antonov, Steve LaValle
00007 
00008 Copyright   :   Copyright 2012 Oculus VR, Inc. All Rights reserved.
00009 
00010 Use of this software is subject to the terms of the Oculus license
00011 agreement provided at the time of installation or download, or which
00012 otherwise accompanies this software in either electronic or hard copy form.
00013 
00014 *************************************************************************************/
00015 
00016 #include "OVR_SensorFusion.h"
00017 #include "Kernel/OVR_Log.h"
00018 #include "Kernel/OVR_System.h"
00019 
00020 namespace OVR {
00021 
00022 //-------------------------------------------------------------------------------------
00023 // ***** Sensor Fusion
00024 
00025 SensorFusion::SensorFusion(SensorDevice* sensor)
00026   : Handler(getThis()), pDelegate(0),
00027     Gain(0.05f), YawMult(1), EnableGravity(true), Stage(0), RunningTime(0), DeltaT(0.001f),
00028         EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f),
00029     FRawMag(10), FAccW(20), FAngV(20),
00030     TiltCondCount(0), TiltErrorAngle(0), 
00031     TiltErrorAxis(0,1,0),
00032     MagCondCount(0), MagCalibrated(false), MagRefQ(0, 0, 0, 1), 
00033         MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.5f),
00034     YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false), 
00035         EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false)
00036 {
00037    if (sensor)
00038        AttachToSensor(sensor);
00039    MagCalibrationMatrix.SetIdentity();
00040 }
00041 
00042 SensorFusion::~SensorFusion()
00043 {
00044 }
00045 
00046 
00047 bool SensorFusion::AttachToSensor(SensorDevice* sensor)
00048 {
00049     
00050     if (sensor != NULL)
00051     {
00052         MessageHandler* pCurrentHandler = sensor->GetMessageHandler();
00053 
00054         if (pCurrentHandler == &Handler)
00055         {
00056             Reset();
00057             return true;
00058         }
00059 
00060         if (pCurrentHandler != NULL)
00061         {
00062             OVR_DEBUG_LOG(
00063                 ("SensorFusion::AttachToSensor failed - sensor %p already has handler", sensor));
00064             return false;
00065         }
00066     }
00067 
00068     if (Handler.IsHandlerInstalled())
00069     {
00070         Handler.RemoveHandlerFromDevices();
00071     }
00072 
00073     if (sensor != NULL)
00074     {
00075         sensor->SetMessageHandler(&Handler);
00076     }
00077 
00078     Reset();
00079     return true;
00080 }
00081 
00082 
00083     // Resets the current orientation
00084 void SensorFusion::Reset()
00085 {
00086     Lock::Locker lockScope(Handler.GetHandlerLock());
00087     Q                     = Quatf();
00088     QUncorrected          = Quatf();
00089     Stage                 = 0;
00090         RunningTime           = 0;
00091         MagNumReferences      = 0;
00092         MagHasNearbyReference = false;
00093 }
00094 
00095 
00096 void SensorFusion::handleMessage(const MessageBodyFrame& msg)
00097 {
00098     if (msg.Type != Message_BodyFrame)
00099         return;
00100   
00101     // Put the sensor readings into convenient local variables
00102     Vector3f angVel    = msg.RotationRate; 
00103     Vector3f rawAccel  = msg.Acceleration;
00104     Vector3f mag       = msg.MagneticField;
00105 
00106     // Set variables accessible through the class API
00107         DeltaT = msg.TimeDelta;
00108     AngV = msg.RotationRate;
00109     AngV.y *= YawMult;  // Warning: If YawMult != 1, then AngV is not true angular velocity
00110     A = rawAccel;
00111 
00112     // Allow external access to uncalibrated magnetometer values
00113     RawMag = mag;  
00114 
00115     // Apply the calibration parameters to raw mag
00116     if (HasMagCalibration())
00117     {
00118         mag.x += MagCalibrationMatrix.M[0][3];
00119         mag.y += MagCalibrationMatrix.M[1][3];
00120         mag.z += MagCalibrationMatrix.M[2][3];
00121     }
00122 
00123     // Provide external access to calibrated mag values
00124     // (if the mag is not calibrated, then the raw value is returned)
00125     CalMag = mag;
00126 
00127     float angVelLength = angVel.Length();
00128     float accLength    = rawAccel.Length();
00129 
00130 
00131     // Acceleration in the world frame (Q is current HMD orientation)
00132     Vector3f accWorld  = Q.Rotate(rawAccel);
00133 
00134     // Keep track of time
00135     Stage++;
00136     RunningTime += DeltaT;
00137 
00138     // Insert current sensor data into filter history
00139     FRawMag.AddElement(RawMag);
00140     FAccW.AddElement(accWorld);
00141     FAngV.AddElement(angVel);
00142 
00143     // Update orientation Q based on gyro outputs.  This technique is
00144     // based on direct properties of the angular velocity vector:
00145     // Its direction is the current rotation axis, and its magnitude
00146     // is the rotation rate (rad/sec) about that axis.  Our sensor
00147     // sampling rate is so fast that we need not worry about integral
00148     // approximation error (not yet, anyway).
00149     if (angVelLength > 0.0f)
00150     {
00151         Vector3f     rotAxis      = angVel / angVelLength;  
00152         float        halfRotAngle = angVelLength * DeltaT * 0.5f;
00153         float        sinHRA       = sin(halfRotAngle);
00154         Quatf        deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle));
00155 
00156         Q =  Q * deltaQ;
00157     }
00158     
00159     // The quaternion magnitude may slowly drift due to numerical error,
00160     // so it is periodically normalized.
00161     if (Stage % 5000 == 0)
00162         Q.Normalize();
00163     
00164         // Maintain the uncorrected orientation for later use by predictive filtering
00165         QUncorrected = Q;
00166 
00167     // Perform tilt correction using the accelerometer data. This enables 
00168     // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected
00169     // because the rotation axis is parallel to the gravity vector.
00170     if (EnableGravity)
00171     {
00172         // Correcting for tilt error by using accelerometer data
00173         const float  gravityEpsilon = 0.4f;
00174         const float  angVelEpsilon  = 0.1f; // Relatively slow rotation
00175         const int    tiltPeriod     = 50;   // Required time steps of stability
00176         const float  maxTiltError   = 0.05f;
00177         const float  minTiltError   = 0.01f;
00178 
00179         // This condition estimates whether the only measured acceleration is due to gravity 
00180         // (the Rift is not linearly accelerating).  It is often wrong, but tends to average
00181         // out well over time.
00182         if ((fabs(accLength - 9.81f) < gravityEpsilon) &&
00183             (angVelLength < angVelEpsilon))
00184             TiltCondCount++;
00185         else
00186             TiltCondCount = 0;
00187     
00188         // After stable measurements have been taken over a sufficiently long period,
00189         // estimate the amount of tilt error and calculate the tilt axis for later correction.
00190         if (TiltCondCount >= tiltPeriod)
00191         {   // Update TiltErrorEstimate
00192             TiltCondCount = 0;
00193             // Use an average value to reduce noise (could alternatively use an LPF)
00194             Vector3f accWMean = FAccW.Mean();
00195             // Project the acceleration vector into the XZ plane
00196             Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z);
00197             // The unit normal of xzAcc will be the rotation axis for tilt correction
00198             Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized();
00199             Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f);
00200             // This is the amount of rotation
00201             float    tiltAngle = yUp.Angle(accWMean);
00202             // Record values if the tilt error is intolerable
00203             if (tiltAngle > maxTiltError) 
00204             {
00205                 TiltErrorAngle = tiltAngle;
00206                 TiltErrorAxis = tiltAxis;
00207             }
00208         }
00209 
00210         // This part performs the actual tilt correction as needed
00211         if (TiltErrorAngle > minTiltError) 
00212         {
00213             if ((TiltErrorAngle > 0.4f)&&(RunningTime < 8.0f))
00214             {   // Tilt completely to correct orientation
00215                 Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q;
00216                 TiltErrorAngle = 0.0f;
00217             }
00218             else 
00219             {
00220                 //LogText("Performing tilt correction  -  Angle: %f   Axis: %f %f %f\n",
00221                 //        TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z);
00222                 //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f;
00223                 // This uses aggressive correction steps while your head is moving fast
00224                 float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f);
00225                 // Incrementally "un-tilt" by a small step size
00226                 Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q;
00227                 TiltErrorAngle += deltaTiltAngle;
00228             }
00229         }
00230     }
00231 
00232     // Yaw drift correction based on magnetometer data.  This corrects the part of the drift
00233     // that the accelerometer cannot handle.
00234     // This will only work if the magnetometer has been enabled, calibrated, and a reference
00235     // point has been set.
00236     const float maxAngVelLength = 3.0f;
00237     const int   magWindow = 5;
00238     const float yawErrorMax = 0.1f;
00239     const float yawErrorMin = 0.01f;
00240     const int   yawErrorCountLimit = 50;
00241     const float yawRotationStep = 0.00002f;
00242 
00243     if (angVelLength < maxAngVelLength)
00244         MagCondCount++;
00245     else
00246         MagCondCount = 0;
00247 
00248         // Find, create, and utilize reference points for the magnetometer
00249         // Need to be careful not to set reference points while there is significant tilt error
00250     if ((EnableYawCorrection && MagCalibrated)&&(RunningTime > 10.0f)&&(TiltErrorAngle < 0.2f))
00251         {
00252           if (MagNumReferences == 0)
00253       {
00254                   SetMagReference(); // Use the current direction
00255       }
00256           else if (Q.Distance(MagRefQ) > MagRefDistance) 
00257       {
00258                   MagHasNearbyReference = false;
00259           float bestDist = 100000.0f;
00260           int bestNdx = 0;
00261           float dist;
00262           for (int i = 0; i < MagNumReferences; i++)
00263           {
00264               dist = Q.Distance(MagRefTableQ[i]);
00265               if (dist < bestDist)
00266               {
00267                   bestNdx = i;
00268                   bestDist = dist;
00269               }
00270           }
00271 
00272           if (bestDist < MagRefDistance)
00273           {
00274               MagHasNearbyReference = true;
00275               MagRefQ   = MagRefTableQ[bestNdx];
00276               MagRefM   = MagRefTableM[bestNdx];
00277               MagRefYaw = MagRefTableYaw[bestNdx];
00278               //LogText("Using reference %d\n",bestNdx);
00279           }
00280           else if (MagNumReferences < MagMaxReferences)
00281               SetMagReference();
00282           }
00283         }
00284 
00285         YawCorrectionInProgress = false;
00286     if (EnableYawCorrection && MagCalibrated && (RunningTime > 2.0f) && (MagCondCount >= magWindow) &&
00287         MagHasNearbyReference)
00288     {
00289         // Use rotational invariance to bring reference mag value into global frame
00290         Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM));
00291         // Bring current (averaged) mag reading into global frame
00292         Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean()));
00293         // Calculate the reference yaw in the global frame
00294         Anglef gryaw = Anglef(atan2(grefmag.x,grefmag.z));
00295         // Calculate the current yaw in the global frame
00296         Anglef gyaw = Anglef(atan2(gmag.x,gmag.z));
00297         // The difference between reference and current yaws is the perceived error
00298         Anglef YawErrorAngle = gyaw - gryaw;
00299 
00300         //LogText("Yaw error estimate: %f\n",YawErrorAngle.Get());
00301         // If the perceived error is large, keep count
00302         if ((YawErrorAngle.Abs() > yawErrorMax) && (!YawCorrectionActivated))
00303             YawErrorCount++;
00304         // After enough iterations of high perceived error, start the correction process
00305         if (YawErrorCount > yawErrorCountLimit)
00306             YawCorrectionActivated = true;
00307         // If the perceived error becomes small, turn off the yaw correction
00308         if ((YawErrorAngle.Abs() < yawErrorMin) && YawCorrectionActivated) 
00309         {
00310             YawCorrectionActivated = false;
00311             YawErrorCount = 0;
00312         }
00313         
00314         // Perform the actual yaw correction, due to previously detected, large yaw error
00315         if (YawCorrectionActivated) 
00316         {
00317                         YawCorrectionInProgress = true;
00318             // Incrementally "unyaw" by a small step size
00319             Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * YawErrorAngle.Sign()) * Q;
00320         }
00321     }
00322 }
00323 
00324  
00325 //  Simple predictive filters based on extrapolating the smoothed, current angular velocity
00326 // or using smooth time derivative information.  The argument is the amount of time into
00327 // the future to predict.
00328 Quatf SensorFusion::GetPredictedOrientation(float pdt)
00329 {               
00330         Lock::Locker lockScope(Handler.GetHandlerLock());
00331         Quatf        qP = QUncorrected;
00332         
00333     if (EnablePrediction)
00334     {
00335 #if 1
00336                 // This method assumes a constant angular velocity
00337             Vector3f angVelF  = FAngV.SavitzkyGolaySmooth8();
00338         float    angVelFL = angVelF.Length();
00339             
00340         if (angVelFL > 0.001f)
00341         {
00342             Vector3f    rotAxisP      = angVelF / angVelFL;  
00343             float       halfRotAngleP = angVelFL * pdt * 0.5f;
00344             float       sinaHRAP      = sin(halfRotAngleP);
00345                     Quatf       deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP,
00346                                 rotAxisP.z*sinaHRAP, cos(halfRotAngleP));
00347             qP = QUncorrected * deltaQP;
00348         }
00349 #else
00350                 // This method estimates angular acceleration, conservatively
00351                 OVR_ASSERT(pdt >= 0);
00352         int       predictionStages = (int)(pdt / PredictionTimeIncrement + 0.5f);
00353         Quatd     qpd        = Quatd(Q.x,Q.y,Q.z,Q.w);
00354         Vector3f  aa         = FAngV.SavitzkyGolayDerivative12();
00355         Vector3d  aad        = Vector3d(aa.x,aa.y,aa.z);
00356         Vector3f  angVelF    = FAngV.SavitzkyGolaySmooth8();
00357         Vector3d  avkd       = Vector3d(angVelF.x,angVelF.y,angVelF.z);
00358                 Vector3d  rotAxisd   = Vector3d(0,1,0);
00359         for (int i = 0; i < predictionStages; i++)
00360         {
00361             double   angVelLengthd = avkd.Length();
00362                         if (angVelLengthd > 0)
00363                 rotAxisd = avkd / angVelLengthd;
00364             double   halfRotAngled = angVelLengthd * PredictionTimeIncrement * 0.5;
00365             double   sinHRAd       = sin(halfRotAngled);
00366             Quatd    deltaQd       = Quatd(rotAxisd.x*sinHRAd, rotAxisd.y*sinHRAd, rotAxisd.z*sinHRAd,
00367                                            cos(halfRotAngled));
00368             qpd = qpd * deltaQd;
00369             // Update angular velocity by using the angular acceleration estimate
00370             avkd += aad;
00371         }
00372         qP = Quatf((float)qpd.x,(float)qpd.y,(float)qpd.z,(float)qpd.w);
00373 #endif
00374         }
00375     return qP;
00376 }    
00377 
00378 
00379 Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const
00380 {
00381     Vector3f mag = rawMag;
00382     OVR_ASSERT(HasMagCalibration());
00383     mag.x += MagCalibrationMatrix.M[0][3];
00384     mag.y += MagCalibrationMatrix.M[1][3];
00385     mag.z += MagCalibrationMatrix.M[2][3];
00386     return mag;
00387 }
00388 
00389 
00390 void SensorFusion::SetMagReference(const Quatf& q, const Vector3f& rawMag) 
00391 {
00392     if (MagNumReferences < MagMaxReferences)
00393     {
00394         MagRefTableQ[MagNumReferences] = q;
00395         MagRefTableM[MagNumReferences] = rawMag; //FRawMag.Mean();
00396 
00397                 //LogText("Inserting reference %d\n",MagNumReferences);
00398         
00399                 MagRefQ   = q;
00400         MagRefM   = rawMag;
00401 
00402         float pitch, roll, yaw;
00403                 Quatf q2 = q;
00404         q2.GetEulerAngles<Axis_X, Axis_Z, Axis_Y>(&pitch, &roll, &yaw);
00405         MagRefTableYaw[MagNumReferences] = yaw;
00406                 MagRefYaw = yaw;
00407 
00408         MagNumReferences++;
00409 
00410         MagHasNearbyReference = true;
00411     }
00412 }
00413 
00414 
00415 SensorFusion::BodyFrameHandler::~BodyFrameHandler()
00416 {
00417     RemoveHandlerFromDevices();
00418 }
00419 
00420 void SensorFusion::BodyFrameHandler::OnMessage(const Message& msg)
00421 {
00422     if (msg.Type == Message_BodyFrame)
00423         pFusion->handleMessage(static_cast<const MessageBodyFrame&>(msg));
00424     if (pFusion->pDelegate)
00425         pFusion->pDelegate->OnMessage(msg);
00426 }
00427 
00428 bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const
00429 {
00430     return (type == Message_BodyFrame);
00431 }
00432 
00433 
00434 } // namespace OVR
00435 


oculus_sdk
Author(s): Tully Foote
autogenerated on Thu Jun 6 2019 20:13:48