updated to new kalman implementation
testing code for gyro drift compensation