// This is a simple predictive filter based only on extrapolating the smoothed, current angular velocity.
// Note that both QP (the predicted future orientation) and Q (the current orientation) are both maintained.
Quatf SensorFusion::GetPredictedOrientation()
{
Lock::Locker lockScope(Handler.GetHandlerLock());
Quatf qP = QUncorrected;
if (EnablePrediction) {
#if 1
Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
float angVelFL = angVelF.Length();
if (angVelFL > 0.001f)
{
Vector3f rotAxisP = angVelF / angVelFL;
float halfRotAngleP = angVelFL * PredictionDT * 0.5f;
float sinaHRAP = sin(halfRotAngleP);
Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP,
rotAxisP.z*sinaHRAP, cos(halfRotAngleP));
qP = QUncorrected * deltaQP;
}
#else
Quatd qpd = Quatd(Q.x,Q.y,Q.z,Q.w);
int predictionStages = (int)(PredictionDT / DeltaT);
Vector3f aa = FAngV.SavitzkyGolayDerivative12();
Vector3d aad = Vector3d(aa.x,aa.y,aa.z);
Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
Vector3d avkd = Vector3d(angVelF.x,angVelF.y,angVelF.z);
for (int i = 0; i < predictionStages; i++)
{
double angVelLengthd = avkd.Length();
Vector3d rotAxisd = avkd / angVelLengthd;
double halfRotAngled = angVelLengthd * DeltaT * 0.5;
double sinHRAd = sin(halfRotAngled);
Quatd deltaQd = Quatd(rotAxisd.x*sinHRAd, rotAxisd.y*sinHRAd, rotAxisd.z*sinHRAd,
cos(halfRotAngled));
qpd = qpd * deltaQd;
// Update vel
avkd += aad;
}
qP = Quatf((float)qpd.x,(float)qpd.y,(float)qpd.z,(float)qpd.w);
#endif
}
return qP;
}
// A predictive filter based on extrapolating the smoothed, current angular velocity
Quatf SensorFusion::GetPredictedOrientation(float pdt)
{
Lock::Locker lockScope(Handler.GetHandlerLock());
Quatf qP = Q;
if (EnablePrediction)
{
// This method assumes a constant angular velocity
Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
float angVelFL = angVelF.Length();
// Force back to raw measurement
angVelF = AngV;
angVelFL = AngV.Length();
// Dynamic prediction interval: Based on angular velocity to reduce vibration
const float minPdt = 0.001f;
const float slopePdt = 0.1f;
float newpdt = pdt;
float tpdt = minPdt + slopePdt * angVelFL;
if (tpdt < pdt)
newpdt = tpdt;
//LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f));
if (angVelFL > 0.001f)
{
Vector3f rotAxisP = angVelF / angVelFL;
float halfRotAngleP = angVelFL * newpdt * 0.5f;
float sinaHRAP = sin(halfRotAngleP);
Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP,
rotAxisP.z*sinaHRAP, cos(halfRotAngleP));
qP = Q * deltaQP;
}
}
return qP;
}
