Fixed errors with gyro drift. Other fixes