ekf2: refactor vertical acceleration check for multiple height sources

2026-10-21 19:13:33 +08:00 · 2022-08-24 09:16:11 -04:00
parent 8fd79688c0
commit f9188b2a14
7 changed files with 230 additions and 95 deletions
@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file height_control.cpp
+ */
+
+#include "ekf.h"
+
+void Ekf::checkVerticalAccelerationHealth()
+{
+	// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
+	// innovations being positive and not stale.
+	// Clipping usually causes the average accel reading to move towards zero which makes the INS
+	// think it is falling and produces positive vertical innovations.
+
+	Likelihood inertial_nav_falling_likelihood = estimateInertialNavFallingLikelihood();
+
+	// Check for more than 50% clipping affected IMU samples within the past 1 second
+	const uint16_t clip_count_limit = 1.f / _dt_ekf_avg;
+	const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] ||
+				 _imu_sample_delayed.delta_vel_clipping[1] ||
+				 _imu_sample_delayed.delta_vel_clipping[2];
+
+	if (is_clipping && _clip_counter < clip_count_limit) {
+		_clip_counter++;
+
+	} else if (_clip_counter > 0) {
+		_clip_counter--;
+	}
+
+	_fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2;
+
+	const bool is_clipping_frequently = _clip_counter > 0;
+
+	// Do not require evidence of clipping if the likelihood of having the INS falling is high
+	const bool bad_vert_accel = (is_clipping_frequently && (inertial_nav_falling_likelihood == Likelihood::MEDIUM))
+				    || (inertial_nav_falling_likelihood == Likelihood::HIGH);
+
+	if (bad_vert_accel) {
+		_time_bad_vert_accel = _time_last_imu;
+
+	} else {
+		_time_good_vert_accel = _time_last_imu;
+	}
+
+	// declare a bad vertical acceleration measurement and make the declaration persist
+	// for a minimum of BADACC_PROBATION seconds
+	if (_fault_status.flags.bad_acc_vertical) {
+		_fault_status.flags.bad_acc_vertical = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
+
+	} else {
+		_fault_status.flags.bad_acc_vertical = bad_vert_accel;
+	}
+}
+
+Likelihood Ekf::estimateInertialNavFallingLikelihood() const
+{
+	bool likelihood_high = false;
+	bool likelihood_medium = false;
+
+	bool failed_min[4] = {false, false, false, false};
+	bool failed_lim[4] = {false, false, false, false};
+
+	if (isVerticalPositionAidingActive()) {
+		if (_control_status.flags.baro_hgt) {
+			const float innov_ratio = _aid_src_baro_hgt.innovation / sqrtf(_aid_src_baro_hgt.innovation_variance);
+			failed_min[HeightSensorRef::BARO] = innov_ratio > _params.vert_innov_test_min;
+			failed_lim[HeightSensorRef::BARO] = innov_ratio > _params.vert_innov_test_lim;
+		}
+
+		if (_control_status.flags.gps_hgt) {
+			const float innov_ratio = _aid_src_gnss_pos.innovation[2] / sqrtf(_aid_src_gnss_pos.innovation_variance[2]);
+			failed_min[HeightSensorRef::GPS] = innov_ratio > _params.vert_innov_test_min;
+			failed_lim[HeightSensorRef::GPS] = innov_ratio > _params.vert_innov_test_lim;
+		}
+
+		if (_control_status.flags.rng_hgt) {
+			const float innov_ratio = _aid_src_rng_hgt.innovation / sqrtf(_aid_src_rng_hgt.innovation_variance);
+			failed_min[HeightSensorRef::RANGE] = innov_ratio > _params.vert_innov_test_min;
+			failed_lim[HeightSensorRef::RANGE] = innov_ratio > _params.vert_innov_test_lim;
+		}
+
+		if (_control_status.flags.ev_hgt) {
+			const float innov_ratio = _ev_pos_innov(2) / sqrtf(_ev_pos_innov_var(2));
+			failed_min[HeightSensorRef::EV] = innov_ratio > _params.vert_innov_test_min;
+			failed_lim[HeightSensorRef::EV] = innov_ratio > _params.vert_innov_test_lim;
+		}
+
+		for (unsigned i = 0; i < 4; i++) {
+
+			if (failed_lim[i]) {
+				// There is a chance that the interial nav is falling if one height source is failing the test
+				likelihood_medium = true;
+			}
+
+			for (unsigned j = 0; j < 4; j++) {
+
+				if ((i != j) && failed_lim[i] && failed_min[j]) {
+					// There is a high chance that the interial nav is falling if two height sources are failing the test
+					likelihood_high = true;
+				}
+			}
+		}
+	}
+
+	if (isVerticalVelocityAidingActive()) {
+		bool gps_vel_failed_min = false;
+		bool gps_vel_failed_lim = false;
+
+		if (_control_status.flags.gps) {
+			const float innov_ratio = _aid_src_gnss_vel.innovation[2] / sqrtf(_aid_src_gnss_vel.innovation_variance[2]);
+			gps_vel_failed_min = innov_ratio > _params.vert_innov_test_min;
+			gps_vel_failed_lim = innov_ratio > _params.vert_innov_test_lim;
+		}
+
+		bool ev_vel_failed_min = false;
+		bool ev_vel_failed_lim = false;
+
+		if (_control_status.flags.ev_vel) {
+			const float innov_ratio = _ev_vel_innov(2) / sqrtf(_ev_vel_innov_var(2));
+			ev_vel_failed_min = innov_ratio > _params.vert_innov_test_min;
+			ev_vel_failed_lim = innov_ratio > _params.vert_innov_test_lim;
+		}
+
+		// If vertical position and velocity come from independent sensors then we can
+		// trust them more if they disagree with the IMU, but need to check that they agree
+		likelihood_high |= gps_vel_failed_lim && (failed_min[HeightSensorRef::BARO]
+							  || failed_min[HeightSensorRef::RANGE]
+							  || failed_min[HeightSensorRef::EV]
+							  || ev_vel_failed_min);
+		likelihood_high |= gps_vel_failed_min && (failed_lim[HeightSensorRef::BARO]
+							  || failed_lim[HeightSensorRef::RANGE]
+							  || failed_lim[HeightSensorRef::EV]
+							  || ev_vel_failed_lim);
+
+		likelihood_high |= ev_vel_failed_lim && (failed_min[HeightSensorRef::BARO]
+							 || failed_min[HeightSensorRef::RANGE]
+							 || failed_min[HeightSensorRef::GPS]
+							 || gps_vel_failed_min);
+		likelihood_high |= ev_vel_failed_min && (failed_lim[HeightSensorRef::BARO]
+							 || failed_lim[HeightSensorRef::RANGE]
+							 || failed_lim[HeightSensorRef::GPS]
+							 || gps_vel_failed_lim);
+
+		likelihood_medium |= gps_vel_failed_lim;
+		likelihood_medium |= ev_vel_failed_lim;
+	}
+
+	if (likelihood_high) {
+		return Likelihood::HIGH;
+
+	} else if (likelihood_medium) {
+		return Likelihood::MEDIUM;
+	}
+
+	return Likelihood::LOW;
+}