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; }