Added gyro and accelerometer bias calibration