Merge pull request #153 from PX4/pr-ekf2ExternalVision

EKF: external vision bug fixes

EKF/common.h

@@ -231,9 +231,7 @@ struct parameters {
 	float rng_gnd_clearance;	// minimum valid value for range when on ground (m)
 
 	// vision position fusion
-	float ev_noise;		// observation noise for vision sensor estimates (m)
 	float ev_innov_gate;		// vision estimator fusion innovation consistency gate size (STD)
-	float ev_gnd_clearance;	// minimum valid value for vision based height estimate when on ground (m)
 
 	// optical flow fusion
 	float flow_noise;		// observation noise for optical flow LOS rate measurements (rad/sec)
@@ -424,6 +422,7 @@ union filter_control_status_u {
 		uint16_t gps_hgt     : 1; // 11 - true when range finder height is being fused as a primary height reference
 		uint16_t ev_pos      : 1; // 12 - true when local position data from external vision is being fused
 		uint16_t ev_yaw      : 1; // 13 - true when yaw data from external vision measurements is being fused
+		uint16_t ev_hgt      : 1; // 14 - true when height data from external vision measurements is being fused
 	} flags;
 	uint16_t value;
 };

EKF/control.cpp

@@ -323,6 +323,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = true;
 				_control_status.flags.rng_hgt = false;
+				_control_status.flags.ev_hgt = false;
 				// request a reset
 				reset_height = true;
 				printf("EKF baro hgt timeout - reset to GPS\n");
@@ -333,6 +334,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_control_status.flags.baro_hgt = true;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
+				_control_status.flags.ev_hgt = false;
 				// request a reset
 				reset_height = true;
 				printf("EKF baro hgt timeout - reset to baro\n");
@@ -372,6 +374,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_control_status.flags.baro_hgt = true;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
+				_control_status.flags.ev_hgt = false;
 				// request a reset
 				reset_height = true;
 				printf("EKF gps hgt timeout - reset to baro\n");
@@ -382,6 +385,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = true;
 				_control_status.flags.rng_hgt = false;
+				_control_status.flags.ev_hgt = false;
 				// request a reset
 				reset_height = true;
 				printf("EKF gps hgt timeout - reset to GPS\n");
@@ -414,6 +418,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_control_status.flags.baro_hgt = true;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = false;
+				_control_status.flags.ev_hgt = false;
 				// request a reset
 				reset_height = true;
 				printf("EKF rng hgt timeout - reset to baro\n");
@@ -424,6 +429,7 @@ void Ekf::controlHeightSensorTimeouts()
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = true;
+				_control_status.flags.ev_hgt = false;
 				// request a reset
 				reset_height = true;
 				printf("EKF rng hgt timeout - reset to rng hgt\n");
@@ -433,6 +439,48 @@ void Ekf::controlHeightSensorTimeouts()
 			}
 		}
 
+		// handle the case where we are using external vision data for height
+		if (_control_status.flags.ev_hgt) {
+			// check if vision data is available
+			extVisionSample ev_init = _ext_vision_buffer.get_newest();
+			bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
+			// check if baro data is available
+			baroSample baro_init = _baro_buffer.get_newest();
+			bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
+
+			// reset to baro if we have no vision data and baro data is available
+			bool reset_to_baro = !ev_data_available && baro_data_available;
+
+			// reset to ev data if it is available
+			bool reset_to_ev = ev_data_available;
+
+			if (reset_to_baro) {
+				// set height sensor health
+				_rng_hgt_faulty = true;
+				_baro_hgt_faulty = false;
+				// reset the height mode
+				_control_status.flags.baro_hgt = true;
+				_control_status.flags.gps_hgt = false;
+				_control_status.flags.rng_hgt = false;
+				_control_status.flags.ev_hgt = false;
+				// request a reset
+				reset_height = true;
+				printf("EKF ev hgt timeout - reset to baro\n");
+			} else if (reset_to_ev) {
+				// reset the height mode
+				_control_status.flags.baro_hgt = false;
+				_control_status.flags.gps_hgt = false;
+				_control_status.flags.rng_hgt = false;
+				_control_status.flags.ev_hgt = true;
+				// request a reset
+				reset_height = true;
+				printf("EKF ev hgt timeout - reset to ev hgt\n");
+			} else {
+				// we have nothing to reset to
+				reset_height = false;
+			}
+		}
+
 		// Reset vertical position and velocity states to the last measurement
 		if (reset_height) {
 			resetHeight();
@@ -448,25 +496,43 @@ void Ekf::controlHeightAiding()
 	// check for height sensor timeouts and reset and change sensor if necessary
 	controlHeightSensorTimeouts();
 
-	// Control the soure of height measurements for the main filter
-	if (_control_status.flags.ev_pos) {
-	       _control_status.flags.baro_hgt = false;
-	       _control_status.flags.gps_hgt = false;
-	       _control_status.flags.rng_hgt = false;
-	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) {
+	// Control the source of height measurements for the main filter
+	// do not switch to a sensor if it is unhealthy or the data is stale
+	if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) &&
+			!_baro_hgt_faulty &&
+			(((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) {
+
 		_control_status.flags.baro_hgt = true;
 		_control_status.flags.gps_hgt = false;
 		_control_status.flags.rng_hgt = false;
+		_control_status.flags.ev_hgt = false;
+
+	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) &&
+		   !_gps_hgt_faulty &&
+		   (((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) {
 
-	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) {
 		_control_status.flags.baro_hgt = false;
 		_control_status.flags.gps_hgt = true;
 		_control_status.flags.rng_hgt = false;
+		_control_status.flags.ev_hgt = false;
+
+	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) &&
+		   !_rng_hgt_faulty &&
+		    (((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL)  || _control_status.flags.rng_hgt)) {
 
-	} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) {
 		_control_status.flags.baro_hgt = false;
 		_control_status.flags.gps_hgt = false;
 		_control_status.flags.rng_hgt = true;
+		_control_status.flags.ev_hgt = false;
+
+	} else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) &&
+		   (((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL)  || _control_status.flags.ev_hgt)) {
+
+		_control_status.flags.baro_hgt = false;
+		_control_status.flags.gps_hgt = false;
+		_control_status.flags.rng_hgt = false;
+		_control_status.flags.ev_hgt = true;
+
 	}
 }
 

EKF/ekf.cpp

@@ -415,6 +415,13 @@ bool Ekf::initialiseFilter(void)
 		if (_ev_counter == 0 && _ev_sample_delayed.time_us !=0) {
 			// initialise the counter
 			_ev_counter = 1;
+			// set the height fusion mode to use external vision data when we start getting valid data from the buffer
+			if (_primary_hgt_source == VDIST_SENSOR_EV) {
+				_control_status.flags.baro_hgt = false;
+				_control_status.flags.gps_hgt = false;
+				_control_status.flags.rng_hgt = false;
+				_control_status.flags.ev_hgt = true;
+			}
 		} else if (_ev_counter != 0) {
 			// increment the sample count
 			_ev_counter ++;
@@ -423,13 +430,10 @@ bool Ekf::initialiseFilter(void)
 
 	// set the default height source from the adjustable parameter
 	if (_hgt_counter == 0) {
-		if (_params.fusion_mode & MASK_USE_EVPOS) {
-			_primary_hgt_source = VDIST_SENSOR_EV;
-		} else {
-			_primary_hgt_source = _params.vdist_sensor_type;
-		}
+		_primary_hgt_source = _params.vdist_sensor_type;
 	}
 
+	// accumulate enough height measurements to be confident in the qulaity of the data
 	if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
 		if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
 			if (_hgt_counter == 0 && _range_sample_delayed.time_us != 0) {
@@ -437,6 +441,7 @@ bool Ekf::initialiseFilter(void)
 				_control_status.flags.baro_hgt = false;
 				_control_status.flags.gps_hgt = false;
 				_control_status.flags.rng_hgt = true;
+				_control_status.flags.ev_hgt = false;
 				_rng_filt_state = _range_sample_delayed.rng;
 				_hgt_counter = 1;
 			} else if (_hgt_counter != 0) {
@@ -446,7 +451,7 @@ bool Ekf::initialiseFilter(void)
 			}
 		}
 
-	} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) {
+	} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
 		// if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS
 		// later when it passes checks.
 		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
@@ -464,14 +469,16 @@ bool Ekf::initialiseFilter(void)
 			}
 		}
 
+	} else if (_primary_hgt_source == VDIST_SENSOR_EV) {
+		// do nothing becasue vision data is checked elsewhere
 	} else {
 		return false;
 	}
 
 	// check to see if we have enough measurements and return false if not
-	bool hgt_count_fail = _hgt_counter <= 10;
-	bool mag_count_fail = _mag_counter <= 10;
-	bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 10);
+	bool hgt_count_fail = _hgt_counter <= OBS_BUFFER_LENGTH;
+	bool mag_count_fail = _mag_counter <= OBS_BUFFER_LENGTH;
+	bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= OBS_BUFFER_LENGTH);
 	if (hgt_count_fail || mag_count_fail || ev_count_fail) {
 		return false;
 
@@ -516,12 +523,18 @@ bool Ekf::initialiseFilter(void)
 		// calculate the initial magnetic field and yaw alignment
 		_control_status.flags.yaw_align = resetMagHeading(mag_init);
 
-		// if we are using the range finder as the primary source, then calculate the baro height at origin so  we can use baro as a backup
-		// so it can be used as a backup ad set the initial height using the range finder
 		if (_control_status.flags.rng_hgt) {
+			// if we are using the range finder as the primary source, then calculate the baro height at origin so  we can use baro as a backup
+			// so it can be used as a backup ad set the initial height using the range finder
 			baroSample baro_newest = _baro_buffer.get_newest();
 			_baro_hgt_offset = baro_newest.hgt;
 			_state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance);
+
+		} else if (_control_status.flags.ev_hgt) {
+			// if we are using external vision data for height, then the vertical position state needs to be reset
+			// because the initialisation position is not the zero datum
+			resetHeight();
+
 		}
 
 		// initialise the state covariance matrix

EKF/ekf_helper.cpp

@@ -204,16 +204,19 @@ void Ekf::resetHeight()
 			// TODO: reset to last known gps based estimate
 		}
 
-	} else if (_control_status.flags.ev_pos) {
+	} else if (_control_status.flags.ev_hgt) {
 		// initialize vertical position with newest measurement
 		extVisionSample ev_newest = _ext_vision_buffer.get_newest();
 
-		if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) {
+		// use the most recent data if it's time offset from the fusion time horizon is smaller
+		int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
+		int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
+		if (abs(dt_newest) < abs(dt_delayed)) {
 			_state.pos(2) = ev_newest.posNED(2);
-
 		} else {
-			// TODO: reset to last known baro based estimate
+			_state.pos(2) = _ev_sample_delayed.posNED(2);
 		}
+
 	}
 
 	// reset the vertical velocity covariance values

EKF/vel_pos_fusion.cpp

@@ -86,35 +86,49 @@ void Ekf::fuseVelPosHeight()
 
 		// Calculate innovations and observation variance depending on type of observations
 		// being used
-		if (!_control_status.flags.gps && !_control_status.flags.ev_pos) {
-			// No observations - use a static position to constrain drift
-			if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-				R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
-			} else {
-				R[3] = 0.5f;
-			}
-			_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
-			_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
-
-		} else if (_control_status.flags.gps) {
+		if (_control_status.flags.gps) {
+			// we are using GPS measurements
 			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
 			R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
 			_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
 			_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
 
+			// innovation gate size
+			gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
+
+
 		} else if (_control_status.flags.ev_pos) {
+			// we are using external vision measurements
 			R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
 			_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
 			_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
 
+			// innovation gate size
+			gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f);
+
+		} else {
+			// No observations - use a static position to constrain drift
+			if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
+				R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
+			} else {
+				R[3] = 0.5f;
+			}
+			_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
+			_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
+
+			// glitch protection is not required so set gate to a large value
+			gate_size[3] = 100.0f;
+
 		}
 
+		// convert North position noise to variance
 		R[3] = R[3] * R[3];
+
+		// copy North axis values to East axis
 		R[4] = R[3];
-		// innovation gate sizes
-		gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
 		gate_size[4] = gate_size[3];
+
 	}
 
 	if (_fuse_height) {
@@ -151,7 +165,7 @@ void Ekf::fuseVelPosHeight()
 			R[5] = R[5] * R[5];
 			// innovation gate size
 			gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
-		} else if (_control_status.flags.ev_pos) {
+		} else if (_control_status.flags.ev_hgt) {
 			fuse_map[5] = true;
 			// calculate the innovation assuming the external vision observaton is in local NED frame
 			_vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
@@ -159,7 +173,7 @@ void Ekf::fuseVelPosHeight()
 			R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
 			R[5] = R[5] * R[5];
 			// innovation gate size
-			gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
+			gate_size[5] = fmaxf(_params.ev_innov_gate, 1.0f);
 		}
 
 	}
@@ -183,9 +197,7 @@ void Ekf::fuseVelPosHeight()
 	bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f)
 			      && (_vel_pos_test_ratio[2] <= 1.0f);
 	innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
-	bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
-	bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f))
-			      || using_synthetic_measurements || !_control_status.flags.tilt_align;
+	bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || !_control_status.flags.tilt_align;
 	innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
 	innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;