[navigation] fix altitude in some extra nav routines

seems this was converted wrongly from the old estimator_z to Enu_f()->z instead of altitude above MSL
in commits 6603e1f0dfc2a48c4fc46ce1f634a3a2046f7367 and 0e9f8ed9476c66704a549ac20491f5fdb75bc616