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