private void Predict()
{
if (!EnableGyro || !Gyro.HasValue)
{
return;
}
double qs = StateQ.Scalar;
double qx = StateQ.X;
double qy = StateQ.Y;
double qz = StateQ.Z;
double x2 = Gyro.Value.X / 2.0;
double y2 = Gyro.Value.Y / 2.0;
double z2 = Gyro.Value.Z / 2.0;
// Predict new state
double timeDeltaSeconds = TimeDelta.TotalSeconds;
StateQ.Scalar = qs + (-x2 * qx - y2 * qy - z2 * qz) * timeDeltaSeconds;
StateQ.X = qx + (x2 * qs + z2 * qy - y2 * qz) * timeDeltaSeconds;
StateQ.Y = qy + (y2 * qs - z2 * qx + x2 * qz) * timeDeltaSeconds;
StateQ.Z = qz + (z2 * qs + y2 * qx - x2 * qy) * timeDeltaSeconds;
StateQ.Normalize();
}
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();
}
