ekf2: allow yaw estimator to run on ground (not at rest) and init yaw align

- add a common resetMagStates() that uses the WMM to reinit mag states (I & B) if possible

src/modules/ekf2/EKF/ekf.h

@@ -222,9 +222,11 @@ public:
 	bool isYawFinalAlignComplete() const
 	{
 		const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
+
 		const bool is_mag_alignment_in_flight_complete = is_using_mag
 				&& _control_status.flags.mag_aligned_in_flight
 				&& ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)1e6);
+
 		return _control_status.flags.yaw_align
 		       && (is_mag_alignment_in_flight_complete || !is_using_mag);
 	}
@@ -836,7 +838,9 @@ private:
 	void controlGpsFusion();
 	bool shouldResetGpsFusion() const;
 	bool hasHorizontalAidingTimedOut() const;
-	bool isYawFailure() const;
+
+	bool isYawError(float error_threshold = math::radians(10.f)) const;
+	bool isYawFailure() const { return isYawError(math::radians(25.f)); }
 
 	void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing);
 
@@ -862,6 +866,8 @@ private:
 	void runMagAndMagDeclFusions(const Vector3f &mag);
 	void run3DMagAndDeclFusions(const Vector3f &mag);
 
+	bool resetMagStates();
+
 	// control fusion of range finder observations
 	void controlRangeFinderFusion();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -356,22 +356,22 @@ bool Ekf::realignYawGPS(bool force)
 	}
 
 	// check for excessive horizontal GPS velocity innovations
-	const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.f) && _control_status.flags.gps;
+	const bool bad_vel_innov = (_gps_vel_test_ratio(0) > 1.f) && _control_status.flags.gps;
 
 	// calculate GPS course over ground angle
-	const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
+	const float gps_cog = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
 
 	// calculate course yaw angle
-	const float ekfCOG = atan2f(_state.vel(1), _state.vel(0));
+	const float ekf_cog = atan2f(_state.vel(1), _state.vel(0));
 
 	// Check the EKF and GPS course over ground for consistency
-	const float courseYawError = wrap_pi(gpsCOG - ekfCOG);
+	const float course_yaw_error = wrap_pi(gps_cog - ekf_cog);
 
 	// If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
-	const bool badYawErr = fabsf(courseYawError) > math::radians(25.f);
-	const bool badMagYaw = (badYawErr && badVelInnov);
+	const bool bad_yaw_error = fabsf(course_yaw_error) > math::radians(25.f);
+	const bool bad_mag_yaw = (bad_yaw_error && bad_vel_innov);
 
-	if (badMagYaw) {
+	if (bad_mag_yaw) {
 		_num_bad_flight_yaw_events++;
 
 		_warning_events.flags.bad_yaw_using_gps_course = true;
@@ -389,7 +389,9 @@ bool Ekf::realignYawGPS(bool force)
 	}
 
 	// correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
-	if (badMagYaw || !_control_status.flags.yaw_align || force) {
+	if (bad_mag_yaw || !_control_status.flags.yaw_align || force) {
+
+		ECL_INFO("realign yaw GPS resetting yaw");
 
 		// calculate new yaw estimate
 		float yaw_new;
@@ -398,7 +400,7 @@ bool Ekf::realignYawGPS(bool force)
 			// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
 			// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
 			const float current_yaw = getEulerYaw(_R_to_earth);
-			yaw_new = current_yaw + courseYawError;
+			yaw_new = current_yaw + course_yaw_error;
 			_control_status.flags.mag_aligned_in_flight = true;
 
 		} else if (_control_status.flags.wind) {
@@ -409,7 +411,7 @@ bool Ekf::realignYawGPS(bool force)
 
 		} else {
 			// we don't have wind estimates, so align yaw to the GPS velocity vector
-			yaw_new = gpsCOG;
+			yaw_new = gps_cog;
 		}
 
 		// use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment
@@ -419,37 +421,20 @@ bool Ekf::realignYawGPS(bool force)
 
 		// Apply updated yaw and yaw variance to states and covariances
 		resetQuatStateYaw(yaw_new, yaw_variance_new, true);
-
-		// Use the last magnetometer measurements to reset the field states
-		_state.mag_B.zero();
-		_state.mag_I = _R_to_earth * _mag_lpf.getState();
-
-		resetMagCov();
-
-		// record the start time for the magnetic field alignment
-		_flt_mag_align_start_time = _imu_sample_delayed.time_us;
+		_control_status.flags.yaw_align = true;
 
 		// If heading was bad, then we also need to reset the velocity and position states
-		if (badMagYaw) {
+		if (bad_mag_yaw && _control_status.flags.gps) {
 			resetVelocityToGps(_gps_sample_delayed);
 			resetHorizontalPositionToGps(_gps_sample_delayed);
 		}
 
-		return true;
-
-	} else {
-		// align mag states only
-
-		// calculate initial earth magnetic field states
-		_state.mag_I = _R_to_earth * _mag_lpf.getState();
-
-		resetMagCov();
-
-		// record the start time for the magnetic field alignment
-		_flt_mag_align_start_time = _imu_sample_delayed.time_us;
+		resetMagStates();
 
 		return true;
 	}
+
+	return false;
 }
 
 // Reset heading and magnetic field states
@@ -472,6 +457,8 @@ bool Ekf::resetMagHeading()
 
 	if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || ((_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) && heading_required_for_navigation)) {
 
+		ECL_INFO("reset mag heading resetting yaw");
+
 		const Vector3f mag_init = _mag_lpf.getState();
 
 		// rotate the magnetometer measurements into earth frame using a zero yaw angle
@@ -1523,6 +1510,9 @@ void Ekf::saveMagCovData()
 
 void Ekf::loadMagCovData()
 {
+	P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
+	P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
+
 	// re-instate variances for the XYZ body axis field
 	P(19, 19) = _saved_mag_bf_variance(0);
 	P(20, 20) = _saved_mag_bf_variance(1);
@@ -1556,6 +1546,48 @@ void Ekf::stopAirspeedFusion()
 void Ekf::startGpsFusion()
 {
 	if (!_control_status.flags.gps) {
+
+		bool yaw_reset_needed = false;
+
+		if (_control_status.flags.ev_yaw) {
+			// Stop the vision for yaw fusion and do not allow it to start again
+			stopEvYawFusion();
+
+			yaw_reset_needed = true;
+		}
+
+		if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
+			if (_mag_inhibit_yaw_reset_req || _mag_yaw_reset_req) {
+				yaw_reset_needed = true;
+			}
+		}
+
+		if (isYawEmergencyEstimateAvailable() && isYawError(math::radians(10.f))) {
+			yaw_reset_needed = true;
+		}
+
+		// Do not use external vision for yaw if using GPS because yaw needs to be
+		// defined relative to an NED reference frame
+		if (yaw_reset_needed) {
+			if (resetYawToEKFGSF()) {
+				ECL_INFO("starting GPS, reset yaw using yaw estimator");
+
+			} else if (resetYawToGps()) {
+				ECL_INFO("starting GPS, reset yaw to GPS");
+
+			} else if (realignYawGPS(true)) {
+				ECL_INFO("starting GPS, reset yaw using GPS course");
+
+			} else if (resetMagHeading()) {
+				ECL_INFO("starting GPS, reset yaw using mag");
+
+			} else {
+				// all failed
+				ECL_WARN("starting GPS, yaw reset failed");
+				_mag_yaw_reset_req = true;
+			}
+		}
+
 		resetHorizontalPositionToGps(_gps_sample_delayed);
 
 		// when already using another velocity source velocity reset is not necessary
@@ -1744,30 +1776,25 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 // Returns true if the reset was successful
 bool Ekf::resetYawToEKFGSF()
 {
-	if (!isYawEmergencyEstimateAvailable()) {
+	// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
+	// to improve its estimate if the previous reset was not successful.
+	if (!isYawEmergencyEstimateAvailable()
+	&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000)
+	&& _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) {
+
 		return false;
 	}
 
 	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar(), true);
 
-	// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
-	_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 	_control_status.flags.yaw_align = true;
-
-	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-		// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
-		// and cause another navigation failure
-		_control_status.flags.mag_fault = true;
-		_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
-
-	} else if (_control_status.flags.gps_yaw) {
-		_control_status.flags.gps_yaw_fault = true;
-		_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
-	}
+	_information_events.flags.yaw_aligned_to_imu_gps = true;
 
 	_ekfgsf_yaw_reset_time = _time_last_imu;
 	_ekfgsf_yaw_reset_count++;
 
+	resetMagStates();
+
 	return true;
 }
 

src/modules/ekf2/EKF/gps_control.cpp

@@ -53,6 +53,15 @@ void Ekf::controlGpsFusion()
 
 		controlGpsYawFusion(gps_checks_passing, gps_checks_failing);
 
+		if (!_control_status.flags.yaw_align && _control_status.flags.tilt_align
+		    && gps_checks_passing && !gps_checks_failing
+		    && isYawEmergencyEstimateAvailable()) {
+
+			if (resetYawToEKFGSF()) {
+				ECL_INFO("Yaw aligned using IMU and GPS");
+			}
+		}
+
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
 		const bool mandatory_conditions_passing = _control_status.flags.tilt_align
@@ -79,23 +88,33 @@ void Ekf::controlGpsFusion()
 						if (isYawFailure()
 						    && _control_status.flags.in_air
 						    && !was_gps_signal_lost
-						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit
-						    && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) {
-							// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
-							// to improve its estimate if the previous reset was not successful.
-							if (resetYawToEKFGSF()) {
-								ECL_WARN("GPS emergency yaw reset");
+						    && resetYawToEKFGSF()) {
+
+							ECL_WARN("GPS emergency yaw reset");
+
+							if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
+								// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
+								// and cause another navigation failure
+								_control_status.flags.mag_fault = true;
+								_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
+
+							} else if (_control_status.flags.gps_yaw) {
+								_control_status.flags.gps_yaw_fault = true;
+								_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
+
+							} else if (_control_status.flags.ev_yaw) {
+								stopEvYawFusion();
 							}
 
 						} else {
+							_warning_events.flags.gps_fusion_timout = true;
+							ECL_WARN("GPS fusion timeout - resetting");
+
 							// use GPS velocity data to check and correct yaw angle if a FW vehicle
 							if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
 								// if flying a fixed wing aircraft, do a complete reset that includes yaw
 								realignYawGPS();
 							}
-
-							_warning_events.flags.gps_fusion_timout = true;
-							ECL_WARN("GPS fusion timeout - resetting");
 						}
 
 						resetVelocityToGps(_gps_sample_delayed);
@@ -120,29 +139,7 @@ void Ekf::controlGpsFusion()
 
 		} else {
 			if (starting_conditions_passing) {
-				// Do not use external vision for yaw if using GPS because yaw needs to be
-				// defined relative to an NED reference frame
-				if (_control_status.flags.ev_yaw
-				    || _mag_inhibit_yaw_reset_req
-				    || _mag_yaw_reset_req) {
-
-					_mag_yaw_reset_req = true;
-
-					// Stop the vision for yaw fusion and do not allow it to start again
-					stopEvYawFusion();
-
-				} else {
-					startGpsFusion();
-				}
-
-			} else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) {
-				// If no mag is used, align using the yaw estimator (if available)
-				if (resetYawToEKFGSF()) {
-					_information_events.flags.yaw_aligned_to_imu_gps = true;
-					ECL_INFO("Yaw aligned using IMU and GPS");
-					resetVelocityToGps(_gps_sample_delayed);
-					resetHorizontalPositionToGps(_gps_sample_delayed);
-				}
+				startGpsFusion();
 			}
 		}
 
@@ -192,7 +189,7 @@ bool Ekf::shouldResetGpsFusion() const
 	return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
 }
 
-bool Ekf::isYawFailure() const
+bool Ekf::isYawError(float error_threshold) const
 {
 	if (!isYawEmergencyEstimateAvailable()) {
 		return false;
@@ -201,5 +198,5 @@ bool Ekf::isYawFailure() const
 	const float euler_yaw = getEulerYaw(_R_to_earth);
 	const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
 
-	return fabsf(yaw_error) > math::radians(25.f);
+	return fabsf(yaw_error) > error_threshold;
 }

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -206,11 +206,17 @@ bool Ekf::resetYawToGps()
 	// GPS yaw measurement is alreday compensated for antenna offset in the driver
 	const float measured_yaw = _gps_sample_delayed.yaw;
 
-	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
-	resetQuatStateYaw(measured_yaw, yaw_variance, true);
+	if (PX4_ISFINITE(measured_yaw) && !_control_status.flags.gps_yaw_fault) {
+		const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
+		resetQuatStateYaw(measured_yaw, yaw_variance, true);
 
-	_time_last_gps_yaw_fuse = _time_last_imu;
-	_yaw_signed_test_ratio_lpf.reset(0.f);
+		_time_last_gps_yaw_fuse = _time_last_imu;
+		_yaw_signed_test_ratio_lpf.reset(0.f);
 
-	return true;
+		resetMagStates();
+
+		return true;
+	}
+
+	return false;
 }

src/modules/ekf2/EKF/mag_control.cpp

@@ -362,3 +362,63 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		}
 	}
 }
+
+bool Ekf::resetMagStates()
+{
+	bool reset = false;
+
+	// reinit mag states
+	const bool mag_available = (_mag_counter != 0) && isRecent(_time_last_mag, 500000);
+
+	// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
+	if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
+		// use predicted earth field to reset states
+		const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
+		_state.mag_I = mag_earth_pred;
+
+		ECL_DEBUG("resetting mag I to [%.3f, %.3f, %.3f]", (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2));
+
+		if (mag_available) {
+			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
+			_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
+
+			ECL_DEBUG("resetting mag B to [%.3f, %.3f, %.3f]", (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2));
+
+		} else {
+			_state.mag_B.zero();
+		}
+
+		reset = true;
+
+	} else if (mag_available && !magFieldStrengthDisturbed(_mag_lpf.getState())) {
+		// Use the last magnetometer measurements to reset the field states
+
+		// calculate initial earth magnetic field states
+		_state.mag_I = _R_to_earth * _mag_lpf.getState();
+		_state.mag_B.zero();
+
+		ECL_DEBUG("resetting mag I to [%.3f, %.3f, %.3f]", (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2));
+
+		reset = true;
+	}
+
+	if (reset) {
+		resetMagCov();
+
+		// clear any pending resets
+		_mag_yaw_reset_req = false;
+		_mag_inhibit_yaw_reset_req = false;
+
+		if (mag_available) {
+			// record the start time for the magnetic field alignment
+			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
+			_control_status.flags.mag_aligned_in_flight = true;
+			_time_last_mag_heading_fuse = _time_last_imu;
+			_time_last_mag_3d_fuse = _time_last_imu;
+		}
+
+		return true;
+	}
+
+	return false;
+}

src/modules/ekf2/test/test_EKF_yaw_estimator.cpp

@@ -106,10 +106,10 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment)
 	EXPECT_NEAR(yaw_est, yaw, tolerance_rad);
 	EXPECT_LT(yaw_est_var, tolerance_rad);
 
-	// 2 resets: 1 after IMU+GNSS yaw alignment and 1 when starting GNSS aiding
+	// 1 reset after IMU+GNSS yaw alignment
 	reset_logging_checker.capturePostResetState();
-	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(2));
-	EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2));
+	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
+	EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1));
 
 	EXPECT_TRUE(_ekf->local_position_is_valid());
 	EXPECT_TRUE(_ekf->global_position_is_valid());