PreFlightChecker: add spike limit argument for innovation check and

increase optical flow test limits.

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -55,7 +55,7 @@ bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s
 
 	const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim);
 
-	return checkInnovFailed(innov.heading, heading_innov_lpf, heading_test_limit);
+	return checkInnovFailed(heading_innov_lpf, innov.heading, heading_test_limit, heading_innov_spike_lim);
 }
 
 float PreFlightChecker::selectHeadingTestLimit()
@@ -79,7 +79,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
 		Vector2f vel_ne_innov_lpf;
 		vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim);
 		vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim);
-		has_failed |= checkInnov2DFailed(vel_ne_innov, vel_ne_innov_lpf, _vel_innov_test_lim);
+		has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
 	}
 
 	if (_is_using_flow_aiding) {
@@ -87,7 +87,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
 		Vector2f flow_innov_lpf;
 		flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
 		flow_innov_lpf(1) = _filter_flow_x_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);
-		has_failed |= checkInnov2DFailed(flow_innov, flow_innov_lpf, _flow_innov_test_lim);
+		has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim);
 	}
 
 	return has_failed;
@@ -97,7 +97,7 @@ bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s
 {
 	const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel));     // only temporary solution
 	const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
-	return checkInnovFailed(vel_d_innov, vel_d_innov_lpf, _vel_innov_test_lim);
+	return checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
 }
 
 bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
@@ -105,18 +105,20 @@ bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s
 	const float hgt_innov = fmaxf(fabsf(innov.gps_vpos), fmaxf(fabs(innov.ev_vpos),
 				      fabs(innov.rng_vpos)));    // only temporary solution
 	const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim);
-	return checkInnovFailed(hgt_innov, hgt_innov_lpf, _hgt_innov_test_lim);
+	return checkInnovFailed(hgt_innov_lpf, hgt_innov, _hgt_innov_test_lim, _hgt_innov_spike_lim);
 }
 
-bool PreFlightChecker::checkInnovFailed(const float innov, const float innov_lpf, const float test_limit)
+bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit,
+					const float spike_limit)
 {
-	return fabsf(innov_lpf) > test_limit || fabsf(innov) > 2.0f * test_limit;
+	return fabsf(innov_lpf) > test_limit || fabsf(innov) > spike_limit;
 }
 
-bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov, const Vector2f &innov_lpf, const float test_limit)
+bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, const float test_limit,
+		const float spike_limit)
 {
 	return innov_lpf.norm_squared() > sq(test_limit)
-	       || innov.norm_squared() > sq(2.0f * test_limit);
+	       || innov.norm_squared() > sq(spike_limit);
 }
 
 void PreFlightChecker::reset()

src/modules/ekf2/Utility/PreFlightChecker.hpp

@@ -104,26 +104,28 @@ public:
 	/*
 	 * Check if the innovation fails the test
 	 * To pass the test, the following conditions should be true:
-	 * innov <= test_limit
-	 * innov_lpf <= 2 * test_limit
-	 * @param innov the current unfiltered innovation
+	 * innov_lpf <= test_limit
+	 * innov <= spike_limit
 	 * @param innov_lpf the low-pass filtered innovation
-	 * @param test_limit the magnitude test limit
+	 * @param innov the current unfiltered innovation
+	 * @param test_limit the magnitude test limit for innov_lpf
+	 * @param spike_limit the magnitude test limit for innov
 	 * @return true if the check failed the test, false otherwise
 	 */
-	static bool checkInnovFailed(float innov, float innov_lpf, float test_limit);
+	static bool checkInnovFailed(float innov_lpf, float innov, float test_limit, float spike_limit);
 
 	/*
 	 * Check if the a innovation of a 2D vector fails the test
 	 * To pass the test, the following conditions should be true:
-	 * innov <= test_limit
-	 * innov_lpf <= 2 * test_limit
-	 * @param innov the current unfiltered innovation
+	 * innov_lpf <= test_limit
+	 * innov <= spike_limit
 	 * @param innov_lpf the low-pass filtered innovation
-	 * @param test_limit the magnitude test limit
+	 * @param innov the current unfiltered innovation
+	 * @param test_limit the magnitude test limit for innov_lpf
+	 * @param spike_limit the magnitude test limit for innov
 	 * @return true if the check failed the test, false otherwise
 	 */
-	static bool checkInnov2DFailed(const Vector2f &innov, const Vector2f &innov_lpf, float test_limit);
+	static bool checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, float test_limit, float spike_limit);
 
 	static constexpr float sq(float var) { return var * var; }
 
@@ -168,7 +170,7 @@ private:
 	// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
 	static constexpr float _heading_innov_test_lim = 0.52f;
 	// Maximum permissible flow innovation to pass pre-flight checks
-	static constexpr float _flow_innov_test_lim = 0.1f;
+	static constexpr float _flow_innov_test_lim = 0.25f;
 	// Preflight velocity innovation spike limit (m/sec)
 	static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;
 	// Preflight position innovation spike limit (m)