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];
}