// saves time elaspsed so we know how much time was spent moving between images
public void mark() {
time.stop();
T = (double) time.getLastTaskTimeMillis();
T /= 1000; // convert to seconds
time.start();
}
public IMU() {
time = new StopWatch();
time.start();
Vxyzrpy = new double[6];
}