EKF: Change the EKF initialization to

1) init tilt
2) init yaw
3) init tilt uncertainty
4) init yaw uncertainty

Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>

EKF/ekf.cpp

@@ -233,9 +233,6 @@ bool Ekf::initialiseFilter()
 		_state.mag_B.setZero();
 		_state.wind_vel.setZero();
 
-		// initialise the state covariance matrix
-		initialiseCovariance();
-
 		// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
 		float pitch = 0.0f;
 		float roll = 0.0f;
@@ -260,7 +257,19 @@ bool Ekf::initialiseFilter()
 		// calculate the initial magnetic field and yaw alignment
 		// Get the magnetic declination
 		calcMagDeclination();
-		_control_status.flags.yaw_align = resetMagHeading(_mag_filt_state);
+		_control_status.flags.yaw_align = resetMagHeading(_mag_filt_state, false, false);
+
+		// initialise the state covariance matrix
+		initialiseCovariance();
+
+		// update the yaw angle variance using the variance of the measurement
+		if (_control_status.flags.ev_yaw) {
+			// using error estimate from external vision data TODO: this is never true
+			increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
+		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
+			// using magnetic heading tuning parameter
+			increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
+		}
 
 		if (_control_status.flags.rng_hgt) {
 			// if we are using the range finder as the primary source, then calculate the baro height at origin so  we can use baro as a backup

EKF/ekf.h

@@ -544,9 +544,9 @@ private:
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	void fuseHagl();
 
-	// reset the heading and magnetic field states using the declination and magnetometer measurements
+	// reset the heading and magnetic field states using the declination and magnetometer/external vision measurements
 	// return true if successful
-	bool resetMagHeading(Vector3f &mag_init);
+	bool resetMagHeading(Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer=true);
 
 	// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
 	// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.

EKF/ekf_helper.cpp

@@ -558,7 +558,7 @@ bool Ekf::realignYawGPS()
 }
 
 // Reset heading and magnetic field states
-bool Ekf::resetMagHeading(Vector3f &mag_init)
+bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update_buffer)
 {
 	// prevent a reset being performed more than once on the same frame
 	if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) {
@@ -737,24 +737,28 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 		resetExtVisRotMat();
 	}
 
-	// update the yaw angle variance using the variance of the measurement
-	if (_control_status.flags.ev_yaw) {
-		// using error estimate from external vision data
-		increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
-	} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
-		// using magnetic heading tuning parameter
-		increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
+	if (increase_yaw_var) {
+		// update the yaw angle variance using the variance of the measurement
+		if (_control_status.flags.ev_yaw) {
+			// using error estimate from external vision data
+			increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
+		} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
+			// using magnetic heading tuning parameter
+			increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
+		}
 	}
 
-	// add the reset amount to the output observer buffered data
-	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		// Note q1 *= q2 is equivalent to q1 = q2 * q1
-		_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
-	}
+	if (update_buffer) {
+		// add the reset amount to the output observer buffered data
+		for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
+			// Note q1 *= q2 is equivalent to q1 = q2 * q1
+			_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
+		}
 
-	// apply the change in attitude quaternion to our newest quaternion estimate
-	// which was already taken out from the output buffer
-	_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
+		// apply the change in attitude quaternion to our newest quaternion estimate
+		// which was already taken out from the output buffer
+		_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
+	}
 
 	// capture the reset event
 	_state_reset_status.quat_counter++;