Пример #1
0
        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);
        }
Пример #2
0
        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++;
        }
Пример #3
0
        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];
        }
Пример #4
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();
        }
Пример #5
0
        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;
        }
Пример #6
0
        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;
        }
Пример #7
0
 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 );
        }
Пример #9
0
 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);
 }
Пример #10
0
        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);
        }
Пример #11
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());
        }