protected void CalculatePose(Vector3?acceleration, Vector3?magneticField, double magDeclination)
{
if (acceleration.HasValue)
{
MeasuredPose = acceleration.Value.AccelToEuler();
}
else
{
MeasuredPose = FusionPose;
MeasuredPose.Z = 0;
}
if (magneticField.HasValue)
{
var q = new Quaternion();
q.FromEuler(MeasuredPose);
var m = new Quaternion(0, magneticField.Value.X, magneticField.Value.Y, magneticField.Value.Z);
m = q * m * q.Conjugate();
MeasuredPose.Z = -Math.Atan2(m.Y, m.X) - magDeclination;
}
else
{
MeasuredPose.Z = FusionPose.Z;
}
MeasuredQPose.FromEuler(MeasuredPose);
// check for quaternion aliasing. If the quaternion has the wrong sign
// the filter will be very unhappy.
int maxIndex = -1;
double maxVal = -1000;
for (int i = 0; i < 4; i++)
{
if (Math.Abs(MeasuredQPose.GetData(i)) > maxVal)
{
maxVal = Math.Abs(MeasuredQPose.GetData(i));
maxIndex = i;
}
}
// if the biggest component has a different sign in the measured and kalman poses,
// change the sign of the measured pose to match.
if (((MeasuredQPose.GetData(maxIndex) < 0) && (FusionQPose.GetData(maxIndex) > 0)) ||
((MeasuredQPose.GetData(maxIndex) > 0) && (FusionQPose.GetData(maxIndex) < 0)))
{
MeasuredQPose.Scalar = -MeasuredQPose.Scalar;
MeasuredQPose.X = -MeasuredQPose.X;
MeasuredQPose.Y = -MeasuredQPose.Y;
MeasuredQPose.Z = -MeasuredQPose.Z;
MeasuredQPose.ToEuler(out MeasuredPose);
}
}
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);
}
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;
}