position_estimator_inav: stability fix

Attempt to fix strange bug when position_estimator_inav ignores accel (crazy altitude oscillations as result).