ekf2: Handle blending of dissimilar rate GPS data (#10570)

A filtered update update interval is calculated for each receiver.
If dissimilar interval data is detected, blending occurs when data is received from the slower of the receivers.
If similar interval data is detected, blending occurs when receiver data with similar time stamps is available.
If no data is received from either receiver for longer than 300msec, then no blending will be performed and the most recent data will be used instead.

src/modules/ekf2/ekf2_main.cpp

@@ -129,12 +129,12 @@ private:
 	const Vector3f get_vel_body_wind();
 
 	/*
-	 * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical receiver solutions
+	 * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
-	 * with weights are calculated in calc_gps_blend_weights(). This internal state cannot be used directly by estimators
+	 * receiver solutions. This internal state cannot be used directly by estimators because if physical receivers
-	 * because if physical receivers have significant position differences,  variation in receiver estimated accuracy will
+	 * have significant position differences, variation in receiver estimated accuracy will cause undesirable
-	 * cause undesirable variation in the position soution.
+	 * variation in the position soution.
 	*/
-	bool calc_gps_blend_weights();
+	bool blend_gps_data();
 
 	/*
 	 * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated
@@ -235,6 +235,13 @@ private:
 	uint64_t _time_prev_us[GPS_MAX_RECEIVERS] = {};	///< the previous value of time_us for that GPS instance - used to detect new data.
 	uint8_t _gps_best_index = 0;			///< index of the physical receiver with the lowest reported error
 	uint8_t _gps_select_index = 0;			///< 0 = GPS1, 1 = GPS2, 2 = blended
+	uint8_t _gps_time_ref_index =
+		0;		///< index of the receiver that is used as the timing reference for the blending update
+	uint8_t _gps_oldest_index = 0;			///< index of the physical receiver with the oldest data
+	uint8_t _gps_newest_index = 0;			///< index of the physical receiver with the newest data
+	uint8_t _gps_slowest_index = 0;			///< index of the physical receiver with the slowest update rate
+	float _gps_dt[GPS_MAX_RECEIVERS] = {};		///< average time step in seconds.
+	bool  _gps_new_output_data = false;		///< true if there is new output data for the EKF
 
 	int _airdata_sub{-1};
 	int _airspeed_sub{-1};
@@ -1006,14 +1013,7 @@ void Ekf2::run()
 			// blend dual receivers if available
 
 			// calculate blending weights
-			if (calc_gps_blend_weights()) {
+			if (!blend_gps_data()) {
-				// With updated weights we can calculate a blended GPS solution and
-				// offsets for each physical receiver
-				update_gps_blend_states();
-				update_gps_offsets();
-				_gps_select_index = 2;
-
-			} else {
 				// handle case where the blended states cannot be updated
 				if (_gps_state[0].fix_type > _gps_state[1].fix_type) {
 					// GPS 1 has the best fix status so use that
@@ -1032,9 +1032,18 @@ void Ekf2::run()
 						_gps_select_index = 1;
 					}
 				}
+
+				// Only use selected receiver data if it has been updated
+				if ((gps1_updated && _gps_select_index == 0) || (gps2_updated && _gps_select_index == 1)) {
+					_gps_new_output_data = true;
+
+				} else {
+					_gps_new_output_data = false;
+				}
 			}
 
-			// correct the physical receiver data for steady state offsets
+			if (_gps_new_output_data) {
+				// correct the _gps_state data for steady state offsets and write to _gps_output
 				apply_gps_offsets();
 
 				// calculate a blended output from the offset corrected receiver data
@@ -1065,6 +1074,10 @@ void Ekf2::run()
 
 				// Publish to the EKF blended GPS topic
 				orb_publish_auto(ORB_ID(ekf_gps_position), &_blended_gps_pub, &gps, &_gps_orb_instance, ORB_PRIO_LOW);
+
+				// clear flag to avoid re-use of the same data
+				_gps_new_output_data = false;
+			}
 		}
 
 		bool airspeed_updated = false;
@@ -1778,15 +1791,41 @@ const Vector3f Ekf2::get_vel_body_wind()
 	return R_to_body * v_wind_comp;
 }
 
-/*
+bool Ekf2::blend_gps_data()
- calculate the weightings used to blend GPS location and velocity data
-*/
-bool Ekf2::calc_gps_blend_weights()
 {
 	// zero the blend weights
 	memset(&_blend_weights, 0, sizeof(_blend_weights));
 
-	// Use the oldest non-zero time, but if time difference is excessive, use newest to prevent a disconnected receiver from blocking updates
+	/*
+	 * If both receivers have the same update rate, use the oldest non-zero time.
+	 * If two receivers with different update rates are used, use the slowest.
+	 * If time difference is excessive, use newest to prevent a disconnected receiver
+	 * from blocking updates.
+	 */
+
+	// Calculate the time step for each receiver with some filtering to reduce the effects of jitter
+	// Find the largest and smallest time step.
+	float dt_max = 0.0f;
+	float dt_min = 0.3f;
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		float raw_dt = 0.001f * (float)(_gps_state[i].time_usec - _time_prev_us[i]);
+
+		if (raw_dt > 0.0f && raw_dt < 0.3f) {
+			_gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];
+		}
+
+		if (_gps_dt[i] > dt_max) {
+			dt_max = _gps_dt[i];
+			_gps_slowest_index = i;
+		}
+
+		if (_gps_dt[i] < dt_min) {
+			dt_min = _gps_dt[i];
+		}
+	}
+
+	// Find the receiver that is last be updated
 	uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message
 	uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message
 
@@ -1794,22 +1833,50 @@ bool Ekf2::calc_gps_blend_weights()
 		// Find largest and smallest times
 		if (_gps_state[i].time_usec > max_us) {
 			max_us = _gps_state[i].time_usec;
+			_gps_newest_index = i;
 		}
 
 		if ((_gps_state[i].time_usec < min_us) && (_gps_state[i].time_usec > 0)) {
 			min_us = _gps_state[i].time_usec;
+			_gps_oldest_index = i;
 		}
 	}
 
-	if ((max_us - min_us) < 300000) {
+	if ((max_us - min_us) > 300000) {
-		// data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated
+		// A receiver has timed out so fall out of blending
-		_gps_blended_state.time_usec = min_us;
-
-	} else {
-		// receiver data has timed out so fail out of blending
 		return false;
 	}
 
+	/*
+	 * If the largest dt is less than 20% greater than the smallest, then we have  receivers
+	 * running at the same rate then we wait until we have two messages with an arrival time
+	 * difference that is less than 50% of the smallest time step and use the time stamp from
+	 * the newest data.
+	 * Else we have two receivers at different update rates and use the slowest receiver
+	 * as the timing reference.
+	 */
+
+	if ((dt_max - dt_min) < 0.2f * dt_min) {
+		// both receivers assumed to be running at the same rate
+		if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) {
+			// data arrival within a short time window enables the two measurements to be blended
+			_gps_time_ref_index = _gps_newest_index;
+			_gps_new_output_data = true;
+		}
+
+	} else {
+		// both receivers running at different rates
+		_gps_time_ref_index = _gps_slowest_index;
+
+		if (_gps_state[_gps_time_ref_index].time_usec > _time_prev_us[_gps_time_ref_index]) {
+			// blend data at the rate of the slower receiver
+			_gps_new_output_data = true;
+		}
+	}
+
+	if (_gps_new_output_data) {
+		_gps_blended_state.time_usec = _gps_state[_gps_time_ref_index].time_usec;
+
 		// calculate the sum squared speed accuracy across all GPS sensors
 		float speed_accuracy_sum_sq = 0.0f;
 
@@ -1946,6 +2013,14 @@ bool Ekf2::calc_gps_blend_weights()
 			_blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights;
 		}
 
+		// With updated weights we can calculate a blended GPS solution and
+		// offsets for each physical receiver
+		update_gps_blend_states();
+		update_gps_offsets();
+		_gps_select_index = 2;
+
+	}
+
 	return true;
 }