EKF: set global origin method

EKF: add convergence after global origin reset test

EKF/control.cpp

@@ -1131,7 +1131,7 @@ void Ekf::controlHeightFusion()
 			gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
 			// fuse height information
 			fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate,
-				gps_hgt_obs_var, _gps_pos_innov_var,_gps_pos_test_ratio);
+				gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
 
 		} else if (_control_status.flags.rng_hgt) {
 			Vector2f rng_hgt_innov_gate;

EKF/ekf.h

@@ -151,7 +151,12 @@ public:
 
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
-	bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const;
+	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
+	bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
+
+	float getEkfGlobalOriginAltitude() const { return _gps_alt_ref; }
+	bool setEkfGlobalOriginAltitude(const float altitude);
+
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
@@ -621,6 +626,8 @@ private:
 
 	inline void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
 
+	inline void resetVerticalPositionTo(const float &new_vert_pos);
+
 	void resetHeight();
 
 	// fuse optical flow line of sight rate measurements

EKF/ekf_helper.cpp

@@ -210,7 +210,7 @@ void Ekf::resetHorizontalPositionToVision()
 
 void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
 {
-	const Vector2f delta_horz_pos = new_horz_pos - Vector2f(_state.pos);
+	const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
 	_state.pos.xy() = new_horz_pos;
 
 	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
@@ -223,28 +223,43 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
 	_state_reset_status.posNE_counter++;
 }
 
+void Ekf::resetVerticalPositionTo(const float &new_vert_pos)
+{
+	const float old_vert_pos = _state.pos(2);
+	_state.pos(2) = new_vert_pos;
+
+	// store the reset amount and time to be published
+	_state_reset_status.posD_change = new_vert_pos - old_vert_pos;
+	_state_reset_status.posD_counter++;
+
+	// apply the change in height / height rate to our newest height / height rate estimate
+	// which have already been taken out from the output buffer
+	_output_new.pos(2) += _state_reset_status.posD_change;
+
+	// add the reset amount to the output observer buffered data
+	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
+		_output_buffer[i].pos(2) += _state_reset_status.posD_change;
+		_output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change;
+	}
+
+	// add the reset amount to the output observer vertical position state
+	_output_vert_new.vert_vel_integ = _state.pos(2);
+}
+
 // Reset height state using the last height measurement
 void Ekf::resetHeight()
 {
 	// Get the most recent GPS data
 	const gpsSample &gps_newest = _gps_buffer.get_newest();
 
-	// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
-	const float old_vert_pos = _state.pos(2);
-	bool vert_pos_reset = false;
-
 	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {
-		const float new_pos_down = _hgt_sensor_offset - _range_sensor.getDistBottom();
-
 		// update the state and associated variance
-		_state.pos(2) = new_pos_down;
+		resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom());
 
 		// the state variance is the same as the observation
 		P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
 
-		vert_pos_reset = true;
-
 		// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
 		const baroSample &baro_newest = _baro_buffer.get_newest();
 		_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
@@ -254,13 +269,11 @@ void Ekf::resetHeight()
 		const baroSample &baro_newest = _baro_buffer.get_newest();
 
 		if (!_baro_hgt_faulty) {
-			_state.pos(2) = -baro_newest.hgt + _baro_hgt_offset;
+			resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset);
 
 			// the state variance is the same as the observation
 			P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
 
-			vert_pos_reset = true;
-
 		} else {
 			// TODO: reset to last known baro based estimate
 		}
@@ -268,13 +281,11 @@ void Ekf::resetHeight()
 	} else if (_control_status.flags.gps_hgt) {
 		// initialize vertical position and velocity with newest gps measurement
 		if (!_gps_hgt_intermittent) {
-			_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
+			resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref);
 
 			// the state variance is the same as the observation
 			P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc));
 
-			vert_pos_reset = true;
-
 			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
 			const baroSample &baro_newest = _baro_buffer.get_newest();
 			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
@@ -288,16 +299,11 @@ void Ekf::resetHeight()
 		const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
 
 		// use the most recent data if it's time offset from the fusion time horizon is smaller
-		const int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
-		const int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
-
-		vert_pos_reset = true;
-
-		if (std::abs(dt_newest) < std::abs(dt_delayed)) {
-			_state.pos(2) = ev_newest.pos(2);
+		if (ev_newest.time_us >= _ev_sample_delayed.time_us) {
+			resetVerticalPositionTo(ev_newest.pos(2));
 
 		} else {
-			_state.pos(2) = _ev_sample_delayed.pos(2);
+			resetVerticalPositionTo(_ev_sample_delayed.pos(2));
 		}
 	}
 
@@ -317,31 +323,6 @@ void Ekf::resetHeight()
 		// that does not destabilise the filter
 		P.uncorrelateCovarianceSetVariance<1>(6, 10.0f);
 	}
-
-	// store the reset amount and time to be published
-	if (vert_pos_reset) {
-		_state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
-		_state_reset_status.posD_counter++;
-	}
-
-	// apply the change in height / height rate to our newest height / height rate estimate
-	// which have already been taken out from the output buffer
-	if (vert_pos_reset) {
-		_output_new.pos(2) += _state_reset_status.posD_change;
-	}
-
-	// add the reset amount to the output observer buffered data
-	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		if (vert_pos_reset) {
-			_output_buffer[i].pos(2) += _state_reset_status.posD_change;
-			_output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change;
-		}
-	}
-
-	// add the reset amount to the output observer vertical position state
-	if (vert_pos_reset) {
-		_output_vert_new.vert_vel_integ = _state.pos(2);
-	}
 }
 
 // align output filter states to match EKF states at the fusion time horizon
@@ -693,16 +674,49 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 	return state;
 }
 
-// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
-// return true if the origin is valid
-bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const
+bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
 {
-	memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
-	memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
-	memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
+	origin_time = _last_gps_origin_time_us;
+	latitude    = math::degrees(_pos_ref.lat_rad);
+	longitude   = math::degrees(_pos_ref.lon_rad);
+	origin_alt  = _gps_alt_ref;
 	return _NED_origin_initialised;
 }
 
+bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude)
+{
+	bool current_pos_available = false;
+	double current_lat = static_cast<double>(NAN);
+	double current_lon = static_cast<double>(NAN);
+	float current_alt  = NAN;
+
+	// if we are already doing aiding, correct for the change in position since the EKF started navigating
+	if (map_projection_initialized(&_pos_ref) && isHorizontalAidingActive()) {
+		map_projection_reproject(&_pos_ref, _state.pos(0), _state.pos(1), &current_lat, &current_lon);
+		current_alt = -_state.pos(2) + _gps_alt_ref;
+		current_pos_available = true;
+	}
+
+	// reinitialize map projection to latitude, longitude, altitude, and reset position
+	if (map_projection_init_timestamped(&_pos_ref, latitude, longitude, _time_last_imu) == 0) {
+
+		if (current_pos_available) {
+			// reset horizontal position
+			Vector2f position;
+			map_projection_project(&_pos_ref, current_lat, current_lon, &position(0), &position(1));
+			resetHorizontalPositionTo(position);
+
+			// reset altitude
+			_gps_alt_ref = altitude;
+			resetVerticalPositionTo(_gps_alt_ref - current_alt);
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
 /*
 	First argument returns GPS drift  metrics in the following array locations
 	0 : Horizontal position drift rate (m/s)

EKF/estimator_interface.h

@@ -229,6 +229,9 @@ public:
 	// Getter for the baro sample on the delayed time horizon
 	const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
 
+	const bool& global_origin_valid() const { return _NED_origin_initialised; }
+	const map_projection_reference_s& global_origin() const { return _pos_ref; }
+
 	void print_status();
 
 	static constexpr unsigned FILTER_UPDATE_PERIOD_MS{10};	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta

EKF/gps_checks.cpp

@@ -65,13 +65,17 @@ bool Ekf::collect_gps(const gps_message &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;
-		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
-		if (isHorizontalAidingActive()) {
-			double est_lat, est_lon;
-			map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
-			map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
+		if (!map_projection_initialized(&_pos_ref)) {
+			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 navigating
+			if (isHorizontalAidingActive()) {
+				double est_lat;
+				double est_lon;
+				map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
+				map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
+			}
 		}
 
 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
@@ -89,7 +93,7 @@ bool Ekf::collect_gps(const gps_message &gps)
 
 		// request a reset of the yaw using the new declination
 		if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
-			// try to reset the yaw using the EKF-GSF yaw esitimator
+			// try to reset the yaw using the EKF-GSF yaw estimator
 			_do_ekfgsf_yaw_reset = true;
 			_ekfgsf_yaw_reset_time = 0;
 
@@ -104,7 +108,6 @@ bool Ekf::collect_gps(const gps_message &gps)
 		_gps_origin_epv = gps.epv;
 
 		// if the user has selected GPS as the primary height source, switch across to using it
-
 		if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
 			startGpsHgtFusion();
 		}

test/sensor_simulator/gps.cpp

@@ -86,15 +86,11 @@ void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change)
 {
 	float hposN_curr;
 	float hposE_curr;
-	map_projection_reference_s origin;
-	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(&_ekf->global_origin(), _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &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);
+	map_projection_reproject(&_ekf->global_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);
 }

test/test_EKF_basics.cpp

@@ -31,6 +31,7 @@
  *
  ****************************************************************************/
 
+#include <chrono>
 #include <gtest/gtest.h>
 #include <math.h>
 #include <memory>
@@ -187,4 +188,56 @@ TEST_F(EkfBasicsTest, accelBiasEstimation)
 		<< "gyro_bias = " << gyro_bias(0) << ", " << gyro_bias(1) << ", " << gyro_bias(2);
 }
 
+TEST_F(EkfBasicsTest, reset_ekf_global_origin)
+{
+	double latitude  {0.0};
+	double longitude {0.0};
+	float  altitude  {0.f};
+
+	double latitude_new  {0.0};
+	double longitude_new {0.0};
+	float  altitude_new  {0.f};
+
+	uint64_t origin_time = 0;
+
+	_ekf->getEkfGlobalOrigin(origin_time, latitude_new, longitude_new, altitude_new);
+
+	EXPECT_DOUBLE_EQ(latitude, latitude_new);
+	EXPECT_DOUBLE_EQ(longitude, longitude_new);
+	EXPECT_FLOAT_EQ(altitude, altitude_new);
+
+	_sensor_simulator.startGps();
+	_ekf->set_min_required_gps_health_time(1e6);
+	_sensor_simulator.runSeconds(1);
+	sleep(1);
+
+	latitude_new  = 45.0000005;
+	longitude_new = -111.0000005;
+	altitude_new  = 1500.0;
+
+	_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new);
+	_ekf->getEkfGlobalOrigin(origin_time, latitude, longitude, altitude);
+
+	// EKF origin MSL altitude cannot be reset without valid MSL origin.
+	EXPECT_DOUBLE_EQ(latitude, latitude_new);
+	EXPECT_DOUBLE_EQ(longitude, longitude_new);
+
+	// After the change of origin, the pos and vel innovations should stay small
+	_sensor_simulator.runSeconds(1);
+	sleep(1);
+
+	float hpos = 0.f;
+	float vpos = 0.f;
+	float hvel = 0.f;
+	float vvel = 0.f;
+
+	_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
+
+	EXPECT_NEAR(hvel, 0.f, 0.02f);
+	EXPECT_NEAR(vvel, 0.f, 0.02f);
+
+	EXPECT_NEAR(hpos, 0.f, 0.05f);
+	EXPECT_NEAR(vpos, 0.f, 0.05f);
+}
+
 // TODO: Add sampling tests