protected MatrixLibrary.Matrix magneticCompensation(MyQuaternion q, double m_x, double m_y, double m_z)
{
MatrixLibrary.Matrix h = new MatrixLibrary.Matrix(4, 1);
MatrixLibrary.Matrix temp;
//compute the direction of the magnetic field
MatrixLibrary.Matrix quaternion = q.getQuaternionAsVector();
MatrixLibrary.Matrix quaternion_conjugate = q.getConjugate();
//magnetic field compensation
temp = MyQuaternion.quaternionProduct(quaternion, new MyQuaternion(0.0, m_x, m_y, m_z).getQuaternionAsVector());
h = MyQuaternion.quaternionProduct(temp, quaternion_conjugate);
double bx = Math.Sqrt((h[1, 0] * h[1, 0] + h[2, 0] * h[2, 0]));
double bz = h[3, 0];
double norm = Math.Sqrt(bx * bx + bz * bz);
bx /= norm;
bz /= norm;
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(3, 1);
result[0, 0] = bx;
result[1, 0] = 0;
result[2, 0] = bz;
return(result);
}
protected MatrixLibrary.Matrix magneticCompensation(MyQuaternion q, double m_x, double m_y, double m_z)
{
MatrixLibrary.Matrix h = new MatrixLibrary.Matrix(4, 1);
MatrixLibrary.Matrix temp;
//compute the direction of the magnetic field
MatrixLibrary.Matrix quaternion = q.getQuaternionAsVector();
MatrixLibrary.Matrix quaternion_conjugate = q.getConjugate();
//magnetic field compensation
temp = MyQuaternion.quaternionProduct(quaternion, new MyQuaternion(0.0, m_x, m_y, m_z).getQuaternionAsVector());
h = MyQuaternion.quaternionProduct(temp, quaternion_conjugate);
double bx = Math.Sqrt((h[1, 0] * h[1, 0] + h[2, 0] * h[2, 0]));
double bz = h[3, 0];
double norm = Math.Sqrt(bx * bx + bz * bz);
bx /= norm;
bz /= norm;
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(3, 1);
result[0, 0] = bx;
result[1, 0] = 0;
result[2, 0] = bz;
return result;
}
public static MatrixLibrary.Matrix quaternionProduct(MyQuaternion q_a, MyQuaternion q_b)
{
MatrixLibrary.Matrix v1 = q_a.getQuaternionAsVector();
MatrixLibrary.Matrix v2 = q_b.getQuaternionAsVector();
return(quaternionProduct(v1, v2));
}
public static MatrixLibrary.Matrix quaternionProduct(MyQuaternion q_a, MyQuaternion q_b)
{
MatrixLibrary.Matrix v1 = q_a.getQuaternionAsVector();
MatrixLibrary.Matrix v2 = q_b.getQuaternionAsVector();
return quaternionProduct(v1, v2);
}