00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
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
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
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
00102 Vector3f angVel = msg.RotationRate;
00103 Vector3f rawAccel = msg.Acceleration;
00104 Vector3f mag = msg.MagneticField;
00105
00106
00107 DeltaT = msg.TimeDelta;
00108 AngV = msg.RotationRate;
00109 AngV.y *= YawMult;
00110 A = rawAccel;
00111
00112
00113 RawMag = mag;
00114
00115
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
00124
00125 CalMag = mag;
00126
00127 float angVelLength = angVel.Length();
00128 float accLength = rawAccel.Length();
00129
00130
00131
00132 Vector3f accWorld = Q.Rotate(rawAccel);
00133
00134
00135 Stage++;
00136 RunningTime += DeltaT;
00137
00138
00139 FRawMag.AddElement(RawMag);
00140 FAccW.AddElement(accWorld);
00141 FAngV.AddElement(angVel);
00142
00143
00144
00145
00146
00147
00148
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
00160
00161 if (Stage % 5000 == 0)
00162 Q.Normalize();
00163
00164
00165 QUncorrected = Q;
00166
00167
00168
00169
00170 if (EnableGravity)
00171 {
00172
00173 const float gravityEpsilon = 0.4f;
00174 const float angVelEpsilon = 0.1f;
00175 const int tiltPeriod = 50;
00176 const float maxTiltError = 0.05f;
00177 const float minTiltError = 0.01f;
00178
00179
00180
00181
00182 if ((fabs(accLength - 9.81f) < gravityEpsilon) &&
00183 (angVelLength < angVelEpsilon))
00184 TiltCondCount++;
00185 else
00186 TiltCondCount = 0;
00187
00188
00189
00190 if (TiltCondCount >= tiltPeriod)
00191 {
00192 TiltCondCount = 0;
00193
00194 Vector3f accWMean = FAccW.Mean();
00195
00196 Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z);
00197
00198 Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized();
00199 Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f);
00200
00201 float tiltAngle = yUp.Angle(accWMean);
00202
00203 if (tiltAngle > maxTiltError)
00204 {
00205 TiltErrorAngle = tiltAngle;
00206 TiltErrorAxis = tiltAxis;
00207 }
00208 }
00209
00210
00211 if (TiltErrorAngle > minTiltError)
00212 {
00213 if ((TiltErrorAngle > 0.4f)&&(RunningTime < 8.0f))
00214 {
00215 Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q;
00216 TiltErrorAngle = 0.0f;
00217 }
00218 else
00219 {
00220
00221
00222
00223
00224 float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f);
00225
00226 Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q;
00227 TiltErrorAngle += deltaTiltAngle;
00228 }
00229 }
00230 }
00231
00232
00233
00234
00235
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
00249
00250 if ((EnableYawCorrection && MagCalibrated)&&(RunningTime > 10.0f)&&(TiltErrorAngle < 0.2f))
00251 {
00252 if (MagNumReferences == 0)
00253 {
00254 SetMagReference();
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
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
00290 Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM));
00291
00292 Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean()));
00293
00294 Anglef gryaw = Anglef(atan2(grefmag.x,grefmag.z));
00295
00296 Anglef gyaw = Anglef(atan2(gmag.x,gmag.z));
00297
00298 Anglef YawErrorAngle = gyaw - gryaw;
00299
00300
00301
00302 if ((YawErrorAngle.Abs() > yawErrorMax) && (!YawCorrectionActivated))
00303 YawErrorCount++;
00304
00305 if (YawErrorCount > yawErrorCountLimit)
00306 YawCorrectionActivated = true;
00307
00308 if ((YawErrorAngle.Abs() < yawErrorMin) && YawCorrectionActivated)
00309 {
00310 YawCorrectionActivated = false;
00311 YawErrorCount = 0;
00312 }
00313
00314
00315 if (YawCorrectionActivated)
00316 {
00317 YawCorrectionInProgress = true;
00318
00319 Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * YawErrorAngle.Sign()) * Q;
00320 }
00321 }
00322 }
00323
00324
00325
00326
00327
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
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
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
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;
00396
00397
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 }
00435