position_estimator_inav: GPS delay compensation

Tested in HIL with simulated GPS delay, makes thing better. Going to test this in fligh soon.