Merge pull request #59 from PX4/pr-ImprovedStartup

Improvements to EKF startup - partially replaces PR#57

EKF/covariance.cpp

@@ -52,34 +52,37 @@ void Ekf::initialiseCovariance()
 		}
 	}
 
+	// calculate average prediction time step in sec
+	float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS;
+
 	// XXX use initial guess for the diagonal elements for the covariance matrix
 	// angle error
-	P[0][0] = 0.001f;
-	P[1][1] = 0.001f;
-	P[2][2] = 0.001f;
+	P[0][0] = 0.1f;
+	P[1][1] = 0.1f;
+	P[2][2] = 0.1f;
 
 	// velocity
-	P[3][3] = 0.1f;
-	P[4][4] = 0.1f;
-	P[5][5] = 0.1f;
+	P[3][3] = sq(fmaxf(_params.gps_vel_noise, 0.01f));
+	P[4][4] = P[3][3];
+	P[5][5] = sq(1.5f) * P[3][3];
 
 	// position
-	P[6][6] = 0.1f;
-	P[7][7] = 0.1f;
-	P[8][8] = 0.1f;
+	P[6][6] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
+	P[7][7] = P[6][6];
+	P[8][8] = sq(fmaxf(_params.baro_noise, 0.01f));
 
 	// gyro bias
-	P[9][9] = 0.00001f;
-	P[10][10] = 0.00001f;
-	P[11][11] = 0.00001f;
+	P[9][9] = sq(0.035f * dt);
+	P[10][10] = P[9][9];
+	P[11][11] = P[9][9];
 
 	// gyro scale
-	P[12][12] = 0.0001f;
-	P[13][13] = 0.0001f;
-	P[14][14] = 0.0001f;
+	P[12][12] = sq(0.001f);
+	P[13][13] = P[12][12];
+	P[14][14] = P[12][12];
 
 	// accel z bias
-	P[15][15] = 0.0001f;
+	P[15][15] = sq(0.2f * dt);
 
 	// variances for optional states
 	// these state variances are set to zero until the states are required, then they must be initialised

EKF/ekf.cpp

@@ -265,7 +265,7 @@ bool Ekf::initialiseFilter(void)
 		if (_delVel_sum.norm() > 0.001f) {
 			_delVel_sum.normalize();
 			pitch = asinf(_delVel_sum(0));
-			roll = -asinf(_delVel_sum(1) / cosf(pitch));
+			roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));
 
 		} else {
 			return false;

EKF/mag_fusion.cpp

@@ -610,9 +610,11 @@ void Ekf::fuseHeading()
 
 	// TODO - enable use of an off-board heading measurement
 
-	// rotate the magnetometer measurement into earth frame using an assumed zero yaw angle
+	// rotate the magnetometer measurement into earth frame
 	matrix::Euler<float> euler(_state.quat_nominal);
 	float predicted_hdg = euler(2); // we will need the predicted heading to calculate the innovation
+
+	// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
 	euler(2) = 0.0f;
 	matrix::Dcm<float> R_to_earth(euler);
 	matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;