modifications for replays

use-gps-as-height-on-ekf-to-init (PX4#876)
use-gps-data-with-increased-precision
never-turn-off-yaw-fusion (PX4#905)
do-not-use-mag_heading_noise-as-gps-yaw-variance
EKF-allow-ekf-init-without-magnetometer
control-fix-to-startGpsHgtFusion-again-after-timeout (PX4#951)
allow-EKFGSF_yaw-to-work (PX4#902)
remove-default-15-m-s-airspeed

EKF/EKFGSF_yaw.cpp

@@ -319,7 +319,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
 {
 	// set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum
-	const float velObsVar = sq(fmaxf(_vel_accuracy, 0.5f));
+	const float velObsVar = sq(fmaxf(_vel_accuracy, 0.1f));
 
 	// calculate velocity observation innovations
 	_ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0);

EKF/common.h

@@ -62,9 +62,9 @@ enum class velocity_frame_t : uint8_t {
 
 struct gps_message {
 	uint64_t time_usec{0};
-	int32_t lat;		///< Latitude in 1E-7 degrees
-	int32_t lon;		///< Longitude in 1E-7 degrees
-	int32_t alt;		///< Altitude in 1E-3 meters (millimeters) above MSL
+	double lat;		///< Latitude in degrees
+	double lon;		///< Longitude in degrees
+	float alt;		///< Altitude in meters above MSL
 	float yaw;		///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
 	float yaw_offset;	///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
 	uint8_t fix_type;	///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
@@ -369,7 +369,7 @@ struct parameters {
 	int32_t check_mag_strength{0};
 
 	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
-	float EKFGSF_tas_default{15.0f};	///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
+	float EKFGSF_tas_default{0.0f};	///< default airspeed value assumed if no airspeed measurement available (m/s)
 	unsigned EKFGSF_reset_delay{1000000};	///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
 	float EKFGSF_yaw_err_max{0.262f}; 	///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
 	unsigned EKFGSF_reset_count_limit{3};	///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value

EKF/control.cpp

@@ -597,6 +597,7 @@ void Ekf::controlGpsFusion()
 			   could have been caused by bad GPS.
 			*/
 
+			/*
 			const bool recent_takeoff_nav_failure = _control_status.flags.in_air &&
 								!isTimedOut(_time_last_on_ground_us, 30000000) &&
 								isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) &&
@@ -649,17 +650,14 @@ void Ekf::controlGpsFusion()
 					_time_last_of_fuse = _time_last_imu;
 				}
 
-			} else if (do_vel_pos_reset) {
-				// use GPS velocity data to check and correct yaw angle if a FW vehicle
-				if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
-					// if flying a fixed wing aircraft, do a complete reset that includes yaw
-					_control_status.flags.mag_aligned_in_flight = realignYawGPS();
-				}
+			} else
+			*/
 
+			if (do_vel_pos_reset) {
 				resetVelocity();
 				resetHorizontalPosition();
 				_velpos_reset_request = false;
-				ECL_WARN_TIMESTAMPED("GPS fusion timeout - reset to GPS");
+				ECL_WARN_TIMESTAMPED("GPS fusion reset");
 
 				// Reset the timeout counters
 				_time_last_hor_pos_fuse = _time_last_imu;
@@ -755,6 +753,8 @@ void Ekf::controlGpsYawFusion()
 		}
 	}
 
+	/* Temporary do not turn off yaw fusion. Issue https://github.com/PX4/ecl/issues/905
+	 *
 	// Check if the data is constantly fused by the estimator,
 	// if not, declare the sensor faulty and stop the fusion
 	// By doing this, another source of yaw aiding is allowed to start
@@ -763,6 +763,7 @@ void Ekf::controlGpsYawFusion()
 		_is_gps_yaw_faulty = true;
 		stopGpsYawFusion();
 	}
+	*/
 }
 
 void Ekf::controlHeightSensorTimeouts()
@@ -876,6 +877,34 @@ void Ekf::controlHeightSensorTimeouts()
 				failing_height_source = "ev";
 				new_height_source = "baro";
 			}
+		} else {
+			if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
+				// check if GPS height is available
+				const gpsSample &gps_init = _gps_buffer.get_newest();
+				const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
+
+				// check for inertial sensing errors in the last BADACC_PROBATION seconds
+				const bool prev_bad_vert_accel = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
+
+				// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
+				const bool reset_to_gps = !_gps_hgt_intermittent &&
+						    ((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty);
+
+				if (reset_to_gps) {
+					// set height sensor health
+					_baro_hgt_faulty = true;
+
+					startGpsHgtFusion();
+
+					//reset sensor offset because height can drift quite far away
+					_hgt_sensor_offset = 0.0f;
+
+					request_height_reset = true;
+					failing_height_source = "";
+					new_height_source = "gps";
+
+				}
+			}
 		}
 
 		if (failing_height_source && new_height_source) {

EKF/ekf.cpp

@@ -169,17 +169,33 @@ bool Ekf::initialiseFilter()
 		}
 	}
 
-	// accumulate enough height measurements to be confident in the quality of the data
-	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
-		if (_baro_sample_delayed.time_us != 0) {
-			if (_baro_counter == 0) {
-				_baro_hgt_offset = _baro_sample_delayed.hgt;
-
-			} else {
-				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
+	if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
+		// accumulate enough height measurements to be confident in the quality of the data
+		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
+			if (_baro_sample_delayed.time_us != 0) {
+				if (_hgt_counter == 0) {
+					_baro_hgt_offset = _baro_sample_delayed.hgt;
+
+				} else {
+					_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
+				}
+
+				_hgt_counter++;
 			}
+		}
+
+	} else if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
+		if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
+			if (_gps_sample_delayed.time_us != 0) {
+				if (_hgt_counter == 0) {
+					_baro_hgt_offset = _gps_sample_delayed.hgt;
 
-			_baro_counter++;
+				} else {
+					_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _gps_sample_delayed.hgt;
+				}
+
+				_hgt_counter++;
+			}
 		}
 	}
 
@@ -190,13 +206,18 @@ bool Ekf::initialiseFilter()
 		}
 	}
 
-	if (_baro_counter < _obs_buffer_length) {
+	if (_hgt_counter < _obs_buffer_length) {
 		// not enough baro samples accumulated
 		return false;
 	}
 
-	// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
-	setControlBaroHeight();
+	if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
+		// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
+		setControlBaroHeight();
+	} else if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
+		// without baro sensor use gps height initially
+		setControlGPSHeight();
+	}
 
 	if (!initialiseTilt()) {
 		return false;

EKF/ekf.h

@@ -468,14 +468,15 @@ class Ekf final : public EstimatorInterface
 	float _gps_error_norm{1.0f};		///< normalised gps error
 	uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
 	bool _gps_checks_passed{false};		///> true when all active GPS checks have passed
+	uint8_t _gps_fix_quality{0};
 
 	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	uint64_t _last_gps_origin_time_us{0};	///< time the origin was last set (uSec)
 	float _gps_alt_ref{0.0f};		///< WGS-84 height (m)
 
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
-	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
+	uint32_t _hgt_counter{0};		///< number of hgt samples read during initialisation
 	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
 	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)

EKF/ekf_helper.cpp

@@ -1607,7 +1607,7 @@ void Ekf::runYawEKFGSF()
 	}
 
 	const Vector3f imu_gyro_bias = getGyroBias();
-	_yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
+	_yawEstimator.update(_imu_sample_delayed, _control_status.flags.tilt_align, TAS, imu_gyro_bias);
 
 	// basic sanity check on GPS velocity data
 	if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON &&

EKF/estimator_interface.cpp

@@ -202,7 +202,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 		gps_sample_new.hacc = gps.eph;
 		gps_sample_new.vacc = gps.epv;
 
-		gps_sample_new.hgt = (float)gps.alt * 1e-3f;
+		gps_sample_new.hgt = gps.alt;
 
 		gps_sample_new.yaw = gps.yaw;
 
@@ -217,7 +217,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 		if (collect_gps(gps)) {
 			float lpos_x = 0.0f;
 			float lpos_y = 0.0f;
-			map_projection_project(&_pos_ref, (gps.lat / 1.0e7), (gps.lon / 1.0e7), &lpos_x, &lpos_y);
+			map_projection_project(&_pos_ref, gps.lat, gps.lon, &lpos_x, &lpos_y);
 			gps_sample_new.pos(0) = lpos_x;
 			gps_sample_new.pos(1) = lpos_y;
 

EKF/gps_checks.cpp

@@ -63,8 +63,8 @@ bool Ekf::collect_gps(const gps_message &gps)
 
 	if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
 		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-		const double lat = gps.lat * 1.0e-7;
-		const double lon = gps.lon * 1.0e-7;
+		const double lat = gps.lat;
+		const double lon = gps.lon;
 		map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
 
 		// if we are already doing aiding, correct for the change in position since the EKF started navigationg
@@ -75,7 +75,7 @@ bool Ekf::collect_gps(const gps_message &gps)
 		}
 
 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
-		_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
+		_gps_alt_ref = gps.alt + _state.pos(2);
 		_NED_origin_initialised = true;
 		_earth_rate_NED = calcEarthRateNED((float)_pos_ref.lat_rad);
 		_last_gps_origin_time_us = _time_last_imu;
@@ -118,8 +118,8 @@ bool Ekf::collect_gps(const gps_message &gps)
 			const bool declination_was_valid = ISFINITE(_mag_declination_gps);
 
 			// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-			const double lat = gps.lat * 1.0e-7;
-			const double lon = gps.lon * 1.0e-7;
+			const double lat = gps.lat;
+			const double lon = gps.lon;
 
 			// set the magnetic field data returned by the geo library using the current GPS position
 			_mag_declination_gps = get_mag_declination_radians(lat, lon);
@@ -150,6 +150,8 @@ bool Ekf::collect_gps(const gps_message &gps)
 */
 bool Ekf::gps_is_good(const gps_message &gps)
 {
+	_gps_fix_quality = gps.fix_type;
+
 	// Check the fix type
 	_gps_check_fail_status.flags.fix = (gps.fix_type < 3);
 
@@ -176,9 +178,8 @@ bool Ekf::gps_is_good(const gps_message &gps)
 	const float filter_coef = dt / filt_time_const;
 
 	// The following checks are only valid when the vehicle is at rest
-	const double lat = gps.lat * 1.0e-7;
-	const double lon = gps.lon * 1.0e-7;
-
+	const double lat = gps.lat;
+	const double lon = gps.lon;
 	if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 		// Calculate position movement since last measurement
 		float delta_pos_n = 0.0f;
@@ -191,12 +192,12 @@ bool Ekf::gps_is_good(const gps_message &gps)
 		} else {
 			// no previous position has been set
 			map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
-			_gps_alt_prev = 1e-3f * (float)gps.alt;
+			_gps_alt_prev = gps.alt;
 		}
 
 		// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
 		const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
-		Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - 1e-3f * (float)gps.alt));
+		Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt));
 		pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit);
 
 		// Apply a low pass filter
@@ -239,7 +240,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 
 	// save GPS fix for next time
 	map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
-	_gps_alt_prev = 1e-3f * (float)gps.alt;
+	_gps_alt_prev = gps.alt;
 
 	// Check  the filtered difference between GPS and EKF vertical velocity
 	const float vz_diff_limit = 10.0f * _params.req_vdrift;

EKF/gps_yaw_fusion.cpp

@@ -71,7 +71,7 @@ void Ekf::fuseGpsYaw()
 
 	// using magnetic heading process noise
 	// TODO extend interface to use yaw uncertainty provided by GPS if available
-	const float R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
+	const float R_YAW = sq(1.0e-2f);
 
 	// calculate intermediate variables
 	const float HK0 = sinf(_gps_yaw_offset);
@@ -208,7 +208,7 @@ bool Ekf::resetYawToGps()
 	// GPS yaw measurement is alreday compensated for antenna offset in the driver
 	const float measured_yaw = _gps_sample_delayed.yaw;
 
-	const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
+	const float yaw_variance = sq(1.0e-2f);
 	resetQuatStateYaw(measured_yaw, yaw_variance, true);
 
 	_time_last_gps_yaw_fuse = _time_last_imu;

EKF/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m

@@ -9,15 +9,15 @@
 gps_data.start_index = max(min(find(gps_data.pos_error < 5.0)),min(find(gps_data.spd_error < 1.0)));
 if isempty(gps_data.start_index)
     gps_data.start_index = 1;
-    gps_data.refLLH = [1e-7*median(lat);1e-7*median(lon);0.001*median(alt)];
+    gps_data.refLLH = [median(lat);median(lon);median(alt)];
 else
-    gps_data.refLLH = [1e-7*lat(gps_data.start_index);1e-7*lon(gps_data.start_index);0.001*alt(gps_data.start_index)];
+    gps_data.refLLH = [lat(gps_data.start_index);lon(gps_data.start_index);alt(gps_data.start_index)];
 end
 
 % convert GPS data to NED
 for index = 1:length(timestamp)
     if (fix_type(index) >= 3)
-        gps_data.pos_ned(index,:) = LLH2NED([1e-7*lat(index);1e-7*lon(index);0.001*alt(index)],gps_data.refLLH);
+        gps_data.pos_ned(index,:) = LLH2NED([lat(index);lon(index);alt(index)],gps_data.refLLH);
         gps_data.vel_ned(index,:) = [vel_n_m_s(index),vel_e_m_s(index),vel_d_m_s(index)];
     else
         gps_data.pos_ned(index,:) = [0,0,0];

test/sensor_simulator/gps.cpp

@@ -34,17 +34,17 @@ void Gps::setData(const gps_message& gps)
 	_gps_data = gps;
 }
 
-void Gps::setAltitude(int32_t alt)
+void Gps::setAltitude(float alt)
 {
 	_gps_data.alt = alt;
 }
 
-void Gps::setLatitude(int32_t lat)
+void Gps::setLatitude(double lat)
 {
 	_gps_data.lat = lat;
 }
 
-void Gps::setLongitude(int32_t lon)
+void Gps::setLongitude(double lon)
 {
 	_gps_data.lon = lon;
 }
@@ -79,7 +79,7 @@ void Gps::setPdop(float pdop)
 
 void Gps::stepHeightByMeters(float hgt_change)
 {
-	_gps_data.alt += hgt_change * 1e3f;
+	_gps_data.alt += hgt_change;
 }
 
 void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change)
@@ -90,13 +90,13 @@ void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change)
 	uint64_t time;
 	float alt;
 	_ekf->get_ekf_origin(&time, &origin, &alt);
-	map_projection_project(&origin, _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &hposN_curr, &hposE_curr);
+	map_projection_project(&origin, _gps_data.lat, _gps_data.lon, &hposN_curr, &hposE_curr);
 	Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change;
 	double lat_new;
 	double lon_new;
 	map_projection_reproject(&origin, hpos_new(0), hpos_new(1), &lat_new, &lon_new);
-	_gps_data.lon = static_cast<int32_t>(lon_new * 1e7);
-	_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
+	_gps_data.lon = lon_new;
+	_gps_data.lat = lat_new;
 }
 
 gps_message Gps::getDefaultGpsData()

test/sensor_simulator/gps.h

@@ -54,9 +54,9 @@ class Gps: public Sensor
 	void stepHeightByMeters(float hgt_change);
 	void stepHorizontalPositionByMeters(Vector2f hpos_change);
 	void setPositionRateNED(const Vector3f& rate) { _gps_pos_rate = rate; }
-	void setAltitude(int32_t alt);
-	void setLatitude(int32_t lat);
-	void setLongitude(int32_t lon);
+	void setAltitude(float alt);
+	void setLatitude(double lat);
+	void setLongitude(double lon);
 	void setVelocity(const Vector3f& vel);
 	void setYaw(float yaw);
 	void setYawOffset(float yaw);