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 AccelToEuler(out RTVector3 rollPitchYaw)
{
RTVector3 normAccel = this;
normAccel.Normalize();
rollPitchYaw = new RTVector3(Math.Atan2(normAccel.mY, normAccel.mZ),
-Math.Atan2(normAccel.mX, Math.Sqrt(normAccel.mY * normAccel.mY + normAccel.mZ * normAccel.mZ)), 0);
}
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 FromAngleVector(double angle, RTVector3 vec)
{
double sinHalfTheta = Math.Sin(angle / 2.0);
mScalar = Math.Cos(angle / 2.0);
mX = vec.X * sinHalfTheta;
mY = vec.Y * sinHalfTheta;
mZ = vec.Z * sinHalfTheta;
}
public static void ConvertToVector(byte[] rawData, out RTVector3 vec, double scale, bool bigEndian)
{
if (bigEndian) {
vec = new RTVector3((double)((UInt16)(((UInt16)rawData[0] << 8) | (UInt16)rawData[1])) * scale,
(double)((Int16)(((UInt16)rawData[2] << 8) | (UInt16)rawData[3])) * scale,
(double)((Int16)(((UInt16)rawData[4] << 8) | (UInt16)rawData[5])) * scale);
} else {
vec = new RTVector3((double)((Int16)(((UInt16)rawData[1] << 8) | (UInt16)rawData[0])) * scale,
(double)((Int16)(((UInt16)rawData[3] << 8) | (UInt16)rawData[2])) * scale,
(double)((Int16)(((UInt16)rawData[5] << 8) | (UInt16)rawData[4])) * scale);
}
}
public void Reset()
{
mFirstTime = true;
mFusionPose = new RTVector3();
mFusionQPose.FromEuler(mFusionPose);
mGyro = new RTVector3();
mAccel = new RTVector3();
mMag = new RTVector3();
mMeasuredPose = new RTVector3();
mMeasuredQPose.FromEuler(mMeasuredPose);
mSampleNumber = 0;
}
public void FromEuler(RTVector3 vec)
{
double cosX2 = Math.Cos(vec.X / 2.0f);
double sinX2 = Math.Sin(vec.X / 2.0f);
double cosY2 = Math.Cos(vec.Y / 2.0f);
double sinY2 = Math.Sin(vec.Y / 2.0f);
double cosZ2 = Math.Cos(vec.Z / 2.0f);
double sinZ2 = Math.Sin(vec.Z / 2.0f);
mScalar = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
mX = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
mY = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
mZ = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
Normalize();
}
public static void ConvertToVector(byte[] rawData, out RTVector3 vec, double scale, bool bigEndian)
{
if (bigEndian)
{
vec = new RTVector3((double)((UInt16)(((UInt16)rawData[0] << 8) | (UInt16)rawData[1])) * scale,
(double)((Int16)(((UInt16)rawData[2] << 8) | (UInt16)rawData[3])) * scale,
(double)((Int16)(((UInt16)rawData[4] << 8) | (UInt16)rawData[5])) * scale);
}
else
{
vec = new RTVector3((double)((Int16)(((UInt16)rawData[1] << 8) | (UInt16)rawData[0])) * scale,
(double)((Int16)(((UInt16)rawData[3] << 8) | (UInt16)rawData[2])) * scale,
(double)((Int16)(((UInt16)rawData[5] << 8) | (UInt16)rawData[4])) * scale);
}
}
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 NewIMUData(ref RTIMUData data)
{
mSampleNumber++;
if (EnableGyro)
{
mGyro = data.gyro;
}
else
{
mGyro = new RTVector3();
}
mAccel = data.accel;
mMag = data.mag;
mMagValid = data.magValid;
if (mFirstTime)
{
mLastFusionTime = data.timestamp;
CalculatePose(mAccel, mMag, CompassAdjDeclination);
// initialize the poses
mStateQ.FromEuler(mMeasuredPose);
mFusionQPose = mStateQ;
mFusionPose = mMeasuredPose;
mFirstTime = false;
}
else
{
mTimeDelta = (double)(data.timestamp - mLastFusionTime) / (double)1000000;
if (mTimeDelta > 0)
{
CalculatePose(mAccel, mMag, CompassAdjDeclination);
Predict();
Update();
mStateQ.ToEuler(out mFusionPose);
mFusionQPose = mStateQ;
}
mLastFusionTime = data.timestamp;
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = mFusionPose;
data.fusionQPose = mFusionQPose;
}
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 void ToAngleVector(out double angle, out RTVector3 vec)
{
double halfTheta;
double sinHalfTheta;
halfTheta = Math.Acos(mScalar);
sinHalfTheta = Math.Sin(halfTheta);
if (sinHalfTheta == 0)
{
vec = new RTVector3(1.0, 0, 0);
}
else
{
vec = new RTVector3(mX / sinHalfTheta, mY / sinHalfTheta, mZ / sinHalfTheta);
}
angle = 2.0 * halfTheta;
}
public void NewIMUData(ref RTIMUData data)
{
mSampleNumber++;
if (EnableGyro)
mGyro = data.gyro;
else
mGyro = new RTVector3();
mAccel = data.accel;
mMag = data.mag;
mMagValid = data.magValid;
if (mFirstTime) {
mLastFusionTime = data.timestamp;
CalculatePose(mAccel, mMag, CompassAdjDeclination);
// initialize the poses
mStateQ.FromEuler(mMeasuredPose);
mFusionQPose = mStateQ;
mFusionPose = mMeasuredPose;
mFirstTime = false;
} else {
mTimeDelta = (double)(data.timestamp - mLastFusionTime) / (double)1000000;
if (mTimeDelta > 0) {
CalculatePose(mAccel, mMag, CompassAdjDeclination);
Predict();
Update();
mStateQ.ToEuler(out mFusionPose);
mFusionQPose = mStateQ;
}
mLastFusionTime = data.timestamp;
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = mFusionPose;
data.fusionQPose = mFusionQPose;
}
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 RTVector3 GetAccelResiduals()
{
RTQuaternion rotatedGravity;
RTQuaternion fusedConjugate;
RTQuaternion qTemp;
RTVector3 residuals;
// do gravity rotation and subtraction
// create the conjugate of the pose
fusedConjugate = mFusionQPose.Conjugate();
// now do the rotation - takes two steps with qTemp as the intermediate variable
qTemp = mGravity * mFusionQPose;
rotatedGravity = fusedConjugate * qTemp;
// now adjust the measured accel and change the signs to make sense
residuals = new RTVector3(-(mAccel.X - rotatedGravity.X), -(mAccel.Y - rotatedGravity.Y), -(mAccel.Z - rotatedGravity.Z));
return(residuals);
}
public void FromEuler(RTVector3 vec)
{
double cosX2 = Math.Cos(vec.X / 2.0f);
double sinX2 = Math.Sin(vec.X / 2.0f);
double cosY2 = Math.Cos(vec.Y / 2.0f);
double sinY2 = Math.Sin(vec.Y / 2.0f);
double cosZ2 = Math.Cos(vec.Z / 2.0f);
double sinZ2 = Math.Sin(vec.Z / 2.0f);
mScalar = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
mX = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
mY = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
mZ = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
Normalize();
}
public static string DisplayDegrees(string label, RTVector3 vec)
{
return string.Format("{0}: roll: {1:F4}, pitch: {2:F4}, yaw: {3:F4}", label, vec.X * RTMATH_RAD_TO_DEGREE,
vec.Y * RTMATH_RAD_TO_DEGREE, vec.Z * RTMATH_RAD_TO_DEGREE);
}
public static void CrossProduct(RTVector3 a, RTVector3 b, out RTVector3 c)
{
c = new RTVector3(a.mY * b.mZ - a.mZ * b.mY, a.mZ * b.mX - a.mX * b.mZ, a.mX * b.mY - a.mY * b.mX);
}
public static double DotProduct(RTVector3 a, RTVector3 b)
{
return(a.mX * b.mX + a.mY * b.mY + a.mZ * b.mZ);
}
public void FromAngleVector(double angle, RTVector3 vec)
{
double sinHalfTheta = Math.Sin(angle / 2.0);
mScalar = Math.Cos(angle / 2.0);
mX = vec.X * sinHalfTheta;
mY = vec.Y * sinHalfTheta;
mZ = vec.Z * sinHalfTheta;
}
public void ToEuler(out RTVector3 vec)
{
vec = new RTVector3(Math.Atan2(2.0 * (mY * mZ + mScalar * mX), 1 - 2.0 * (mX * mX + mY * mY)),
Math.Asin(2.0 * (mScalar * mY - mX * mZ)),
Math.Atan2(2.0 * (mX * mY + mScalar * mZ), 1 - 2.0 * (mY * mY + mZ * mZ)));
}
public RTVector3 GetAccelResiduals()
{
RTQuaternion rotatedGravity;
RTQuaternion fusedConjugate;
RTQuaternion qTemp;
RTVector3 residuals;
// do gravity rotation and subtraction
// create the conjugate of the pose
fusedConjugate = mFusionQPose.Conjugate();
// now do the rotation - takes two steps with qTemp as the intermediate variable
qTemp = mGravity * mFusionQPose;
rotatedGravity = fusedConjugate * qTemp;
// now adjust the measured accel and change the signs to make sense
residuals = new RTVector3(-(mAccel.X - rotatedGravity.X), -(mAccel.Y - rotatedGravity.Y), -(mAccel.Z - rotatedGravity.Z));
return residuals;
}
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 static string DisplayDegrees(string label, RTVector3 vec)
{
return(string.Format("{0}: roll: {1:F4}, pitch: {2:F4}, yaw: {3:F4}", label, vec.X * RTMATH_RAD_TO_DEGREE,
vec.Y * RTMATH_RAD_TO_DEGREE, vec.Z * RTMATH_RAD_TO_DEGREE));
}
public static string DisplayRadians(string label, RTVector3 vec)
{
return string.Format("{0}: x:{1:F4}, y: {2:F4}, z: {3:F4}", label, vec.X, vec.Y, vec.Z);
}
public static void CrossProduct(RTVector3 a, RTVector3 b, out RTVector3 c)
{
c = new RTVector3(a.mY * b.mZ - a.mZ * b.mY, a.mZ * b.mX - a.mX * b.mZ, a.mX * b.mY - a.mY * b.mX);
}
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);
}
// Note - code assumes that this is the first thing called after axis swapping
// for each specific IMU chip has occurred.
protected void HandleGyroBias()
{
// do axis rotation
if ((mAxisRotation > 0) && (mAxisRotation < RTIMU_AXIS_ROTATION_COUNT))
{
// need to do an axis rotation
RTIMUData tempIMU = mImuData;
// do new x value
if (mAxisRotationArray[mAxisRotation, 0] != 0)
{
mImuData.gyro.X = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 0];
mImuData.accel.X = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 0];
mImuData.mag.X = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 0];
}
else if (mAxisRotationArray[mAxisRotation, 1] != 0)
{
mImuData.gyro.X = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 1];
mImuData.accel.X = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 1];
mImuData.mag.X = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 1];
}
else if (mAxisRotationArray[mAxisRotation, 2] != 0)
{
mImuData.gyro.X = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 2];
mImuData.accel.X = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 2];
mImuData.mag.X = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 2];
}
// do new y value
if (mAxisRotationArray[mAxisRotation, 3] != 0)
{
mImuData.gyro.Y = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 3];
mImuData.accel.Y = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 3];
mImuData.mag.Y = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 3];
}
else if (mAxisRotationArray[mAxisRotation, 4] != 0)
{
mImuData.gyro.Y = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 4];
mImuData.accel.Y = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 4];
mImuData.mag.Y = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 4];
}
else if (mAxisRotationArray[mAxisRotation, 5] != 0)
{
mImuData.gyro.Y = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 5];
mImuData.accel.Y = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 5];
mImuData.mag.Y = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 5];
}
// do new z value
if (mAxisRotationArray[mAxisRotation, 6] != 0)
{
mImuData.gyro.Z = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 6];
mImuData.accel.Z = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 6];
mImuData.mag.Z = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 6];
}
else if (mAxisRotationArray[mAxisRotation, 7] != 0)
{
mImuData.gyro.Z = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 7];
mImuData.accel.Z = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 7];
mImuData.mag.Z = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 7];
}
else if (mAxisRotationArray[mAxisRotation, 8] != 0)
{
mImuData.gyro.Z = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 8];
mImuData.accel.Z = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 8];
mImuData.mag.Z = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 8];
}
}
RTVector3 deltaAccel = mPreviousAccel;
deltaAccel -= mImuData.accel; // compute difference
mPreviousAccel = mImuData.accel;
if ((deltaAccel.length() < RTIMU_FUZZY_ACCEL_ZERO) && (mImuData.gyro.length() < RTIMU_FUZZY_GYRO_ZERO))
{
// what we are seeing on the gyros should be bias only so learn from this
if (mGyroSampleCount < (5 * mSampleRate))
{
mGyroBias.X = (1.0 - mGyroLearningAlpha) * mGyroBias.X + mGyroLearningAlpha * mImuData.gyro.X;
mGyroBias.Y = (1.0 - mGyroLearningAlpha) * mGyroBias.Y + mGyroLearningAlpha * mImuData.gyro.Y;
mGyroBias.Z = (1.0 - mGyroLearningAlpha) * mGyroBias.Z + mGyroLearningAlpha * mImuData.gyro.Z;
mGyroSampleCount++;
if (mGyroSampleCount == (5 * mSampleRate))
{
// this could have been true already of course
mGyroBiasValid = true;
}
}
else
{
mGyroBias.X = (1.0 - mGyroContinuousAlpha) * mGyroBias.X + mGyroContinuousAlpha * mImuData.gyro.X;
mGyroBias.Y = (1.0 - mGyroContinuousAlpha) * mGyroBias.Y + mGyroContinuousAlpha * mImuData.gyro.Y;
mGyroBias.Z = (1.0 - mGyroContinuousAlpha) * mGyroBias.Z + mGyroContinuousAlpha * mImuData.gyro.Z;
}
}
mImuData.gyro -= mGyroBias;
}
public void ToEuler(out RTVector3 vec)
{
vec = new RTVector3(Math.Atan2(2.0 * (mY * mZ + mScalar * mX), 1 - 2.0 * (mX * mX + mY * mY)),
Math.Asin(2.0 * (mScalar * mY - mX * mZ)),
Math.Atan2(2.0 * (mX * mY + mScalar * mZ), 1 - 2.0 * (mY * mY + mZ * mZ)));
}
public void ToAngleVector(out double angle, out RTVector3 vec)
{
double halfTheta;
double sinHalfTheta;
halfTheta = Math.Acos(mScalar);
sinHalfTheta = Math.Sin(halfTheta);
if (sinHalfTheta == 0) {
vec = new RTVector3(1.0, 0, 0);
} else {
vec = new RTVector3(mX / sinHalfTheta, mY / sinHalfTheta, mZ / sinHalfTheta);
}
angle = 2.0 * halfTheta;
}
public void Reset()
{
mFirstTime = true;
mFusionPose = new RTVector3();
mFusionQPose.FromEuler(mFusionPose);
mGyro = new RTVector3();
mAccel = new RTVector3();
mMag = new RTVector3();
mMeasuredPose = new RTVector3();
mMeasuredQPose.FromEuler(mMeasuredPose);
mSampleNumber = 0;
}
// Note - code assumes that this is the first thing called after axis swapping
// for each specific IMU chip has occurred.
protected void HandleGyroBias()
{
// do axis rotation
if ((mAxisRotation > 0) && (mAxisRotation < RTIMU_AXIS_ROTATION_COUNT)) {
// need to do an axis rotation
RTIMUData tempIMU = mImuData;
// do new x value
if (mAxisRotationArray[mAxisRotation, 0] != 0) {
mImuData.gyro.X = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 0];
mImuData.accel.X = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 0];
mImuData.mag.X = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 0];
} else if (mAxisRotationArray[mAxisRotation, 1] != 0) {
mImuData.gyro.X = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 1];
mImuData.accel.X = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 1];
mImuData.mag.X = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 1];
} else if (mAxisRotationArray[mAxisRotation, 2] != 0) {
mImuData.gyro.X = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 2];
mImuData.accel.X = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 2];
mImuData.mag.X = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 2];
}
// do new y value
if (mAxisRotationArray[mAxisRotation, 3] != 0) {
mImuData.gyro.Y = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 3];
mImuData.accel.Y = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 3];
mImuData.mag.Y = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 3];
} else if (mAxisRotationArray[mAxisRotation, 4] != 0) {
mImuData.gyro.Y = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 4];
mImuData.accel.Y = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 4];
mImuData.mag.Y = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 4];
} else if (mAxisRotationArray[mAxisRotation, 5] != 0) {
mImuData.gyro.Y = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 5];
mImuData.accel.Y = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 5];
mImuData.mag.Y = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 5];
}
// do new z value
if (mAxisRotationArray[mAxisRotation, 6] != 0) {
mImuData.gyro.Z = tempIMU.gyro.X * mAxisRotationArray[mAxisRotation, 6];
mImuData.accel.Z = tempIMU.accel.X * mAxisRotationArray[mAxisRotation, 6];
mImuData.mag.Z = tempIMU.mag.X * mAxisRotationArray[mAxisRotation, 6];
} else if (mAxisRotationArray[mAxisRotation, 7] != 0) {
mImuData.gyro.Z = tempIMU.gyro.Y * mAxisRotationArray[mAxisRotation, 7];
mImuData.accel.Z = tempIMU.accel.Y * mAxisRotationArray[mAxisRotation, 7];
mImuData.mag.Z = tempIMU.mag.Y * mAxisRotationArray[mAxisRotation, 7];
} else if (mAxisRotationArray[mAxisRotation, 8] != 0) {
mImuData.gyro.Z = tempIMU.gyro.Z * mAxisRotationArray[mAxisRotation, 8];
mImuData.accel.Z = tempIMU.accel.Z * mAxisRotationArray[mAxisRotation, 8];
mImuData.mag.Z = tempIMU.mag.Z * mAxisRotationArray[mAxisRotation, 8];
}
}
RTVector3 deltaAccel = mPreviousAccel;
deltaAccel -= mImuData.accel; // compute difference
mPreviousAccel = mImuData.accel;
if ((deltaAccel.length() < RTIMU_FUZZY_ACCEL_ZERO) && (mImuData.gyro.length() < RTIMU_FUZZY_GYRO_ZERO)) {
// what we are seeing on the gyros should be bias only so learn from this
if (mGyroSampleCount < (5 * mSampleRate)) {
mGyroBias.X = (1.0 - mGyroLearningAlpha) * mGyroBias.X + mGyroLearningAlpha * mImuData.gyro.X;
mGyroBias.Y = (1.0 - mGyroLearningAlpha) * mGyroBias.Y + mGyroLearningAlpha * mImuData.gyro.Y;
mGyroBias.Z = (1.0 - mGyroLearningAlpha) * mGyroBias.Z + mGyroLearningAlpha * mImuData.gyro.Z;
mGyroSampleCount++;
if (mGyroSampleCount == (5 * mSampleRate)) {
// this could have been true already of course
mGyroBiasValid = true;
}
} else {
mGyroBias.X = (1.0 - mGyroContinuousAlpha) * mGyroBias.X + mGyroContinuousAlpha * mImuData.gyro.X;
mGyroBias.Y = (1.0 - mGyroContinuousAlpha) * mGyroBias.Y + mGyroContinuousAlpha * mImuData.gyro.Y;
mGyroBias.Z = (1.0 - mGyroContinuousAlpha) * mGyroBias.Z + mGyroContinuousAlpha * mImuData.gyro.Z;
}
}
mImuData.gyro -= mGyroBias;
}
public static double DotProduct(RTVector3 a, RTVector3 b)
{
return a.mX * b.mX + a.mY * b.mY + a.mZ * b.mZ;
}
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 void AccelToEuler(out RTVector3 rollPitchYaw)
{
RTVector3 normAccel = this;
normAccel.Normalize();
rollPitchYaw = new RTVector3(Math.Atan2(normAccel.mY, normAccel.mZ),
-Math.Atan2(normAccel.mX, Math.Sqrt(normAccel.mY * normAccel.mY + normAccel.mZ * normAccel.mZ)), 0);
}
public static string DisplayRadians(string label, RTVector3 vec)
{
return(string.Format("{0}: x:{1:F4}, y: {2:F4}, z: {3:F4}", label, vec.X, vec.Y, vec.Z));
}