Fixed bug for GPS/INS mode