Exemplo n.º 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);
        }
Exemplo n.º 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)
        {
            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        = state_filtered[0, 0];
            q_filt2        = state_filtered[1, 0];
            q_filt3        = state_filtered[2, 0];
            q_filt4        = state_filtered[3, 0];
        }
Exemplo n.º 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)
        {
            // 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++;
        }