示例#1
0
        public /*synchronized*/ void processGyro(float[] gyro, long sensorTimeStamp)
        {
            float kTimeThreshold = 0.04F;
            float kdTdefault     = 0.01F;

            if (sensorTimeStampGyro != 0L)
            {
                float dT = (float)(sensorTimeStamp - sensorTimeStampGyro) * 1.0E-09F;
                if (dT > kTimeThreshold)
                {
                    dT = gyroFilterValid ? filteredGyroTimestep : kdTdefault;
                }
                else
                {
                    filterGyroTimestep(dT);
                }

                mu.set(gyro[0] * -dT, gyro[1] * -dT, gyro[2] * -dT);
                So3Util.sO3FromMu(mu, so3LastMotion);

                processGyroTempM1.set(so3SensorFromWorld);
                Matrix3x3d.mult(so3LastMotion, so3SensorFromWorld, processGyroTempM1);
                so3SensorFromWorld.set(processGyroTempM1);

                updateCovariancesAfterMotion();

                processGyroTempM2.set(mQ);
                processGyroTempM2.scale(dT * dT);
                mP.plusEquals(processGyroTempM2);
            }
            sensorTimeStampGyro = sensorTimeStamp;
            lastGyro[0]         = gyro[0];
            lastGyro[1]         = gyro[1];
            lastGyro[2]         = gyro[2];
        }