added stuff to detect and compensate for drift

It still isn't that great for maintaining absolute orientation... I
really should have gotten an accelerometer