Exemplo n.º 1
0
        public void reset()
        {
            sensorTimeStampGyro = 0L;
            sensorTimeStampAcc  = 0L;
            sensorTimeStampMag  = 0L;

            so3SensorFromWorld.setIdentity();
            so3LastMotion.setIdentity();

            double initialSigmaP = 5.0D;

            mP.setZero();
            mP.setSameDiagonal(initialSigmaP * initialSigmaP);

            double initialSigmaQ = 1.0D;

            mQ.setZero();
            mQ.setSameDiagonal(initialSigmaQ * initialSigmaQ);

            double initialSigmaR = 0.25D;

            mR.setZero();
            mR.setSameDiagonal(initialSigmaR * initialSigmaR);

            double initialSigmaRaccel = 0.75D;

            mRaccel.setZero();
            mRaccel.setSameDiagonal(initialSigmaRaccel * initialSigmaRaccel);

            mS.setZero();
            mH.setZero();
            mK.setZero();
            mNu.setZero();
            mz.setZero();
            mh.setZero();
            mu.setZero();
            mx.setZero();

            down.set(0.0D, 0.0D, 9.810000000000001D);
            north.set(0.0D, 1.0D, 0.0D);
        }