Merge branch 'paul_estimator' of github.com:PX4/Firmware into paul_estimator

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);

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];