private void Update()
{
RTQuaternion rotationDelta;
RTQuaternion rotationPower;
RTVector3 rotationUnitVector;
if (EnableMag || EnableAccel)
{
// calculate rotation delta
rotationDelta = mStateQ.Conjugate() * mMeasuredQPose;
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);
rotationUnitVector = new RTVector3(rotationDelta.X, rotationDelta.Y, rotationDelta.Z);
rotationUnitVector.Normalize();
rotationPower = new RTQuaternion(cosPowerTheta,
sinPowerTheta * rotationUnitVector.X,
sinPowerTheta * rotationUnitVector.Y,
sinPowerTheta * rotationUnitVector.Z);
rotationPower.Normalize();
// multiple this by predicted value to get result
mStateQ *= rotationPower;
mStateQ.Normalize();
}
}
public void CalculatePose(RTVector3 accel, RTVector3 mag, double magDeclination)
{
RTQuaternion m;
RTQuaternion q = new RTQuaternion();
if (EnableAccel)
{
accel.AccelToEuler(out mMeasuredPose);
}
else
{
mMeasuredPose = mFusionPose;
mMeasuredPose.Z = 0;
}
if (EnableMag && mMagValid)
{
q.FromEuler(mMeasuredPose);
m = new RTQuaternion(0, mag.X, mag.Y, mag.Z);
m = q * m * q.Conjugate();
mMeasuredPose.Z = -Math.Atan2(m.Y, m.X) - magDeclination;
}
else
{
mMeasuredPose.Z = mFusionPose.Z;
}
mMeasuredQPose.FromEuler(mMeasuredPose);
// 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(mMeasuredQPose.data(i)) > maxVal)
{
maxVal = Math.Abs(mMeasuredQPose.data(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 (((mMeasuredQPose.data(maxIndex) < 0) && (mFusionQPose.data(maxIndex) > 0)) ||
((mMeasuredQPose.data(maxIndex) > 0) && (mFusionQPose.data(maxIndex) < 0)))
{
mMeasuredQPose.Scalar = -mMeasuredQPose.Scalar;
mMeasuredQPose.X = -mMeasuredQPose.X;
mMeasuredQPose.Y = -mMeasuredQPose.Y;
mMeasuredQPose.Z = -mMeasuredQPose.Z;
mMeasuredQPose.ToEuler(out mMeasuredPose);
}
}
public void CalculatePose(RTVector3 accel, RTVector3 mag, double magDeclination)
{
RTQuaternion m;
RTQuaternion q = new RTQuaternion();
if (EnableAccel) {
accel.AccelToEuler(out mMeasuredPose);
} else {
mMeasuredPose = mFusionPose;
mMeasuredPose.Z = 0;
}
if (EnableMag && mMagValid) {
q.FromEuler(mMeasuredPose);
m = new RTQuaternion(0, mag.X, mag.Y, mag.Z);
m = q * m * q.Conjugate();
mMeasuredPose.Z = -Math.Atan2(m.Y, m.X) - magDeclination;
} else {
mMeasuredPose.Z = mFusionPose.Z;
}
mMeasuredQPose.FromEuler(mMeasuredPose);
// 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(mMeasuredQPose.data(i)) > maxVal) {
maxVal = Math.Abs(mMeasuredQPose.data(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 (((mMeasuredQPose.data(maxIndex) < 0) && (mFusionQPose.data(maxIndex) > 0)) ||
((mMeasuredQPose.data(maxIndex) > 0) && (mFusionQPose.data(maxIndex) < 0))) {
mMeasuredQPose.Scalar = -mMeasuredQPose.Scalar;
mMeasuredQPose.X = -mMeasuredQPose.X;
mMeasuredQPose.Y = -mMeasuredQPose.Y;
mMeasuredQPose.Z = -mMeasuredQPose.Z;
mMeasuredQPose.ToEuler(out mMeasuredPose);
}
}
public void AccelToQuaternion(out RTQuaternion qPose)
{
RTVector3 normAccel = this;
RTVector3 vec;
RTVector3 z = new RTVector3(0, 0, 1.0);
qPose = new RTQuaternion();
normAccel.Normalize();
double angle = Math.Acos(RTVector3.DotProduct(z, normAccel));
RTVector3.CrossProduct(normAccel, z, out vec);
vec.Normalize();
qPose.FromAngleVector(angle, vec);
}
public static RTVector3 PoseFromAccelMag(RTVector3 accel, RTVector3 mag)
{
RTVector3 result;
RTQuaternion m;
RTQuaternion q;
accel.AccelToEuler(out result);
// 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);
q = new RTQuaternion(cosX2 * cosY2, sinX2 * cosY2, cosX2 * sinY2, -sinX2 * sinY2);
m = new RTQuaternion(0, mag.X, mag.Y, mag.Z);
m = q * m * q.Conjugate();
result.Z = -Math.Atan2(m.Y, m.X);
return(result);
}
private void Update()
{
RTQuaternion rotationDelta;
RTQuaternion rotationPower;
RTVector3 rotationUnitVector;
if (EnableMag || EnableAccel) {
// calculate rotation delta
rotationDelta = mStateQ.Conjugate() * mMeasuredQPose;
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);
rotationUnitVector = new RTVector3(rotationDelta.X, rotationDelta.Y, rotationDelta.Z);
rotationUnitVector.Normalize();
rotationPower = new RTQuaternion(cosPowerTheta,
sinPowerTheta * rotationUnitVector.X,
sinPowerTheta * rotationUnitVector.Y,
sinPowerTheta * rotationUnitVector.Z);
rotationPower.Normalize();
// multiple this by predicted value to get result
mStateQ *= rotationPower;
mStateQ.Normalize();
}
}
public void AccelToQuaternion(out RTQuaternion qPose)
{
RTVector3 normAccel = this;
RTVector3 vec;
RTVector3 z = new RTVector3(0, 0, 1.0);
qPose = new RTQuaternion();
normAccel.Normalize();
double angle = Math.Acos(RTVector3.DotProduct(z, normAccel));
RTVector3.CrossProduct(normAccel, z, out vec);
vec.Normalize();
qPose.FromAngleVector(angle, vec);
}
public static RTVector3 PoseFromAccelMag(RTVector3 accel, RTVector3 mag)
{
RTVector3 result;
RTQuaternion m;
RTQuaternion q;
accel.AccelToEuler(out result);
// 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);
q = new RTQuaternion(cosX2 * cosY2, sinX2 * cosY2, cosX2 * sinY2, -sinX2 * sinY2);
m = new RTQuaternion(0, mag.X, mag.Y, mag.Z);
m = q * m * q.Conjugate();
result.Z = -Math.Atan2(m.Y, m.X);
return result;
}
public static string Display(string label, RTQuaternion quat)
{
return string.Format("{0}: scalar: {1:F4}, x: {2:F4}, y: {3:F4}, z: {4:F4}", label, quat.Scalar, quat.X, quat.Y, quat.Z);
}
public static string Display(string label, RTQuaternion quat)
{
return(string.Format("{0}: scalar: {1:F4}, x: {2:F4}, y: {3:F4}, z: {4:F4}", label, quat.Scalar, quat.X, quat.Y, quat.Z));
}