Fix bug in GPS fusion mode 1 (2D velocity)