Example #1
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];
        }
Example #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)
        {
            a_x = a_x / 1000;
            a_y = a_y / 1000;
            a_z = a_z / 1000;

            // 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)
            {
                anglesEuler           = computeAnglesFromAccAndMagn(new double[] { a_x, a_y, a_z }, new double[] { m_x, m_y, m_z });
                anglesFiltered[0]     = anglesEuler[0];
                anglesFiltered[1]     = anglesEuler[1];
                anglesFiltered[2]     = anglesEuler[2];
                anglesGyro[0]         = anglesEuler[0];
                anglesGyro[1]         = anglesEuler[1];
                anglesGyro[2]         = anglesEuler[2];
                anglesGyroFiltered[0] = anglesEuler[0];
                anglesGyroFiltered[1] = anglesEuler[1];
                anglesGyroFiltered[2] = anglesEuler[2];
            }
            else
            {
                //Filtering
                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();

                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;
                w_y = w_y - gyroOffPitch;
                w_z = -w_z + gyroOffYaw;

                //w_x = (w_x + w_x_old) / 2;
                //w_y = (w_y + w_y_old) / 2;
                //w_z = (w_z + w_z_old) / 2;

                anglesGyro[0] = anglesGyro[0] + w_x * dt;
                anglesGyro[1] = anglesGyro[1] + w_y * dt;
                anglesGyro[2] = anglesGyro[2] + w_z * dt;

                double[] dEulerGyroFiltered = computeEulerFromGyro(new double[] { w_x, w_y, w_z }, anglesFiltered);

                dEulerGyroFiltered[0] = dEulerGyroFiltered[0] * dt;
                dEulerGyroFiltered[1] = dEulerGyroFiltered[1] * dt;
                dEulerGyroFiltered[2] = dEulerGyroFiltered[2] * dt;

                anglesGyroFiltered[0] = anglesFiltered[0] + dEulerGyroFiltered[0];
                anglesGyroFiltered[1] = anglesFiltered[1] + dEulerGyroFiltered[1];
                anglesGyroFiltered[2] = anglesFiltered[2] + dEulerGyroFiltered[2];

                anglesEuler = computeAnglesFromAccAndMagn(new double[] { a_x, a_y, a_z }, new double[] { m_x, m_y, m_z });

                for (int j = 0; j < 3; j++)
                {
                    if ((180 - Math.Abs(anglesEuler[j])) < 15 && (180 - Math.Abs(anglesGyroFiltered[j])) < 15)
                    {
                        if (Math.Sign(anglesEuler[j]) != Math.Sign(anglesGyroFiltered[j]))
                        {
                            anglesGyroFiltered[j] = Math.Sign(anglesEuler[j]) * (360 - Math.Abs(anglesGyroFiltered[j]));
                        }
                    }
                }

                anglesFiltered[0] = k * anglesGyroFiltered[0] + (1 - k) * anglesEuler[0];
                anglesFiltered[1] = k * anglesGyroFiltered[1] + (1 - k) * anglesEuler[1];
                anglesFiltered[2] = k * anglesGyroFiltered[2] + (1 - k) * anglesEuler[2];

                anglesFiltered     = limitRotations(anglesFiltered);
                anglesGyroFiltered = limitRotations(anglesGyroFiltered);
            }
            step++;
        }
Example #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++;
        }