コード例 #1
0
        public ComplementaryFilter()
        {
            k                  = 0.98;
            anglesGyro         = new double[3];
            anglesEuler        = new double[3];
            anglesFiltered     = new double[3];
            anglesGyroFiltered = new double[3];

            aAcc  = new List <double>();
            bAcc  = new List <double>();
            aMagn = new List <double>();
            bMagn = new List <double>();

            aAcc.Add(1);
            aAcc.Add(-2.9529);
            aAcc.Add(2.9069);
            aAcc.Add(-0.954);

            bAcc.Add(0.000001597);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000001597);

            accFilter = new IIRFilter(aAcc, bAcc);

            aMagn.Add(1);
            aMagn.Add(-1.73);
            aMagn.Add(0.76);

            bMagn.Add(0.0078);
            bMagn.Add(0.0156);
            bMagn.Add(0.0078);

            magnFilter = new IIRFilter(aMagn, bMagn);

            AccObservX = new List <double>();
            AccObservY = new List <double>();
            AccObservZ = new List <double>();
            AccFiltX   = new List <double>();
            AccFiltY   = new List <double>();
            AccFiltZ   = new List <double>();

            MagnObservX = new List <double>();
            MagnObservY = new List <double>();
            MagnObservZ = new List <double>();
            MagnFiltX   = new List <double>();
            MagnFiltY   = new List <double>();
            MagnFiltZ   = new List <double>();
        }
コード例 #2
0
        public QuaternionCF()
        {
            k     = 0.98;
            aAcc  = new List <double>();
            bAcc  = new List <double>();
            aMagn = new List <double>();
            bMagn = new List <double>();

            aAcc.Add(1);
            aAcc.Add(-2.9529);
            aAcc.Add(2.9069);
            aAcc.Add(-0.954);

            bAcc.Add(0.000001597);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000001597);

            accFilter = new IIRFilter(aAcc, bAcc);

            aMagn.Add(1);
            aMagn.Add(-1.73);
            aMagn.Add(0.76);

            bMagn.Add(0.0078);
            bMagn.Add(0.0156);
            bMagn.Add(0.0078);

            magnFilter = new IIRFilter(aMagn, bMagn);

            AccObservX = new List <double>();
            AccObservY = new List <double>();
            AccObservZ = new List <double>();
            AccFiltX   = new List <double>();
            AccFiltY   = new List <double>();
            AccFiltZ   = new List <double>();

            MagnObservX = new List <double>();
            MagnObservY = new List <double>();
            MagnObservZ = new List <double>();
            MagnFiltX   = new List <double>();
            MagnFiltY   = new List <double>();
            MagnFiltZ   = new List <double>();

            qFilt     = new MatrixLibrary.Matrix(4, 1);
            qGyroFilt = new MatrixLibrary.Matrix(4, 1);
            qObserv   = new MatrixLibrary.Matrix(4, 1);

            qFilt[0, 0] = 1;
            qFilt[1, 0] = 0;
            qFilt[2, 0] = 0;
            qFilt[3, 0] = 0;

            qGyroFilt[0, 0] = 1;
            qGyroFilt[1, 0] = 0;
            qGyroFilt[2, 0] = 0;
            qGyroFilt[3, 0] = 0;

            qObserv[0, 0] = 1;
            qObserv[1, 0] = 0;
            qObserv[2, 0] = 0;
            qObserv[3, 0] = 0;
        }
コード例 #3
0
        public KalmanFilter()
        {
            //Compute matrix Q (costant if the time interval is costant)
            Q[0, 0] = sigmaRoll + sigmaPitch + sigmaYaw;
            Q[0, 1] = -sigmaRoll + sigmaPitch - sigmaYaw;
            Q[0, 2] = -sigmaRoll - sigmaPitch + sigmaYaw;
            Q[0, 3] = sigmaRoll - sigmaPitch - sigmaYaw;
            Q[1, 0] = -sigmaRoll + sigmaPitch - sigmaYaw;
            Q[1, 1] = sigmaRoll + sigmaPitch + sigmaYaw;
            Q[1, 2] = sigmaRoll - sigmaPitch - sigmaYaw;
            Q[1, 3] = -sigmaRoll - sigmaPitch + sigmaYaw;
            Q[2, 0] = -sigmaRoll - sigmaPitch + sigmaYaw;
            Q[2, 1] = sigmaRoll - sigmaPitch - sigmaYaw;
            Q[2, 2] = sigmaRoll + sigmaPitch + sigmaYaw;
            Q[2, 3] = -sigmaRoll + sigmaPitch - sigmaYaw;
            Q[3, 0] = sigmaRoll - sigmaPitch - sigmaYaw;
            Q[3, 1] = -sigmaRoll - sigmaPitch + sigmaYaw;
            Q[3, 2] = -sigmaRoll + sigmaPitch - sigmaYaw;
            Q[3, 3] = sigmaRoll + sigmaPitch + sigmaYaw;

            R       = new Matrix(4, 4);
            R[0, 0] = 0.5;
            R[0, 1] = 0;
            R[0, 2] = 0;
            R[0, 3] = 0;

            R[1, 0] = 0;
            R[1, 1] = 0.5;
            R[1, 2] = 0;
            R[1, 3] = 0;

            R[2, 0] = 0;
            R[2, 1] = 0;
            R[2, 2] = 0.5;
            R[2, 3] = 0;

            R[3, 0] = 0;
            R[3, 1] = 0;
            R[3, 2] = 0;
            R[3, 3] = 0.5;



            //Initial conditions
            state_observed  = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
            state_filtered  = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
            state_predicted = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
            P_Update        = Matrix.ScalarMultiply(0.1, new Matrix(Matrix.Identity(4)));

            aAcc  = new List <double>();
            bAcc  = new List <double>();
            aMagn = new List <double>();
            bMagn = new List <double>();

            aAcc.Add(1);
            aAcc.Add(-2.9529);
            aAcc.Add(2.9069);
            aAcc.Add(-0.954);

            bAcc.Add(0.000001597);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000001597);

            accFilter = new IIRFilter(aAcc, bAcc);

            aMagn.Add(1);
            aMagn.Add(-1.73);
            aMagn.Add(0.76);

            bMagn.Add(0.0078);
            bMagn.Add(0.0156);
            bMagn.Add(0.0078);

            magnFilter = new IIRFilter(aMagn, bMagn);

            AccObservX = new List <double>();
            AccObservY = new List <double>();
            AccObservZ = new List <double>();
            AccFiltX   = new List <double>();
            AccFiltY   = new List <double>();
            AccFiltZ   = new List <double>();

            MagnObservX = new List <double>();
            MagnObservY = new List <double>();
            MagnObservZ = new List <double>();
            MagnFiltX   = new List <double>();
            MagnFiltY   = new List <double>();
            MagnFiltZ   = new List <double>();

            weOld = new Matrix(4, 1);
            weOld = Matrix.ScalarMultiply(0, weOld);
        }
コード例 #4
0
        public KalmanFilter()
        {
            //Compute matrix Q (costant if the time interval is costant)
            Q[0, 0] = sigmaRoll + sigmaPitch + sigmaYaw;
            Q[0, 1] = -sigmaRoll + sigmaPitch - sigmaYaw;
            Q[0, 2] = -sigmaRoll - sigmaPitch + sigmaYaw;
            Q[0, 3] = sigmaRoll - sigmaPitch - sigmaYaw;
            Q[1, 0] = -sigmaRoll + sigmaPitch - sigmaYaw;
            Q[1, 1] = sigmaRoll + sigmaPitch + sigmaYaw;
            Q[1, 2] = sigmaRoll - sigmaPitch - sigmaYaw;
            Q[1, 3] = -sigmaRoll - sigmaPitch + sigmaYaw;
            Q[2, 0] = -sigmaRoll - sigmaPitch + sigmaYaw;
            Q[2, 1] = sigmaRoll - sigmaPitch - sigmaYaw;
            Q[2, 2] = sigmaRoll + sigmaPitch + sigmaYaw;
            Q[2, 3] = -sigmaRoll + sigmaPitch - sigmaYaw;
            Q[3, 0] = sigmaRoll - sigmaPitch - sigmaYaw;
            Q[3, 1] = -sigmaRoll - sigmaPitch + sigmaYaw;
            Q[3, 2] = -sigmaRoll + sigmaPitch - sigmaYaw;
            Q[3, 3] = sigmaRoll + sigmaPitch + sigmaYaw;

            R = new Matrix(4, 4);
            R[0, 0] = 0.5;
            R[0, 1] = 0;
            R[0, 2] = 0;
            R[0, 3] = 0;

            R[1, 0] = 0;
            R[1, 1] = 0.5;
            R[1, 2] = 0;
            R[1, 3] = 0;

            R[2, 0] = 0;
            R[2, 1] = 0;
            R[2, 2] = 0.5;
            R[2, 3] = 0;

            R[3, 0] = 0;
            R[3, 1] = 0;
            R[3, 2] = 0;
            R[3, 3] = 0.5;

            //Initial conditions
            state_observed = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
            state_filtered = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
            state_predicted = new MyQuaternion(0.5, 0.5, 0.5, 0.5).getQuaternionAsVector();
            P_Update = Matrix.ScalarMultiply(0.1, new Matrix(Matrix.Identity(4)));

            aAcc = new List<double>();
            bAcc = new List<double>();
            aMagn = new List<double>();
            bMagn = new List<double>();

            aAcc.Add(1);
            aAcc.Add(-2.9529);
            aAcc.Add(2.9069);
            aAcc.Add(-0.954);

            bAcc.Add(0.000001597);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000001597);

            accFilter = new IIRFilter(aAcc, bAcc);

            aMagn.Add(1);
            aMagn.Add(-1.73);
            aMagn.Add(0.76);

            bMagn.Add(0.0078);
            bMagn.Add(0.0156);
            bMagn.Add(0.0078);

            magnFilter = new IIRFilter(aMagn, bMagn);

            AccObservX = new List<double>();
            AccObservY = new List<double>();
            AccObservZ = new List<double>();
            AccFiltX = new List<double>();
            AccFiltY = new List<double>();
            AccFiltZ = new List<double>();

            MagnObservX = new List<double>();
            MagnObservY = new List<double>();
            MagnObservZ = new List<double>();
            MagnFiltX = new List<double>();
            MagnFiltY = new List<double>();
            MagnFiltZ = new List<double>();

            weOld = new Matrix(4, 1);
            weOld = Matrix.ScalarMultiply(0, weOld);
        }
コード例 #5
0
        public QuaternionCF()
        {
            k = 0.98;
            aAcc = new List<double>();
            bAcc = new List<double>();
            aMagn = new List<double>();
            bMagn = new List<double>();

            aAcc.Add(1);
            aAcc.Add(-2.9529);
            aAcc.Add(2.9069);
            aAcc.Add(-0.954);

            bAcc.Add(0.000001597);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000004792);
            bAcc.Add(0.000001597);

            accFilter = new IIRFilter(aAcc, bAcc);

            aMagn.Add(1);
            aMagn.Add(-1.73);
            aMagn.Add(0.76);

            bMagn.Add(0.0078);
            bMagn.Add(0.0156);
            bMagn.Add(0.0078);

            magnFilter = new IIRFilter(aMagn, bMagn);

            AccObservX = new List<double>();
            AccObservY = new List<double>();
            AccObservZ = new List<double>();
            AccFiltX = new List<double>();
            AccFiltY = new List<double>();
            AccFiltZ = new List<double>();

            MagnObservX = new List<double>();
            MagnObservY = new List<double>();
            MagnObservZ = new List<double>();
            MagnFiltX = new List<double>();
            MagnFiltY = new List<double>();
            MagnFiltZ = new List<double>();

            qFilt = new MatrixLibrary.Matrix(4, 1);
            qGyroFilt = new MatrixLibrary.Matrix(4, 1);
            qObserv = new MatrixLibrary.Matrix(4, 1);

            qFilt[0, 0] = 1;
            qFilt[1, 0] = 0;
            qFilt[2, 0] = 0;
            qFilt[3, 0] = 0;

            qGyroFilt[0, 0] = 1;
            qGyroFilt[1, 0] = 0;
            qGyroFilt[2, 0] = 0;
            qGyroFilt[3, 0] = 0;

            qObserv[0, 0] = 1;
            qObserv[1, 0] = 0;
            qObserv[2, 0] = 0;
            qObserv[3, 0] = 0;
        }