Distance Sensor faulty readings affecting local position estimate significantly (PX4 v1.11.3)

junwoo0914 · December 18, 2023, 6:08am

Crash Analysis

A crash occurred with a PX4 v1.11.3 custom version (with no changes to core flight control) multirotor was landing in a mission & had a crash due to the faulty distance sensor readings. Here’s the diagnosis:

Vehicle was descending at 0.5 m/s (MPC_LAND_SPEED), and was closing in on the ground
When the distance sensor data was coming in & got considered valid (vehicle_local_position/dist_bottom_valid) at around 436 seconds mark, EKF2 fused local position z value started to ‘decrease’ (indicating vertically upwards movement)
As the z position setpoint generated from the “FlightTaskAutoLineSmoothVel” with jerk limited trajectory continued increasing (as it integrated velocity setpoint), current fused z position & it’s setpoint started to diverge
Due to P-gain in Multicopter Position Controller, the “vz” velocity setpoint increases significantly and saturated at 1.5 m/s (MPC_Z_VEL_MAX_DN)
Multirotor descends rapidly, overshooting the vz setpoint, and crashes (438.5 seconds mark, based on gps0_alt_m curve showing ground truth terrain at 144.755 meters AMSL)

Cause

There are multiple things that went wrong, but I wanted to focus on the estimator’s behavior regarding the distance sensor’s faulty readings.

As far as I can tell, estimator did provide kinematically inconsistent estimation of it’s z coordinate value as the faulty distance sensor data came in.

Estimator fuse of Height Above Ground Level

When checking the HAGL test ratio & innovation, it is clear that it saturated at 0.15 & 0.1 respectively moments before the crash. Their values were:

HAGL innovation variance: 0.02 - 0.03
HAGL innovation test ratio: Saturated at 0.023
HAGL test ratio: Saturated at 0.15
HAGL innovation: Saturated at 0.10

Also, checking the estimator_status/control_mode_flags value of “8389783” which got set after “vehicle_local_position/dist_bottom_valid” got set to true, indicating that the range finder is being fused as primary height reference.

github.com

PX4/PX4-ECL/blob/395c8d3c3d74e0e01789f32383a6b5621fe3cca1/EKF/common.h#L460


      
          		uint32_t tilt_align  : 1; ///< 0 - true if the filter tilt alignment is complete
          		uint32_t yaw_align   : 1; ///< 1 - true if the filter yaw alignment is complete
          		uint32_t gps         : 1; ///< 2 - true if GPS measurement fusion is intended
          		uint32_t opt_flow    : 1; ///< 3 - true if optical flow measurements fusion is intended
          		uint32_t mag_hdg     : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended
          		uint32_t mag_3D      : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is inteded
          		uint32_t mag_dec     : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended
          		uint32_t in_air      : 1; ///< 7 - true when the vehicle is airborne
          		uint32_t wind        : 1; ///< 8 - true when wind velocity is being estimated
          		uint32_t baro_hgt    : 1; ///< 9 - true when baro height is being fused as a primary height reference
          		uint32_t rng_hgt     : 1; ///< 10 - true when range finder height is being fused as a primary height reference
          		uint32_t gps_hgt     : 1; ///< 11 - true when GPS height is being fused as a primary height reference
          		uint32_t ev_pos      : 1; ///< 12 - true when local position data fusion from external vision is intended
          		uint32_t ev_yaw      : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended
          		uint32_t ev_hgt      : 1; ///< 14 - true when height data from external vision measurements is being fused
          		uint32_t fuse_beta   : 1; ///< 15 - true when synthetic sideslip measurements are being fused
          		uint32_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength
          		uint32_t fixed_wing  : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
          		uint32_t mag_fault   : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
          		uint32_t fuse_aspd   : 1; ///< 19 - true when airspeed measurements are being fused
          		uint32_t gnd_effect  : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active

Questions & Answers

I effectively answered my own questions, but may be incorrect in some places, open for feedback!

First, the “EKF2_HGT_MODE” was set to 1 (Primary source: GPS) (Link to v1.11 Parameters doc). Why did the distance sensor reading cause so much disturbance although it was not primary source?

To answer my own question, I think it’s this logic:

github.com

PX4/PX4-ECL/blob/b3fed06fe822d08d19ab1d2c2f8daf7b7d21951c/EKF/control.cpp#L1078-L1088


      
          		// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
          		// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
          		// Do switching between GPS and rangefinder if using range finder as a height source when close
          		// to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
          		if (!_control_status_prev.flags.rng_hgt && do_range_aid && _range_sensor.isDataHealthy()) {
          			setControlRangeHeight();
          
          			// we have just switched to using range finder, calculate height sensor offset such that current
          			// measurement matches our current height estimate
          			_hgt_sensor_offset = _terrain_vpos;

Second, is it possible to disable the range finder disturbance to the height fusion?

To do that, we need to make this condition false:

github.com

PX4/PX4-ECL/blob/b3fed06fe822d08d19ab1d2c2f8daf7b7d21951c/EKF/control.cpp#L1011-L1012


      
          	checkRangeAidSuitability();
          	const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable();

We can either:

Set “EKF2_RNG_AID” to 0 (it was 1), to make sure range sensor never disturbs the estimate of height
Set lower “EKF2_RNG_GATE” (e.g. to 1.0, as it was 5.0 before), or “EKF2_RNG_A_IGATE” (e.g. to 0.5, as it was 1.0 before)

Extra link to logic where range finder aid suitability is checked:

github.com

PX4/PX4-ECL/blob/b3fed06fe822d08d19ab1d2c2f8daf7b7d21951c/EKF/control.cpp#L1224-L1246


      
          void Ekf::checkRangeAidSuitability()
          {
          	if (_control_status.flags.in_air
          	    && _range_sensor.isHealthy()
          	    && isTerrainEstimateValid()) {
          		// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
          		// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
          		const float range_hagl = _terrain_vpos - _state.pos(2);
          		const float range_hagl_max = _is_range_aid_suitable ? _params.max_hagl_for_range_aid : (_params.max_hagl_for_range_aid * 0.7f);
          		const bool is_in_range = range_hagl < range_hagl_max;
          
          		const float hagl_test_ratio = (_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var));
          		const bool is_hagl_stable = _is_range_aid_suitable ? (hagl_test_ratio < 1.f) : (hagl_test_ratio < 0.01f);
          
          		if (isHorizontalAidingActive()) {
          			const float max_vel = _is_range_aid_suitable ? _params.max_vel_for_range_aid : (_params.max_vel_for_range_aid * 0.7f);
          			const bool is_below_max_speed = !_state.vel.xy().longerThan(max_vel);
          
          			_is_range_aid_suitable = is_in_range && is_hagl_stable && is_below_max_speed;

This file has been truncated. show original

Third, given the obviously inconsistent distance sensor data (for sure kinematically, as distance was increasing when velocity was expected to be down, closing in on the ground), the kinematic gate added in the following PR (for v1.13 and onwards) seems to likely solve this issue (and will reject range finder appropriately). Is this true?

github.com/PX4/PX4-Autopilot

Range finder kinematic consistency check

PX4:master ← PX4:pr-vel-bottom-consistency

opened 03:29PM - 16 Mar 22 UTC

bresch

+217 -5

**Describe problem solved by this pull request** It is quite common that range …finders are reporting incorrect data due to clouds, water, LOS blockage (e.g.: when attaching a payload) or other malfunction of the sensor itself. In most of those issues, the sensor quality metric is not a reliable source of truth. **Describe your solution** A kinematic consistency check is done by comparing the time derivative of the vertical distance measurement ("vel_bottom") and the vertical velocity estimate of EKF2. An inconsistency is detected when the normalized innovation squared (NIS) is on average greater than a defined threshold. When the data is said "inconsistent" it requires several conditions to be valid again: 1. low averaged NIS for more than 1s 2. low NIS 3. significant vertical velocity **Test data / coverage** SITL testing for now TODO: - [x] logging

@bresch Do these diagnosis & solution make sense? I’m also curious if there’s any sane value that RNG_GATE and RNG_A_IGATE params should be set to, instead of lowering them arbitrarily.

junwoo0914 · December 20, 2023, 3:46am

Feedbacks from @bresch for visibility:

it’s unfortunate but normal that the height estimate is affected by the range finder when range aid is enabled. Range aid is something we didn’t like and this is why I removed it in 1.14

Some reference documents:

v1.14 Range finder documentation
PR that introduced new height sensor configuration architecture: EKF2 multiple height sources fusion by bresch · Pull Request #19944 · PX4/PX4-Autopilot · GitHub
Another PR that emphasized importance of not using Range Aid completely?!: Use range finder for height control instead of height aiding by bresch · Pull Request #18864 · PX4/PX4-Autopilot · GitHub

My further questions

After some investigation, it seems that range aiding isn’t really removed (as it’s still ‘conditional’, and those conditions are the same, except for the influence from terrain estimator in regards to kinematic consistency check).

Thus, wouldn’t the system from v1.11 with EKF2_RNG_AID = 1, act the same with v1.14 with EKF2_RNG_CTRL = 1 (Conditional), regarding the disruption of height estimation? (Except for the cases where kinematic consistency check is failing)

Answers

Hi, not exactly. You can have the same behavior if EKF2_HGT_REF is set to “range”. Otherwise, if set to baro or GNSS, the conditional range finder fusion shouldn’t affect too much the height estimate if bad. In 1.11, when range fusion was active (through range aid), it automatically became the reference. Now it isn’t the case anymore, the reference doesn’t change.

Follow up questions

Ahhh gotcha, there’s a difference between being the reference vs just aiding on the side (since v1.14)