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