private void Update()
{
if (!EnableMagneticField && !EnableAcceleration)
{
return;
}
// calculate rotation delta
Quaternion rotationDelta = StateQ.Conjugate() * MeasuredQPose;
rotationDelta.Normalize();
// take it to the power (0 to 1) to give the desired amount of correction
double theta = Math.Acos(rotationDelta.Scalar);
double sinPowerTheta = Math.Sin(theta * SlerpPower);
double cosPowerTheta = Math.Cos(theta * SlerpPower);
Vector3 rotationUnitVector = new Vector3(rotationDelta.X, rotationDelta.Y, rotationDelta.Z);
rotationUnitVector.Normalize();
Quaternion rotationPower = new Quaternion(cosPowerTheta,
sinPowerTheta * rotationUnitVector.X,
sinPowerTheta * rotationUnitVector.Y,
sinPowerTheta * rotationUnitVector.Z);
rotationPower.Normalize();
// multiple this by predicted value to get result
StateQ *= rotationPower;
StateQ.Normalize();
}
