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