Add gyro and accelerometer bias calibration function