void ComputeQuatAlternateMethod()
{
// Trapezoidal integration of gyro readings
float rx = (float)((GyroX+lastGx)*0.5f - GZeroX) * GyroScale + errCorrX2;
float ry = (float)((GyroZ+lastGz)*0.5f - GZeroZ) * -GyroScale + errCorrY2;
float rz = (float)((GyroY+lastGy)*0.5f - GZeroY) * -GyroScale + errCorrZ2;
lastGx = GyroX;
lastGy = GyroY;
lastGz = GyroZ;
float rMag = (float)(Math.Sqrt(rx * rx + ry * ry + rz * rz + 0.0000000001) * 0.5);
float cosr = (float)Math.Cos(rMag);
float sinr = (float)Math.Sin(rMag) / rMag;
qdot.w = -(rx * Q2.x + ry * Q2.y + rz * Q2.z) * 0.5f;
qdot.x = (rx * Q2.w + rz * Q2.y - ry * Q2.z) * 0.5f;
qdot.y = (ry * Q2.w - rz * Q2.x + rx * Q2.z) * 0.5f;
qdot.z = (rz * Q2.w + ry * Q2.x - rx * Q2.y) * 0.5f;
Q2.w = cosr * Q2.w + sinr * qdot.w;
Q2.x = cosr * Q2.x + sinr * qdot.x;
Q2.y = cosr * Q2.y + sinr * qdot.y;
Q2.z = cosr * Q2.z + sinr * qdot.z;
Q2 = Q2.Normalize();
// Convert to matrix form
m2.From(Q2);
// Compute the difference between the accelerometer vector and the matrix Y (up) vector
acc = new Vector( -AccelX, AccelZ, AccelY );
rMag = acc.Length * AccScale;
acc = acc.Normalize();
float accWeight = 1.0f - Math.Min( Math.Abs( 2.0f - rMag * 2.0f ), 1.0f );
// accWeight *= accWeight * 4.0f;
float errDiffX = acc.y * m2.m[1,2] - acc.z * m2.m[1,1];
float errDiffY = acc.z * m2.m[1,0] - acc.x * m2.m[1,2];
float errDiffZ = acc.x * m2.m[1,1] - acc.y * m2.m[1,0];
accWeight *= 1.0f / 512.0f;
errCorrX2 = errDiffX * accWeight;
errCorrY2 = errDiffY * accWeight;
errCorrZ2 = errDiffZ * accWeight;
// At this point, errCorr represents a very small correction rotation vector, but in the WORLD frame
// Rotate it into the current BODY frame
//Vector errVect = new Vector( errCorrX2, errCorrY2, errCorrZ2 );
//errVect = m.Transpose().Mul( errVect );
//errCorrX2 = errVect.x;
//errCorrY2 = errVect.y;
//errCorrZ2 = errVect.z;
}
private float ComputeTiltCompensatedHeading()
{
// Compute pitch and roll from the current accelerometer vector - only accurate if stationary
Vector v = new Vector( AccelX, AccelY, AccelZ );
v = v.Normalize();
float accPitch = (float)Math.Asin( -v.x );
float accRoll = (float)Math.Asin( v.y / Math.Cos(accPitch) );
// Technically we should also calibrate the min/max readings from the mag first - this may not be accurate otherwise
float Mxh = (float)(MagX * Math.Cos( accPitch ) + MagZ * Math.Sin( accPitch ));
float Myh = (float)(MagX * Math.Sin( accRoll ) * Math.Sin( accPitch ) + MagY * Math.Cos( accRoll ) - MagZ * Math.Sin( accRoll ) * Math.Cos( accPitch ));
float Heading = (float)(Math.Atan2( Mxh, Myh ) * 1800.0 / Math.PI);
return Heading;
}
void ComputeQuatOriginalMethod()
{
float rx = (float)(GyroX - GZeroX) * GyroScale + errCorrX1;
float ry = (float)(GyroZ - GZeroZ) * -GyroScale + errCorrY1;
float rz = (float)(GyroY - GZeroY) * -GyroScale + errCorrZ1;
float rMag = (float)(Math.Sqrt(rx * rx + ry * ry + rz * rz + 0.0000000001) * 0.5);
float cosr = (float)Math.Cos(rMag);
float sinr = (float)Math.Sin(rMag) / rMag;
qdot.w = -(rx * Q1.x + ry * Q1.y + rz * Q1.z) * 0.5f;
qdot.x = (rx * Q1.w + rz * Q1.y - ry * Q1.z) * 0.5f;
qdot.y = (ry * Q1.w - rz * Q1.x + rx * Q1.z) * 0.5f;
qdot.z = (rz * Q1.w + ry * Q1.x - rx * Q1.y) * 0.5f;
Q1.w = cosr * Q1.w + sinr * qdot.w;
Q1.x = cosr * Q1.x + sinr * qdot.x;
Q1.y = cosr * Q1.y + sinr * qdot.y;
Q1.z = cosr * Q1.z + sinr * qdot.z;
Q1 = Q1.Normalize();
// Convert to matrix form
m.From(Q1);
// Compute the difference between the accelerometer vector and the matrix Y (up) vector
acc = new Vector( -AccelX, AccelZ, AccelY );
rMag = acc.Length * AccScale;
acc = acc.Normalize();
float accWeight = 1.0f - Math.Min( Math.Abs( 2.0f - rMag * 2.0f ), 1.0f );
float errDiffX = acc.y * m.m[1,2] - acc.z * m.m[1,1];
float errDiffY = acc.z * m.m[1,0] - acc.x * m.m[1,2];
float errDiffZ = acc.x * m.m[1,1] - acc.y * m.m[1,0];
accWeight *= 1.0f / 512.0f;
errCorrX1 = errDiffX * accWeight;
errCorrY1 = errDiffY * accWeight;
errCorrZ1 = errDiffZ * accWeight;
}
