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 override void filterStep(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z, double m_x, double m_y, double m_z) { // normalise the accelerometer measurement double norm = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z); a_x /= norm; a_y /= norm; a_z /= norm; // normalise the magnetometer measurement norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z); m_x /= norm; m_y /= norm; m_z /= norm; if (AccObservX.Count < 10) { AccObservX.Add(a_x); AccObservY.Add(a_y); AccObservZ.Add(a_z); } else { AccObservX.RemoveAt(0); AccObservY.RemoveAt(0); AccObservZ.RemoveAt(0); AccObservX.Add(a_x); AccObservY.Add(a_y); AccObservZ.Add(a_z); } if (MagnObservX.Count < 10) { MagnObservX.Add(m_x); MagnObservY.Add(m_y); MagnObservZ.Add(m_z); } else { MagnObservX.RemoveAt(0); MagnObservY.RemoveAt(0); MagnObservZ.RemoveAt(0); MagnObservX.Add(m_x); MagnObservY.Add(m_y); MagnObservZ.Add(m_z); //Filter stabilization accFilter.Applyfilter(AccObservX, out AccFiltX); accFilter.Applyfilter(AccObservY, out AccFiltY); accFilter.Applyfilter(AccObservZ, out AccFiltZ); magnFilter.Applyfilter(MagnObservX, out MagnFiltX); magnFilter.Applyfilter(MagnObservY, out MagnFiltY); magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ); } if (step < 10) { } else { accFilter.Applyfilter(AccObservX, out AccFiltX); accFilter.Applyfilter(AccObservY, out AccFiltY); accFilter.Applyfilter(AccObservZ, out AccFiltZ); magnFilter.Applyfilter(MagnObservX, out MagnFiltX); magnFilter.Applyfilter(MagnObservY, out MagnFiltY); magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ); a_x = AccFiltX.Last(); a_y = AccFiltY.Last(); a_z = AccFiltZ.Last(); m_x = MagnFiltX.Last(); m_y = MagnFiltY.Last(); m_z = MagnFiltZ.Last(); //Magnetometer Calibration values //m_x =m_x*1.2+ 0.3; //m_y = m_y*1.1+0.04; //m_z += -0.08; double normA = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z); double normM = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z); a_x /= normA; a_y /= normA; a_z /= normA; m_x /= normM; m_y /= normM; m_z /= normM; w_x = (w_x - gyroOffRoll) * Math.PI / 180; w_y = (w_y - gyroOffPitch) * Math.PI / 180; w_z = (w_z - gyroOffYaw) * Math.PI / 180; MatrixLibrary.Matrix dq = 0.5 * (MyQuaternion.quaternionProduct(qFilt, new MyQuaternion(0, w_x, w_y, w_z).getQuaternionAsVector())); qGyroFilt = qFilt + dq * dt; norm = Math.Sqrt(qGyroFilt[0, 0] * qGyroFilt[0, 0] + qGyroFilt[1, 0] * qGyroFilt[1, 0] + qGyroFilt[2, 0] * qGyroFilt[2, 0] + qGyroFilt[3, 0] * qGyroFilt[3, 0]); qGyroFilt /= norm; double dqNorm = Math.Sqrt(dq[0, 0] * dq[0, 0] + dq[1, 0] * dq[1, 0] + dq[2, 0] * dq[2, 0] + dq[3, 0] * dq[3, 0]); double mu = 10 * dqNorm * dt; if (this.obsMethod == 0) { qObserv = GradientDescent(a_x, a_y, a_z, m_x, m_y, m_z, mu, qFilt); } else { qObserv = gaussNewtonMethod(a_x, a_y, a_z, m_x, m_y, m_z, qFilt); } qFilt = qGyroFilt * k + qObserv * (1 - k); norm = Math.Sqrt(qFilt[0, 0] * qFilt[0, 0] + qFilt[1, 0] * qFilt[1, 0] + qFilt[2, 0] * qFilt[2, 0] + qFilt[3, 0] * qFilt[3, 0]); qFilt /= norm; } step++; }
public override void filterStep(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z, double m_x, double m_y, double m_z) { double norm; Matrix temp;//=new Matrix(4,4); Matrix dq = new Matrix(4, 1); double mu; double k; // normalise the accelerometer measurement norm = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z); a_x /= norm; a_y /= norm; a_z /= norm; // normalise the magnetometer measurement norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z); m_x /= norm; m_y /= norm; m_z /= norm; /* if (AccObservX.Count < 10) * { * AccObservX.Add(a_x); * AccObservY.Add(a_y); * AccObservZ.Add(a_z); * } * else * { * AccObservX.RemoveAt(0); * AccObservY.RemoveAt(0); * AccObservZ.RemoveAt(0); * * AccObservX.Add(a_x); * AccObservY.Add(a_y); * AccObservZ.Add(a_z); * } * if (MagnObservX.Count < 10) * { * MagnObservX.Add(m_x); * MagnObservY.Add(m_y); * MagnObservZ.Add(m_z); * } * else * { * MagnObservX.RemoveAt(0); * MagnObservY.RemoveAt(0); * MagnObservZ.RemoveAt(0); * MagnObservX.Add(m_x); * MagnObservY.Add(m_y); * MagnObservZ.Add(m_z); * * //Filter stabilization * accFilter.Applyfilter(AccObservX, out AccFiltX); * accFilter.Applyfilter(AccObservY, out AccFiltY); * accFilter.Applyfilter(AccObservZ, out AccFiltZ); * * magnFilter.Applyfilter(MagnObservX, out MagnFiltX); * magnFilter.Applyfilter(MagnObservY, out MagnFiltY); * magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ); * } * * if (countdata > 10) * { * a_x = AccFiltX.Last(); * a_y = AccFiltY.Last(); * a_z = AccFiltZ.Last(); * * m_x = MagnFiltX.Last(); * m_y = MagnFiltY.Last(); * m_z = MagnFiltZ.Last(); * * // normalise the accelerometer measurement * * norm = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z); * a_x /= norm; * a_y /= norm; * a_z /= norm; * * * // normalise the magnetometer measurement * norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z); * m_x /= norm; * m_y /= norm; * m_z /= norm; * } * * * * countdata++; */ // w_x = w_x - gyroOffRoll; // w_y = w_y - gyroOffPitch; // w_z = w_z - gyroOffYaw; w_x = w_x * Math.PI / 180; w_y = w_y * Math.PI / 180; w_z = w_z * Math.PI / 180; if (this.obsMethod == 0) { dq = 0.5 * (MyQuaternion.quaternionProduct(state_observed, new MyQuaternion(0.0, w_x, w_y, w_z).getQuaternionAsVector())); norm = Math.Sqrt(dq[0, 0] * dq[0, 0] + dq[1, 0] * dq[1, 0] + dq[2, 0] * dq[2, 0] + dq[3, 0] * dq[3, 0]); mu = 10 * norm * dt; state_observed = GradientDescent(a_x, a_y, a_z, m_x, m_y, m_z, mu, state_filtered); } else { state_observed = gaussNewtonMethod(a_x, a_y, a_z, m_x, m_y, m_z, state_filtered); } //KALMAN FILTERING //F matrix computing F[0, 0] = 1; F[0, 1] = -dt / 2 * w_x; F[0, 2] = -dt / 2 * w_y; F[0, 3] = -dt / 2 * w_z; F[1, 0] = dt / 2 * w_x; F[1, 1] = 1; F[1, 2] = dt / 2 * w_z; F[1, 3] = -dt / 2 * w_y; F[2, 0] = dt / 2 * w_y; F[2, 1] = -dt / 2 * w_z; F[2, 2] = 1; F[2, 3] = dt / 2 * w_x; F[3, 0] = dt / 2 * w_z; F[3, 1] = dt / 2 * w_y; F[3, 2] = -dt / 2 * w_x; F[3, 3] = 1; //state prediction state_predicted = Matrix.Multiply(F, state_filtered); //calculate the variance of the prediction P_predicted = Matrix.Multiply(F, P_Update); P_predicted = Matrix.Multiply(P_predicted, Matrix.Transpose(F)) + Q; //compute the gain of the filter K temp = Matrix.Add(Matrix.Multiply(Matrix.Multiply(H, P_predicted), Matrix.Transpose(H)), R); temp = Matrix.Inverse(temp); K = Matrix.Multiply(Matrix.Multiply(P_predicted, Matrix.Transpose(H)), temp); //update the state of the system (this is the output of the filter) temp = Matrix.Subtract(state_observed, Matrix.Multiply(H, state_predicted)); state_filtered = Matrix.Add(state_predicted, Matrix.Multiply(K, temp)); //compute the variance of the state filtered temp = Matrix.Subtract((new Matrix(Matrix.Identity(4))), Matrix.Multiply(K, H)); P_Update = Matrix.Multiply(temp, P_predicted); norm = Math.Sqrt(state_filtered[0, 0] * state_filtered[0, 0] + state_filtered[1, 0] * state_filtered[1, 0] + state_filtered[2, 0] * state_filtered[2, 0] + state_filtered[3, 0] * state_filtered[3, 0]); state_filtered = Matrix.ScalarDivide(norm, state_filtered); q_filt1 = (float)state_filtered[0, 0]; q_filt2 = (float)state_filtered[1, 0]; q_filt3 = (float)state_filtered[2, 0]; q_filt4 = (float)state_filtered[3, 0]; }
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 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 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(MyQuaternion q_a, MyQuaternion q_b) { MatrixLibrary.Matrix v1 = q_a.getQuaternionAsVector(); MatrixLibrary.Matrix v2 = q_b.getQuaternionAsVector(); return(quaternionProduct(v1, v2)); }
//*********************************************** // // data proccessing //********************************************** public void Data_proccessing(int n) { /* if (s.Length == 48) * { * * Int32.TryParse(s.Substring(1, 3), out MAG[0]); * Int32.TryParse(s.Substring(5, 3), out MAG[1]); * Int32.TryParse(s.Substring(9, 3), out MAG[2]); * * UInt32.TryParse(s.Substring(12, 4), out ACC[0]); * UInt32.TryParse(s.Substring(16, 4), out ACC[1]); * UInt32.TryParse(s.Substring(20, 4), out ACC[2]); * * UInt32.TryParse(s.Substring(24, 4), out GYRO[0]); * UInt32.TryParse(s.Substring(28, 4), out GYRO[1]); * UInt32.TryParse(s.Substring(32, 4), out GYRO[2]); * * UInt32.TryParse(s.Substring(36, 4), out ENCODER[0]); * UInt32.TryParse(s.Substring(40, 4), out ENCODER[1]); * UInt32.TryParse(s.Substring(44, 4), out ENCODER[2]); * * if (s.Substring(0, 1) == "-") * { * MAG[0] = -MAG[0]; * } * if (s.Substring(4, 1) == "-") * { * MAG[1] = -MAG[1]; * } * if (s.Substring(8, 1) == "-") * { * MAG[2] = -MAG[2]; * } * }*/ MAG[0] = (Int32)((Input[1] - 0x30) * 100 + (Input[2] - 0x30) * 10 + (Input[3] - 0x30)); MAG[1] = (Int32)((Input[5] - 0x30) * 100 + (Input[6] - 0x30) * 10 + (Input[7] - 0x30)); MAG[2] = (Int32)((Input[9] - 0x30) * 100 + (Input[10] - 0x30) * 10 + (Input[11] - 0x30)); ACC[0] = (UInt32)((Input[12] - 0x30) * 1000 + (Input[13] - 0x30) * 100 + (Input[14] - 0x30) * 10 + (Input[15] - 0x30)); ACC[1] = (UInt32)((Input[16] - 0x30) * 1000 + (Input[17] - 0x30) * 100 + (Input[18] - 0x30) * 10 + (Input[19] - 0x30)); ACC[2] = (UInt32)((Input[20] - 0x30) * 1000 + (Input[21] - 0x30) * 100 + (Input[22] - 0x30) * 10 + (Input[23] - 0x30)); GYRO[0] = (UInt32)((Input[24] - 0x30) * 1000 + (Input[25] - 0x30) * 100 + (Input[26] - 0x30) * 10 + (Input[27] - 0x30)); GYRO[1] = (UInt32)((Input[28] - 0x30) * 1000 + (Input[29] - 0x30) * 100 + (Input[30] - 0x30) * 10 + (Input[31] - 0x30)); GYRO[2] = (UInt32)((Input[32] - 0x30) * 1000 + (Input[33] - 0x30) * 100 + (Input[34] - 0x30) * 10 + (Input[35] - 0x30)); ENCODER[0] = (UInt32)((Input[36] - 0x30) * 1000 + (Input[37] - 0x30) * 100 + (Input[38] - 0x30) * 10 + (Input[39] - 0x30)); ENCODER[1] = (UInt32)((Input[40] - 0x30) * 1000 + (Input[41] - 0x30) * 100 + (Input[42] - 0x30) * 10 + (Input[43] - 0x30)); ENCODER[2] = (UInt32)((Input[44] - 0x30) * 1000 + (Input[45] - 0x30) * 100 + (Input[46] - 0x30) * 10 + (Input[47] - 0x30)); if (Input[0] == '-') { MAG[0] = -MAG[0]; } if (Input[4] == '-') { MAG[1] = -MAG[1]; } if (Input[8] == '-') { MAG[2] = -MAG[2]; } // } //calculate to real value for filter int a = MAG[0]; MAG[0] = -MAG[1]; MAG[1] = a; MAG[2] = MAG[2]; this.form_3DcuboidB.X = (float)MAG[0]; this.form_3DcuboidB.Y = (float)MAG[1]; this.form_3DcuboidB.Z = (float)MAG[2]; ax = ((float)ACC[0] - (float)ACC_OFFSET[0]) * Acc_sensity; // ax real ay = ((float)ACC[1] - (float)ACC_OFFSET[1]) * Acc_sensity; // ay real az = ((float)ACC[2] - (float)ACC_OFFSET[2]) * Acc_sensity; gx = -((float)GYRO[0] - (float)GYRO_OFFSET[0]) * Gyro_sensity; gy = ((float)GYRO[1] - (float)GYRO_OFFSET[1]) * Gyro_sensity; gz = ((float)GYRO[2] - (float)GYRO_OFFSET[2]) * Gyro_sensity; // float norm = (float)Math.Sqrt ( ax * ax + ay * ay + az * az ); // ax /= norm; // ay /= norm; // az /= norm; // // // En_Angle[1] = (float)((ENCODER[0] / 4095.0f) * 2.0f * Math.PI); En_Angle[0] = (float)((ENCODER[1] / 4095.0f) * 2.0f * Math.PI); En_Angle[2] = (float)((ENCODER[2] / (6395.0f)) * 2.0f * Math.PI); if (En_Angle[0] > Math.PI) { En_Angle[0] -= (float)Math.PI * 2; } if (En_Angle[1] > Math.PI) { En_Angle[1] -= (float)Math.PI * 2; } if (En_Angle[2] > Math.PI) { En_Angle[2] -= (float)Math.PI * 2; } /* if(( En_Angle[0]==0)&& (Pre_En_Angle[0]==0)) * { * En_Angle[0]=0; * } * if( ( En_Angle[0]==0) & (Pre_En_Angle[0] != 0)) * { * En_Angle[0]=Pre_En_Angle[0]; * } * if (( En_Angle[0] != 0 )) * { * Pre_En_Angle[0] = En_Angle[0]; * } * if (( En_Angle[1] == 0 ) & ( Pre_En_Angle[1] == 0 )) * { * En_Angle[1] = 0; * } * if (( En_Angle[1] == 0 ) & ( Pre_En_Angle[1] != 0 )) * { * En_Angle[1] = Pre_En_Angle[1]; * } * if (( En_Angle[1] != 0 ) ) * { * Pre_En_Angle[1] = En_Angle[1]; * }*/ // // //this for Encoder // // En_Angle[0] = 1; // En_Angle[1] = 1; // En_Angle[2] = 1; // float[] Quatemp = MyQuaternion.getQuaternionFromAngles3(En_Angle); //En_Angle = MyQuaternion.getAnglesFromQuaternion2(Quatemp); this.form_3DcuboidA.RotationMatrix = MyQuaternion.getRotaionMatrixFromAngle(En_Angle); // this.form_3DcuboidA.RotationMatrix = MyQuaternion.getRotaionMatrixFromAngle ( En_Angle ); //MatrixLibrary.Matrix mat = new MatrixLibrary.Matrix(3,3); //mat[0,0] = rotation[0]; // mat[0, 1] = rotation[1]; //mat[0, 2] = rotation[2]; //mat[1, 0] = rotation[3]; //mat[1, 1] = rotation[4]; //mat[1, 2] = rotation[5]; //mat[2, 0] = rotation[6]; //mat[2, 1] = rotation[7]; //mat[2, 2] = rotation[8]; //double[] dtemp = MyQuaternion.getAngleFromRotation(mat); //float[] ftemp = new float[3]; //ftemp[0] = (float)dtemp[0]; //ftemp[1] = (float)dtemp[1]; //ftemp[2] = (float)dtemp[2]; //this.form_3DcuboidC.RotationMatrix = MyQuaternion.getRotaionMatrixFromAngle ( ftemp ); //gx =(float) HPfilterp.step ((double) gx ); // 16-bit uint ADC gyrocope values to rad/s (HP filtered) //gy = (float)HPfilterp.step ( (double)gy); //gz = (float)HPfilterp.step ( (double)gz); // //update // // AHRS.Update(gx,gy,gz,ax,ay,az); // Quaternion = AHRS.Quaternion; // this.form_3DcuboidA.RotationMatrix = MyQuaternion.getRotationMatrixFromQuaternion3 ( Quatemp ); // //send data to 3D form // this.form_3DcuboidB.Accx = (float)ax; this.form_3DcuboidB.Accy = (float)ay; this.form_3DcuboidB.Accz = (float)az; this.form_3DcuboidB.Gyrox = (float)gx; this.form_3DcuboidB.Gyroy = (float)gy; this.form_3DcuboidB.Gyroz = (float)gz; // //for accellero only // AHRS.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az); if (n == 1) { Kalman.filterStep((double)gx * 180.00f / Math.PI, (double)gy * 180.00f / Math.PI, (double)gz * 180.00f / Math.PI, (double)ax, (double)ay, (double)az, (double)MAG[0], (double)MAG[1], (double)MAG[2]); // Kalman.filterStep((double)gx , (double)gy, (double)gz, (double)ax, (double)ay, (double)az, (double)MAG[0], (double)MAG[1], (double)MAG[2]); Quaternion[0] = Kalman.q_filt1; Quaternion[1] = Kalman.q_filt2; Quaternion[2] = Kalman.q_filt3; Quaternion[3] = Kalman.q_filt4; } else { AHRS.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az, (float)MAG[0], (float)MAG[1], (float)MAG[2]);// Quaternion = AHRS.Quaternion; } // Quaternion[0] = 1.0f; // Quaternion[1] = 0.5f; // Quaternion[2] = 0.0f; //Quaternion[3] = 0.0f; //float[] Angle3 = MyQuaternion.getAnglesFromQuaternion2(Quaternion); //this.form_3DcuboidB.X = (float)Angle3[0]; //this.form_3DcuboidB.Y = (float)Angle3[1]; //this.form_3DcuboidB.Z = (float)Angle3[2]; // float[] temp1 = MyQuaternion.getRotationMatrixFromQuaternion3(Quaternion); // MatrixLibrary.Matrix mat = new MatrixLibrary.Matrix ( 3, 3 ); float[] temp1 = MyQuaternion.getRotationMatrixFromQuaternion3(Quaternion); float[] dtemp = MyQuaternion.getAnglesFromQuaternion2(Quaternion); /* mat[0, 0] = temp1[0]; * mat[0, 1] = temp1[1]; * mat[0, 2] = temp1[2]; * mat[1, 0] = temp1[3]; * mat[1, 1] = temp1[4]; * mat[1, 2] = temp1[5]; * mat[2, 0] = temp1[6]; * mat[2, 1] = temp1[7]; * mat[2, 2] = temp1[8];*/ // double[] dtemp = MyQuaternion.getAngleFromRotation ( mat ); //m double[] Angle5 = new double[3]; //m Angle5[0] = En_Angle2[0]; //m Angle5[1] = En_Angle2[1]; //m Angle5[2] = dtemp[2]; // dtemp[2] = En_Angle[2]; // this.form_3DcuboidB.Gyrox = (float)(dtemp[0] * 180 / Math.PI) ;//Angle5[0]; // this.form_3DcuboidB.Gyroy = (float)(dtemp[1] * 180 / Math.PI);//Angle5[1]; // this.form_3DcuboidB.Gyroz = (float)(dtemp[2] * 180 / Math.PI);//]Angle5[2]; // float[] temp3 = MyQuaternion.getRotaionMatrixFromAngle2(dtemp);//Angle5 this.form_3DcuboidB.RotationMatrix = temp1; // //Error angle // /*m ERROR[0] = (En_Angle[0] -(float)Angle5[0])*180 / (float)Math.PI; * ERROR[1] = En_Angle[1] - (float)Angle5[1] * 180 / (float)Math.PI;*/ // ERROR[0] = (float)(dtemp[0] * 180 / Math.PI); ; //ERROR[1] = (float)(En_Angle[0] * 180 / Math.PI); // ERROR[2] = (float)(dtemp[0] * 180 / Math.PI); //ERROR[3] = 0; // LIST[0] = (float)(dtemp[0] * 180 / Math.PI); LIST[1] = (float)(En_Angle[0] * 180 / Math.PI); LIST[2] = (float)(dtemp[1] * 180 / Math.PI); LIST[3] = (float)(En_Angle[1] * 180 / Math.PI); LIST[4] = (float)(dtemp[2] * 180 / Math.PI); LIST[5] = (float)(En_Angle[2] * 180 / Math.PI); plotForm.add_graph = LIST; // // // // /*MatrixLibrary.Matrix Quaternion12 = new MatrixLibrary.Matrix ( 4, 1 ); * MatrixLibrary.Matrix Angle4 = new MatrixLibrary.Matrix ( 3, 1 ); * Quaternion12[0,0] = 1/Math.Sqrt(4); * Quaternion12[1, 0] =- 1 / Math.Sqrt ( 4); * Quaternion12[2, 0] = -1 / Math.Sqrt ( 4 ); ; * Quaternion12[3, 0] = -1 / Math.Sqrt ( 4 ); * * Angle4 = MyQuaternion.getAnglesFromQuaternion ( Quaternion12 ); * MatrixLibrary.Matrix Temp_AN = new MatrixLibrary.Matrix ( 3, 1 ); * MatrixLibrary.Matrix QuaRe = new MatrixLibrary.Matrix ( 4, 1 ); * QuaRe = MyQuaternion.getQuaternionFromAngles2 ( Angle4 ); * QuaRe = MyQuaternion.getQuaternionFromAngles ( Angle4 ); * */ //this.form_3DcuboidB.RotationMatrix = temp1; //QuaRe = MyQuaternion.getQuaternionFromAngles ( Angle4 ); }
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); }
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); }
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()); }