diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 58d76c0554..7d666873dd 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -189,6 +189,7 @@ bool Ekf::initialiseFilter(void)
 	}
 
 	_state.pos(2) = -baro_init.hgt;
+	_output_new.pos(2) = -baro_init.hgt;
 
 	initialiseCovariance();