EKF: Improve robustness of yaw reset to bad inertial data

EKF/EKFGSF_yaw.cpp

@@ -79,14 +79,23 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
 			if (!bad_update) {
 				float total_weight = 0.0f;
 				// calculate weighting for each model assuming a normal distribution
+				const float min_weight = 1e-5f;
+				uint8_t n_weight_clips = 0;
 				for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
-					_model_weights(model_index) = fmaxf(gaussianDensity(model_index) * _model_weights(model_index), 0.0f);
+					_model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index);
+					if (_model_weights(model_index) < min_weight) {
+						n_weight_clips++;
+						_model_weights(model_index) = min_weight;
+					}
 					total_weight += _model_weights(model_index);
 				}
 
 				// normalise the weighting function
-				if (total_weight > 1e-15f) {
+				if (n_weight_clips < N_MODELS_EKFGSF) {
 					_model_weights /= total_weight;
+				} else {
+					// all weights have collapsed due to excessive innovation variances so reset filters
+					initialiseEKFGSF();
 				}
 
 				// Enforce a minimum weighting value. This was added during initial development but has not been needed

EKF/control.cpp

@@ -304,7 +304,8 @@ void Ekf::controlExternalVisionFusion()
 			Vector3f ev_vel_obs_var;
 			Vector2f ev_vel_innov_gates;
 
-			_ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame();
+			_last_vel_obs = getVisionVelocityInEkfFrame();
+			_ev_vel_innov = _state.vel - _last_vel_obs;
 
 			// check if we have been deadreckoning too long
 			if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) {
@@ -590,9 +591,21 @@ void Ekf::controlGpsFusion()
 							  (_time_last_hor_vel_fuse > _time_last_on_ground_us) &&
 							  (_time_last_hor_pos_fuse > _time_last_on_ground_us);
 
+			bool is_yaw_failure = false;
+			if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) {
+				// Do sanity check to see if the innovation failures is likely caused by a yaw angle error
+				// A 180 deg yaw error would (in the absence of other errors) generate a velocity innovation
+				// magnitude of twice the measured speed so allow up to 3 times to allow for additional errors.
+				const float hvel_obs_sq = sq(_last_vel_obs(0)) + sq(_last_vel_obs(1));
+				const float hvel_innov_sq = sq(_last_fail_hvel_innov(0)) + sq(_last_fail_hvel_innov(1));
+				if (hvel_innov_sq > 9.0f * hvel_obs_sq) {
+					is_yaw_failure = true;
+				}
+			}
+
 			// Detect if coming back after significant time without GPS data
 			const bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
-			const bool do_yaw_vel_pos_reset = (_do_ekfgsf_yaw_reset || recent_takeoff_nav_failure || inflight_nav_failure) &&
+			const bool do_yaw_vel_pos_reset = (_do_ekfgsf_yaw_reset || is_yaw_failure) &&
 							  _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit &&
 							  isTimedOut(_ekfgsf_yaw_reset_time, 5000000) &&
 							  !gps_signal_was_lost;
@@ -665,7 +678,8 @@ void Ekf::controlGpsFusion()
 			gps_vel_obs_var(2) = sq(1.5f) * gps_vel_obs_var(2);
 
 			// calculate innovations
-			_gps_vel_innov = _state.vel - _gps_sample_delayed.vel;
+			_last_vel_obs = _gps_sample_delayed.vel;
+			_gps_vel_innov = _state.vel - _last_vel_obs;
 			_gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos;
 
 			// set innovation gate size
@@ -1291,7 +1305,8 @@ void Ekf::controlAuxVelFusion()
 
 		const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate);
 
-		_aux_vel_innov = _state.vel - _auxvel_sample_delayed.vel;
+		_last_vel_obs = _auxvel_sample_delayed.vel;
+		_aux_vel_innov = _state.vel - _last_vel_obs;
 
 		fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar,
 				_aux_vel_innov_var, _aux_vel_test_ratio);

EKF/covariance.cpp

@@ -890,12 +890,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 	for (int i = 4; i <= 6; i++) {
 		// NED velocity states
-		P(i,i) = math::constrain(P(i,i), 1E-6f, P_lim[1]);
+		P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[1]);
 	}
 
 	for (int i = 7; i <= 9; i++) {
 		// NED position states
-		P(i,i) = math::constrain(P(i,i), 1E-6f, P_lim[2]);
+		P(i,i) = math::constrain(P(i,i), 1e-6f, P_lim[2]);
 	}
 
 	for (int i = 10; i <= 12; i++) {

EKF/ekf.h

@@ -391,6 +391,8 @@ class Ekf : public EstimatorInterface
 	Vector3f _delta_vel_bias_var_accum;		///< kahan summation algorithm accumulator for delta velocity bias variance
 	Vector3f _delta_angle_bias_var_accum;	///< kahan summation algorithm accumulator for delta angle bias variance
 
+	Vector3f _last_vel_obs;			///< last velocity observation (m/s)
+	Vector2f _last_fail_hvel_innov;	///< last failed horizontal velocity innovation (m/s)**2
 
 	Vector3f _gps_vel_innov;	///< GPS velocity innovations (m/sec)
 	Vector3f _gps_vel_innov_var;	///< GPS velocity innovation variances ((m/sec)**2)

EKF/mag_fusion.cpp

@@ -436,10 +436,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 	const float SA2 = SA0*q0 + SA1*q1;
 	const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
 	float SA4, SA5_inv;
-	if (sq(SA3) > 1E-6f) {
+	if (sq(SA3) > 1e-6f) {
 		SA4 = 1.0F/sq(SA3);
 		SA5_inv = sq(SA2)*SA4 + 1;
-		canUseA = fabsf(SA5_inv) > 1E-6f;
+		canUseA = fabsf(SA5_inv) > 1e-6f;
 	}
 
 	bool canUseB = false;
@@ -448,10 +448,10 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
 	const float SB2 = SB0*q3 + SB1*q2;
 	const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
 	float SB3, SB5_inv;
-	if (sq(SB2) > 1E-6f) {
+	if (sq(SB2) > 1e-6f) {
 		SB3 = 1.0F/sq(SB2);
 		SB5_inv = SB3*sq(SB4) + 1;
-		canUseB = fabsf(SB5_inv) > 1E-6f;
+		canUseB = fabsf(SB5_inv) > 1e-6f;
 	}
 
 	Vector4f H_YAW;
@@ -516,10 +516,10 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
 	const float SA2 = SA0*q0 - SA1*q1;
 	const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
 	float SA4, SA5_inv;
-	if (sq(SA3) > 1E-6f) {
+	if (sq(SA3) > 1e-6f) {
 		SA4 = 1.0F/sq(SA3);
 		SA5_inv = sq(SA2)*SA4 + 1;
-		canUseA = fabsf(SA5_inv) > 1E-6f;
+		canUseA = fabsf(SA5_inv) > 1e-6f;
 	}
 
 	bool canUseB = false;
@@ -528,10 +528,10 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
 	const float SB2 = -SB0*q3 + SB1*q2;
 	const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
 	float SB3, SB5_inv;
-	if (sq(SB2) > 1E-6f) {
+	if (sq(SB2) > 1e-6f) {
 		SB3 = 1.0F/sq(SB2);
 		SB5_inv = SB3*sq(SB4) + 1;
-		canUseB = fabsf(SB5_inv) > 1E-6f;
+		canUseB = fabsf(SB5_inv) > 1e-6f;
 	}
 
 	Vector4f H_YAW;

EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp

@@ -210,7 +210,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Magnetomer X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Magnetomer X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -235,7 +235,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Magnetomer X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Magnetomer X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
@@ -387,7 +387,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Magnetomer Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Magnetomer Y axis Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -412,7 +412,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Magnetomer Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Magnetomer Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);
@@ -567,7 +567,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Magnetomer Z axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Magnetomer Z axis Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -592,7 +592,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Magnetomer Z axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Magnetomer Z axis Kfusion max diff fraction = %e\n",max_diff_fraction);

EKF/python/ekf_derivation/generated/acc_bf_fusion_generated_compare.cpp

@@ -237,7 +237,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Specific Force X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Specific Force X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -263,7 +263,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Specific Force X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Specific Force X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
@@ -439,7 +439,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Specific Force Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Specific Force Y axis Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -464,7 +464,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Specific Force Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Specific Force Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);

EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp

@@ -298,7 +298,7 @@ int main()
         }
     }
 
-    if (max_diff_fraction > 1E-5f) {
+    if (max_diff_fraction > 1e-5f) {
         printf("Fail: Sideslip Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
     } else {
         printf("Pass: Sideslip Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -323,7 +323,7 @@ int main()
         }
     }
 
-    if (max_diff_fraction > 1E-5f) {
+    if (max_diff_fraction > 1e-5f) {
         printf("Fail: Sideslip Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
     } else {
         printf("Pass: Sideslip Kfusion max diff fraction = %e\n",max_diff_fraction);

EKF/python/ekf_derivation/generated/flow_fusion_generated_compare.cpp

@@ -364,7 +364,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Optical Flow X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Optical Flow X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -389,7 +389,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Optical Flow X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Optical Flow X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
@@ -603,7 +603,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Optical Flow Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Optical Flow Y axis Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -628,7 +628,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: Optical Flow Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: Optical Flow Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);

EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp

@@ -231,7 +231,7 @@ int main()
 		}
 	}
 
-	if (max_diff_fraction > 1E-5f) {
+	if (max_diff_fraction > 1e-5f) {
 		printf("Fail: GPS yaw Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
 	} else {
 		printf("Pass: GPS yaw Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -256,7 +256,7 @@ int main()
 		}
 	}
 
-	if (max_diff_fraction > 1E-5f) {
+	if (max_diff_fraction > 1e-5f) {
 		printf("Fail: GPS yaw Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
 	} else {
 		printf("Pass: GPS yaw Kfusion max diff fraction = %e\n",max_diff_fraction);

EKF/python/ekf_derivation/generated/mag_decl_fusion_generated_compare.cpp

@@ -192,7 +192,7 @@ int main()
         }
     }
 
-	if (max_diff_fraction > 1E-5f) {
+	if (max_diff_fraction > 1e-5f) {
 		printf("Fail: Mag Declination Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
 	} else {
 		printf("Pass: Mag Declination Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -217,7 +217,7 @@ int main()
         }
     }
 
-	if (max_diff_fraction > 1E-5f) {
+	if (max_diff_fraction > 1e-5f) {
 		printf("Fail: Mag Declination Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
 	} else {
 		printf("Pass: Mag Declination Kfusion max diff fraction = %e\n",max_diff_fraction);

EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp

@@ -189,7 +189,7 @@ int main()
         }
     }
 
-	if (max_diff_fraction > 1E-5f) {
+	if (max_diff_fraction > 1e-5f) {
 		printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
 	} else {
 		printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -217,7 +217,7 @@ int main()
         }
     }
 
-	if (max_diff_fraction > 1E-5f) {
+	if (max_diff_fraction > 1e-5f) {
 		printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
 	} else {
 		printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction);

EKF/python/ekf_derivation/generated/yaw_fusion_generated_compare.cpp

@@ -110,7 +110,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: 321 yaw option A Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: 321 yaw option A Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -165,7 +165,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: 321 yaw option B Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: 321 yaw option B Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -250,7 +250,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: 312 yaw option A Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: 312 yaw option A Hfusion max diff fraction = %e\n",max_diff_fraction);
@@ -304,7 +304,7 @@ int main()
             }
         }
 
-        if (max_diff_fraction > 1E-5f) {
+        if (max_diff_fraction > 1e-5f) {
             printf("Fail: 312 yaw option B Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
         } else {
             printf("Pass: 312 yaw option B Hfusion max diff fraction = %e\n",max_diff_fraction);

EKF/vel_pos_fusion.cpp

@@ -66,6 +66,8 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga
 		return true;
 
 	} else {
+		_last_fail_hvel_innov(0) = innov(0);
+		_last_fail_hvel_innov(1) = innov(1);
 		_innov_check_fail_status.flags.reject_hor_vel = true;
 		return false;
 	}