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();
}
public static Vector3 PoseFromAccelMag(Vector3 accel, Vector3 mag)
{
Vector3 result = accel.AccelToEuler();
// q.fromEuler(result);
// since result.z() is always 0, this can be optimized a little
double cosX2 = Math.Cos(result.X / 2.0f);
double sinX2 = Math.Sin(result.X / 2.0f);
double cosY2 = Math.Cos(result.Y / 2.0f);
double sinY2 = Math.Sin(result.Y / 2.0f);
Quaternion q = new Quaternion(cosX2 * cosY2, sinX2 * cosY2, cosX2 * sinY2, -sinX2 * sinY2);
Quaternion m = new Quaternion(0, mag.X, mag.Y, mag.Z);
m = q * m * q.Conjugate();
result.Z = -Math.Atan2(m.Y, m.X);
return result;
}
private static string ReadingToString(bool valid, Quaternion quaternion, string unit)
{
return !valid ? "N/A" : quaternion + " " + unit;
}
public Quaternion AccelToQuaternion()
{
Vector3 normAccel = this;
Vector3 z = new Vector3(0, 0, 1.0);
normAccel.Normalize();
double angle = Math.Acos(DotProduct(z, normAccel));
Vector3 vec = CrossProduct(normAccel, z);
vec.Normalize();
var qPose = new Quaternion();
qPose.FromAngleVector(angle, vec);
return qPose;
}
