gyro drift correction function added in MARG implementation