public static double[] getAngleFromRotation( MatrixLibrary.Matrix m ) { /** this conversion uses conventions as described on page: * http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm * Coordinate System: right hand * Positive angle: right hand * Order of euler angles: heading first, then attitude, then bank * matrix row column ordering: * [m[0,0] m[0,1] m[0,2]] * [m[1,0] m[1,1] m[1,2]] * [m[2,0] m[2,1] m[2,2]]*/ // Assuming the angles are in radians. double yaw=0, pitch=0, roll=0 ; if (m[1,0] > 0.998) { // singularity at north pole yaw = Math.Atan2(m[0,2],m[2,2]); pitch= Math.PI/2; roll = 0; return new double[] { roll, pitch, yaw }; } if (m[1,0] < -0.998) { // singularity at south pole yaw= Math.Atan2(m[0,2],m[2,2]); pitch = -Math.PI/2; roll = 0; return new double[] { roll, pitch, yaw}; } yaw = Math.Atan2(-m[2,0],m[0,0]); roll = Math.Atan2(-m[1,2],m[1,1]); pitch= Math.Asin(m[1,0]); return new double[] { roll, yaw, pitch}; }
/// <summary> /// Initializes a new instance of the <see cref="PortableMatrix"/> class. /// </summary> /// <param name="matrix">The matrix.</param> /// <param name="matrixname">The matrixname.</param> public PortableMatrix(MatrixLibrary.Matrix matrix, string matrixname) { _matrixname = matrixname; _rows = matrix.NoRows; _cols = matrix.NoCols; _matrixArray = ToSingleDimensionArray(matrix.toArray); }
public static MatrixLibrary.Matrix getAnglesFromQuaternion(MatrixLibrary.Matrix q) { double q0 = q[0, 0]; double q1 = q[1, 0]; double q2 = q[2, 0]; double q3 = q[3, 0]; MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(3, 1); result[0, 0] = Math.Atan2((2 * q2 * q3 + 2 * q0 * q1), (1 - 2 * q1 * q1 - 2 * q2 * q2)) * 180.0 / Math.PI; result[1, 0] = Math.Asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / Math.PI; result[2, 0] = Math.Atan2((2 * q1 * q2 + 2 * q0 * q3), (1 - 2 * (q2 * q2 + q3 * q3))) * 180.0 / Math.PI; return result; }
/// <summary> /// Updates the matrix. /// </summary> /// <param name="matrix">The matrix.</param> public void UpdateMatrix(MatrixLibrary.Matrix matrix) { foreach (MatrixTextbox matrixBox in _userDefinedMatrix.grid1.Children) { matrixBox.Text = matrix[matrixBox.Row, matrixBox.Column].ToString("#0.00"); _matrix[matrixBox.Row, matrixBox.Column] = matrixBox.Value; } }
protected MatrixLibrary.Matrix gaussNewtonMethod(double a_x, double a_y, double a_z, double m_x, double m_y, double m_z,MatrixLibrary.Matrix qObserv) { double norm; MatrixLibrary.Matrix n = new MatrixLibrary.Matrix(4, 1); MatrixLibrary.Matrix bRif = new MatrixLibrary.Matrix(3, 1); MatrixLibrary.Matrix jacobian = new MatrixLibrary.Matrix(6, 4); MatrixLibrary.Matrix R = new MatrixLibrary.Matrix(6, 6); MatrixLibrary.Matrix y_e = new MatrixLibrary.Matrix(6, 1); MatrixLibrary.Matrix y_b = new MatrixLibrary.Matrix(6, 1); double a = qObserv[1, 0]; double b = qObserv[2, 0]; double c = qObserv[3, 0]; double d = qObserv[0, 0]; int i = 0; MatrixLibrary.Matrix n_k = new MatrixLibrary.Matrix(4, 1); n_k[0, 0] = a; n_k[1, 0] = b; n_k[2, 0] = c; n_k[3, 0] = d; //Magnetometer Calibration values m_x = m_x * magnSFX + magnOffX; m_y = m_y * magnSFY + magnOffY; m_z = m_z * magnSFZ + magnOffZ; norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z); m_x /= norm; m_y /= norm; m_z /= norm; while (i < 3) { MyQuaternion q = new MyQuaternion(d, a, b, c); bRif = magneticCompensation(q, m_x, m_y, m_z); double bx = bRif[0, 0]; double by = bRif[1, 0]; double bz = bRif[2, 0]; //Jacobian Computation double j11 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z); double j12 = (-2 * b * a_x + 2 * a * a_y + 2 * d * a_z); double j13 = (-2 * c * a_x - 2 * d * a_y + 2 * a * a_z); double j14 = (2 * d * a_x - 2 * c * a_y + 2 * b * a_z); double j21 = (2 * b * a_x - 2 * a * a_y - 2 * d * a_z); double j22 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z); double j23 = (2 * d * a_x - 2 * c * a_y + 2 * b * a_z); double j24 = (2 * c * a_x + 2 * d * a_y - 2 * a * a_z); double j31 = (2 * c * a_x + 2 * d * a_y - 2 * a * a_z); double j32 = (-2 * d * a_x + 2 * c * a_y - 2 * b * a_z); double j33 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z); double j34 = (-2 * b * a_x + 2 * a * a_y + 2 * d * a_z); double j41 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z); double j42 = (-2 * b * m_x + 2 * a * m_y + 2 * m_z * d); double j43 = (-2 * c * m_x - 2 * d * m_y + 2 * a * m_z); double j44 = (2 * d * m_x - 2 * c * m_y + 2 * b * m_z); double j51 = (2 * b * m_x - 2 * a * m_y - 2 * d * m_z); double j52 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z); double j53 = (2 * d * m_x - 2 * c * m_y + 2 * b * m_z); double j54 = (2 * c * m_x + 2 * d * m_y - 2 * a * m_z); double j61 = (2 * c * m_x + 2 * d * m_y - 2 * a * m_z); double j62 = (-2 * d * m_x + 2 * c * m_y - 2 * b * m_z); double j63 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z); double j64 = (-2 * b * m_x + 2 * a * m_y + 2 * d * m_z); jacobian[0, 0] = j11; jacobian[0, 1] = j12; jacobian[0, 2] = j13; jacobian[0, 3] = j14; jacobian[1, 0] = j21; jacobian[1, 1] = j22; jacobian[1, 2] = j23; jacobian[1, 3] = j24; jacobian[2, 0] = j31; jacobian[2, 1] = j32; jacobian[2, 2] = j33; jacobian[2, 3] = j34; jacobian[3, 0] = j41; jacobian[3, 1] = j42; jacobian[3, 2] = j43; jacobian[3, 3] = j44; jacobian[4, 0] = j51; jacobian[4, 1] = j52; jacobian[4, 2] = j53; jacobian[4, 3] = j54; jacobian[5, 0] = j61; jacobian[5, 1] = j62; jacobian[5, 2] = j63; jacobian[5, 3] = j64; jacobian = -1 * jacobian; //End Jacobian Computation //DCM Rotation Matrix R[0, 0] = d * d + a * a - b * b - c * c; R[0, 1] = 2 * (a * b - c * d); R[0, 2] = 2 * (a * c + b * d); R[1, 0] = 2 * (a * b + c * d); R[1, 1] = d * d + b * b - a * a - c * c; R[1, 2] = 2 * (b * c - a * d); R[2, 0] = 2 * (a * c - b * d); R[2, 1] = 2 * (b * c + a * d); R[2, 2] = d * d + c * c - b * b - a * a; R[3, 3] = d * d + a * a - b * b - c * c; R[3, 4] = 2 * (a * b - c * d); R[3, 5] = 2 * (a * c + b * d); R[4, 3] = 2 * (a * b + c * d); R[4, 4] = d * d + b * b - a * a - c * c; R[4, 5] = 2 * (b * c - a * d); R[5, 3] = 2 * (a * c - b * d); R[5, 4] = 2 * (b * c + a * d); R[5, 5] = d * d + c * c - b * b - a * a; R[3, 0] = 0; R[3, 1] = 0; R[3, 2] = 0; R[4, 0] = 0; R[4, 1] = 0; R[4, 2] = 0; R[5, 0] = 0; R[5, 1] = 0; R[5, 2] = 0; R[0, 3] = 0; R[0, 4] = 0; R[0, 5] = 0; R[1, 3] = 0; R[1, 4] = 0; R[1, 5] = 0; R[2, 3] = 0; R[2, 4] = 0; R[2, 5] = 0; //End DCM //Reference Vector y_e[0, 0] = 0; y_e[1, 0] = 0; y_e[2, 0] = 1; y_e[3, 0] = bx; y_e[4, 0] = by; y_e[5, 0] = bz; //Body frame Vector y_b[0, 0] = a_x; y_b[1, 0] = a_y; y_b[2, 0] = a_z; y_b[3, 0] = m_x; y_b[4, 0] = m_y; y_b[5, 0] = m_z; //Gauss Newton Step n = n_k - MatrixLibrary.Matrix.Inverse((MatrixLibrary.Matrix.Transpose(jacobian) * jacobian)) * MatrixLibrary.Matrix.Transpose(jacobian) * (y_e - R * y_b); double normGauss = Math.Sqrt(n[0, 0] * n[0, 0] + n[1, 0] * n[1, 0] + n[2, 0] * n[2, 0] + n[3, 0] * n[3, 0]); n /= normGauss; //Console.Out.WriteLine(n + " "+ norm); a = n[0, 0]; b = n[1, 0]; c = n[2, 0]; d = n[3, 0]; n_k[0, 0] = a; n_k[1, 0] = b; n_k[2, 0] = c; n_k[3, 0] = d; i++; } return new MyQuaternion(d, a, b, c).getQuaternionAsVector(); }
protected MatrixLibrary.Matrix GradientDescent(double a_x, double a_y, double a_z, double m_x, double m_y, double m_z, double mu,MatrixLibrary.Matrix qObserv) { int i = 0; double q1, q2, q3, q4; MatrixLibrary.Matrix f_obb = new MatrixLibrary.Matrix(6, 1); MatrixLibrary.Matrix Jacobian = new MatrixLibrary.Matrix(6, 4); MatrixLibrary.Matrix Df = new MatrixLibrary.Matrix(4, 1); double norm; double bx, bz, by; MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1); q1 = qObserv[0, 0]; q2 = qObserv[1, 0]; q3 = qObserv[2, 0]; q4 = qObserv[3, 0]; //Magnetometer Calibration values m_x = m_x *magnSFX+ magnOffX; m_y = m_y *magnSFY+ magnOffY; m_z= m_z *magnSFZ+ magnOffZ; norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z); m_x /= norm; m_y /= norm; m_z /= norm; while (i < 10) { //compute the direction of the magnetic field MatrixLibrary.Matrix quaternion = new MyQuaternion(q1, q2, q3, q4).getQuaternionAsVector(); MatrixLibrary.Matrix bRif = magneticCompensation(new MyQuaternion(q1, q2, q3, q4), m_x, m_y, m_z); bx = bRif[0, 0]; by = bRif[1, 0]; bz = bRif[2, 0]; //compute the objective functions f_obb[0, 0] = 2 * (q2 * q4 - q1 * q3) - a_x; f_obb[1, 0] = 2 * (q1 * q2 + q3 * q4) - a_y; f_obb[2, 0] = 2 * (0.5 - q2 * q2 - q3 * q3) - a_z; f_obb[3, 0] = 2 * bx * (0.5 - q3 * q3 - q4 * q4) + 2 * by * (q1 * q4 + q2 * q3) + 2 * bz * (q2 * q4 - q1 * q3) - m_x; f_obb[4, 0] = 2 * bx * (q2 * q3 - q1 * q4) + 2 * by * (0.5 - q2 * q2 - q4 * q4) + 2 * bz * (q1 * q2 + q3 * q4) - m_y; f_obb[5, 0] = 2 * bx * (q1 * q3 + q2 * q4) + 2 * by * (q3 * q4 - q1 * q2) + 2 * bz * (0.5 - q2 * q2 - q3 * q3) - m_z; //compute the jacobian Jacobian[0, 0] = -2 * q3; Jacobian[0, 1] = 2 * q4; Jacobian[0, 2] = -2 * q1; Jacobian[0, 3] = 2 * q2; Jacobian[1, 0] = 2 * q2; Jacobian[1, 1] = 2 * q1; Jacobian[1, 2] = 2 * q4; Jacobian[1, 3] = 2 * q3; Jacobian[2, 0] = 0; Jacobian[2, 1] = -4 * q2; Jacobian[2, 2] = -4 * q3; Jacobian[2, 3] = 0; Jacobian[3, 0] = 2 * by * q4 - 2 * bz * q3; Jacobian[3, 1] = 2 * by * q3 + 2 * bz * q4; Jacobian[3, 2] = -4 * bx * q3 + 2 * by * q2 - 2 * bz * q1; Jacobian[3, 3] = -4 * bx * q4 + 2 * by * q1 + 2 * bz * q2; Jacobian[4, 0] = -2 * bx * q4 + 2 * bz * q2; Jacobian[4, 1] = 2 * bx * q3 - 4 * by * q2 + 2 * bz * q1; Jacobian[4, 2] = 2 * bx * q2 + 2 * bz * q4; Jacobian[4, 3] = -2 * bx * q1 - 4 * by * q4 + 2 * bz * q3; Jacobian[5, 0] = 2 * bx * q3 - 2 * by * q2; Jacobian[5, 1] = 2 * bx * q4 - 2 * by * q1 - 4 * bz * q2; Jacobian[5, 2] = 2 * bx * q1 + 2 * by * q4 - 4 * bz * q3; Jacobian[5, 3] = 2 * bx * q2 + 2 * by * q3; Df = MatrixLibrary.Matrix.Multiply(MatrixLibrary.Matrix.Transpose(Jacobian), f_obb); norm = Math.Sqrt(Df[0, 0] * Df[0, 0] + Df[1, 0] * Df[1, 0] + Df[2, 0] * Df[2, 0] + Df[3, 0] * Df[3, 0]); Df = MatrixLibrary.Matrix.ScalarDivide(norm, Df); result = quaternion - mu * Df; q1 = result[0, 0]; q2 = result[1, 0]; q3 = result[2, 0]; q4 = result[3, 0]; norm = Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); result = MatrixLibrary.Matrix.ScalarDivide(norm, result); q1 = result[0, 0]; q2 = result[1, 0]; q3 = result[2, 0]; q4 = result[3, 0]; i = i + 1; } return result; }
public static MatrixLibrary.Matrix quaternionProduct(MatrixLibrary.Matrix quaternion, MatrixLibrary.Matrix matrix) { double a1 = quaternion[0, 0]; double a2 = quaternion[1, 0]; double a3 = quaternion[2, 0]; double a4 = quaternion[3, 0]; double b1 = matrix[0, 0]; double b2 = matrix[1, 0]; double b3 = matrix[2, 0]; double b4 = matrix[3, 0]; MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1); result[0, 0] = a1 * b1 - a2 * b2 - a3 * b3 - a4 * b4; result[1, 0] = a1 * b2 + a2 * b1 + a3 * b4 - a4 * b3; result[2, 0] = a1 * b3 - a2 * b4 + a3 * b1 + a4 * b2; result[3, 0] = a1 * b4 + a2 * b3 - a3 * b2 + a4 * b1; return result; }
public static MatrixLibrary.Matrix getRotationMatrixFromQuaternion2(MatrixLibrary.Matrix Quaternion) { MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(1, 9); result[0, 0] = -1.0f + 2.0f * Quaternion[0, 0] * Quaternion[0, 0] +2.0f * Quaternion[0, 1] * Quaternion[0, 1]; result[0, 1] = 2.0f * Quaternion[0, 1] * Quaternion[0, 2] + 2.0f * Quaternion[0, 3] * Quaternion[0, 0]; result[0, 2] = 2.0f * Quaternion[0, 1] * Quaternion[0, 3] - 2.0f * Quaternion[0, 2] * Quaternion[0, 0]; result[0, 3] = 2.0f * Quaternion[0, 1] * Quaternion[0, 2] - 2.0f * Quaternion[0, 3] * Quaternion[0, 0]; result[0, 4] = -1.0f +2.0f * Quaternion[0, 0] * Quaternion[0, 0] + 2.0f * Quaternion[0, 2] * Quaternion[0, 2]; result[0, 5] = 2.0f * Quaternion[0, 2] * Quaternion[0, 3] + 2.0f * Quaternion[0, 1] * Quaternion[0, 0]; result[0, 6] = 2.0f * Quaternion[0, 1] * Quaternion[0, 3] + 2.0f * Quaternion[0, 2] * Quaternion[0, 0]; result[0, 7] = 2.0f * Quaternion[0, 2] * Quaternion[0, 3] - 2.0f * Quaternion[0, 1] * Quaternion[0, 0]; result[0, 8] = -1.0f +2.0f * Quaternion[0, 0] * Quaternion[0, 0] + 2.0f * Quaternion[0, 3] * Quaternion[0,3 ]; return result; }
public static MatrixLibrary.Matrix getQuaternionFromAngles2(MatrixLibrary.Matrix angle) { /* Example we take the 90 degree rotation from this: to this: As shown here the axis angle for this rotation is: heading = 0 degrees bank = 90 degrees attitude = 0 degrees so substituting this in the above formula gives: c1 = cos(heading / 2) = 1 c2 = cos(attitude / 2) = 1 c3 = cos(bank / 2) = 0.7071 s1 = sin(heading / 2) = 0 s2 = sin(attitude / 2) = 0 s3 = sin(bank / 2) = 0.7071 w = c1 c2 c3 - s1 s2 s3 = 0.7071 x = s1 s2 c3 +c1 c2 s3 = 0.7071 y = s1 c2 c3 + c1 s2 s3 = 0 z = c1 s2 c3 - s1 c2 s3 = 0 which gives the quaternion 0.7071 + i 0.7071 */ MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1); angle = (angle * Math.PI) / 180.0; double c1 = Math.Cos(angle[2, 0] / 2); double c2 = Math.Cos(angle[1, 0] / 2); double c3 = Math.Cos(angle[0, 0] / 2); double s1 = Math.Sin(angle[2, 0] / 2); double s2 = Math.Sin(angle[1, 0] / 2); double s3 = Math.Sin(angle[0, 0] / 2); result[0, 0] = c1 * c2 * c3 - s1 * s2 * s3; result[1, 0] = s1 * s2 * c3 + c1 * c2 * s3; result[2, 0] = s1 * c2 * c3 + c1 * s2 * s3; result[3, 0] = c1 * s2 * c3 - s1 * c2 * s3; return result; }
public static MatrixLibrary.Matrix getQuaternionFromAngles(MatrixLibrary.Matrix angle) { MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1); angle = (angle * Math.PI) / 180.0; double c1 = Math.Cos(angle[0, 0] / 2); double c2 = Math.Cos(angle[1, 0] / 2); double c3 = Math.Cos(angle[2, 0] / 2); double s1 = Math.Sin(angle[0, 0] / 2); double s2 = Math.Sin(angle[1, 0] / 2); double s3 = Math.Sin(angle[2, 0] / 2); result[0, 0] = c1 * c2 * c3 + s1 * s2 * s3; result[1, 0] = ( s1 * c2 * c3 - c1 * s2 * s3); result[2, 0] = c1 * s2 * c3 + s1 * c2 * s3; result[3, 0] = c1 * c2 * s3 - s1 * s2 * c3; return result; }