diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index c7c9b6476d..9b57dfd550 100644
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -1730,6 +1730,7 @@ void calcEarthRateNED(Vector3f &omega, float latitude)
 void CovarianceInit()
 {
     // Calculate the initial covariance matrix P
+    P[0][0]   = 0.25f*sq(1.0f*deg2rad);
     P[1][1]   = 0.25f*sq(1.0f*deg2rad);
     P[2][2]   = 0.25f*sq(1.0f*deg2rad);
     P[3][3]   = 0.25f*sq(10.0f*deg2rad);
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 5c977f6089..444597e810 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -392,8 +392,6 @@ FixedwingEstimator::task_main()
 
 	parameters_update();
 
-	Vector3f lastAngRate;
-	Vector3f lastAccel;
 	/* set initial filter state */
 	fuseVelData = false;
 	fusePosData = false;
@@ -402,6 +400,11 @@ FixedwingEstimator::task_main()
 	fuseVtasData = false;
 	statesInitialised = false;
 
+	/* initialize measurement data */
+	VtasMeas = 0.0f;
+	Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
+	Vector3f lastAccel = {0.0f, 0.0f, 0.0f};
+
 	/* wakeup source(s) */
 	struct pollfd fds[2];