Add unit tests and update ekf_helper setEkfGlobalOrigin() method to allow for condition when current position is unitialized.

EKF/ekf_helper.cpp

@@ -688,7 +688,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 	bool current_pos_available = false;
 	double current_lat = static_cast<double>(NAN);
 	double current_lon = static_cast<double>(NAN);
-	float current_alt  = NAN;
+	float current_alt  = 0.f;
 
 	// if we are already doing aiding, correct for the change in position since the EKF started navigating
 	if (map_projection_initialized(&_pos_ref) && isHorizontalAidingActive()) {
@@ -699,7 +699,6 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 
 	// 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;
@@ -709,6 +708,9 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 			// reset altitude
 			_gps_alt_ref = altitude;
 			resetVerticalPositionTo(_gps_alt_ref - current_alt);
+		} else {
+			// reset altitude
+			_gps_alt_ref = altitude;
 		}
 
 		return true;

test/sensor_simulator/gps.cpp

@@ -13,7 +13,7 @@ Gps::~Gps()
 {
 }
 
-void Gps::send(uint64_t time)
+void Gps::send(const uint64_t time)
 {
 	const float dt = static_cast<float>(time - _gps_data.time_usec) * 1e-6f;
 
@@ -34,17 +34,17 @@ void Gps::setData(const gps_message& gps)
 	_gps_data = gps;
 }
 
-void Gps::setAltitude(int32_t alt)
+void Gps::setAltitude(const int32_t alt)
 {
 	_gps_data.alt = alt;
 }
 
-void Gps::setLatitude(int32_t lat)
+void Gps::setLatitude(const int32_t lat)
 {
 	_gps_data.lat = lat;
 }
 
-void Gps::setLongitude(int32_t lon)
+void Gps::setLongitude(const int32_t lon)
 {
 	_gps_data.lon = lon;
 }
@@ -54,43 +54,53 @@ void Gps::setVelocity(const Vector3f& vel)
 	_gps_data.vel_ned = vel;
 }
 
-void Gps::setYaw(float yaw) {
+void Gps::setYaw(const float yaw) {
 	_gps_data.yaw = yaw;
 }
 
-void Gps::setYawOffset(float yaw_offset) {
+void Gps::setYawOffset(const float yaw_offset) {
 	_gps_data.yaw_offset = yaw_offset;
 }
 
-void Gps::setFixType(int n)
+void Gps::setFixType(const int fix_type)
 {
-	_gps_data.fix_type = n;
+	_gps_data.fix_type = fix_type;
 }
 
-void Gps::setNumberOfSatellites(int n)
+void Gps::setNumberOfSatellites(const int num_satellites)
 {
-	_gps_data.nsats = n;
+	_gps_data.nsats = num_satellites;
 }
 
-void Gps::setPdop(float pdop)
+void Gps::setPdop(const float pdop)
 {
 	_gps_data.pdop = pdop;
 }
 
-void Gps::stepHeightByMeters(float hgt_change)
+void Gps::setPositionRateNED(const Vector3f& rate)
+{
+	_gps_pos_rate = rate;
+}
+
+void Gps::stepHeightByMeters(const float hgt_change)
 {
 	_gps_data.alt += hgt_change * 1e3f;
 }
 
-void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change)
+void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
 {
-	float hposN_curr;
-	float hposE_curr;
+	float hposN_curr {0.f};
+	float hposE_curr {0.f};
+
+	double lat_new {0.0};
+	double lon_new {0.0};
+
 	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(&_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/sensor_simulator/gps.h

@@ -51,27 +51,26 @@ public:
 	~Gps();
 
 	void setData(const gps_message& gps);
-	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 stepHeightByMeters(const float hgt_change);
+	void stepHorizontalPositionByMeters(const Vector2f hpos_change);
+	void setPositionRateNED(const Vector3f& rate);
+	void setAltitude(const int32_t alt);
+	void setLatitude(const int32_t lat);
+	void setLongitude(const int32_t lon);
 	void setVelocity(const Vector3f& vel);
-	void setYaw(float yaw);
-	void setYawOffset(float yaw);
-	void setFixType(int n);
-	void setNumberOfSatellites(int n);
-	void setPdop(float pdop);
+	void setYaw(const float yaw);
+	void setYawOffset(const float yaw);
+	void setFixType(const int fix_type);
+	void setNumberOfSatellites(const int num_satellites);
+	void setPdop(const float pdop);
 
 	gps_message getDefaultGpsData();
 
 private:
-	gps_message _gps_data{};
-	Vector3f _gps_pos_rate{};
-
 	void send(uint64_t time) override;
 
+	gps_message _gps_data{};
+	Vector3f _gps_pos_rate{};
 };
 
 } // namespace sensor

test/sensor_simulator/sensor_simulator.cpp

@@ -2,15 +2,15 @@
 
 
 SensorSimulator::SensorSimulator(std::shared_ptr<Ekf> ekf):
-_ekf{ekf},
-_imu(ekf),
-_mag(ekf),
-_baro(ekf),
-_gps(ekf),
-_flow(ekf),
-_rng(ekf),
-_vio(ekf),
-_airspeed(ekf)
+	_airspeed(ekf),
+	_baro(ekf),
+	_flow(ekf),
+	_gps(ekf),
+	_imu(ekf),
+	_mag(ekf),
+	_rng(ekf),
+	_vio(ekf),
+	_ekf{ekf}
 {
 	setSensorRateToDefault();
 	setSensorDataToDefault();
@@ -115,23 +115,24 @@ void SensorSimulator::setSensorRateToDefault()
 	_vio.setRateHz(30);
 	_airspeed.setRateHz(100);
 }
+
 void SensorSimulator::setSensorDataToDefault()
 {
-	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G},
-		     Vector3f{0.0f,0.0f,0.0f});
-	_mag.setData(Vector3f{0.2f, 0.0f, 0.4f});
+	_airspeed.setData(0.0f, 0.0f);
 	_baro.setData(122.2f);
-	_gps.setData(_gps.getDefaultGpsData());
 	_flow.setData(_flow.dataAtRest());
+	_gps.setData(_gps.getDefaultGpsData());
+	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}, Vector3f{0.0f,0.0f,0.0f});
+	_mag.setData(Vector3f{0.2f, 0.0f, 0.4f});
 	_rng.setData(0.2f, 100);
 	_vio.setData(_vio.dataAtRest());
-	_airspeed.setData(0.0f, 0.0f);
 }
+
 void SensorSimulator::startBasicSensor()
 {
+	_baro.start();
 	_imu.start();
 	_mag.start();
-	_baro.start();
 }
 
 void SensorSimulator::runSeconds(float duration_seconds)
@@ -280,6 +281,24 @@ void SensorSimulator::setSingleReplaySample(const sensor_info& sample)
 	}
 }
 
+void SensorSimulator::setGpsLatitude(const double latitude)
+{
+	int32_t lat = static_cast<int32_t>(latitude * 1e7);
+	_gps.setLatitude(lat);
+}
+
+void SensorSimulator::setGpsLongitude(const double longitude)
+{
+	int32_t lon = static_cast<int32_t>(longitude * 1e7);
+	_gps.setLongitude(lon);
+}
+
+void SensorSimulator::setGpsAltitude(const float altitude)
+{
+	int32_t alt = static_cast<int32_t>(altitude * 1e3);
+	_gps.setAltitude(alt);
+}
+
 void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)
 {
 	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias,

test/sensor_simulator/sensor_simulator.h

@@ -62,32 +62,19 @@
 using namespace sensor_simulator::sensor;
 
 struct sensor_info {
-uint64_t timestamp {};
-enum measurement_t {IMU, MAG, BARO, GPS, AIRSPEED, RANGE, FLOW, VISION, LANDING_STATUS} sensor_type = IMU;
-std::array<double, 10> sensor_data {};
+	uint64_t timestamp {};
+	enum measurement_t {IMU, MAG, BARO, GPS, AIRSPEED, RANGE, FLOW, VISION, LANDING_STATUS} sensor_type = IMU;
+	std::array<double, 10> sensor_data {};
 };
 
 class SensorSimulator
 {
 
-private:
-	std::shared_ptr<Ekf> _ekf;
-
-	uint64_t _time {0};	// in microseconds
-
-	void setSensorDataToDefault();
-	void setSensorRateToDefault();
-	void startBasicSensor();
-	void updateSensors();
-	void setSensorDataFromReplayData();
-	void setSingleReplaySample(const sensor_info& sample);
-
-
 public:
 	SensorSimulator(std::shared_ptr<Ekf> ekf);
 	~SensorSimulator();
 
-	uint64_t getTime() const{ return _time; };
+	uint64_t getTime() const { return _time; };
 
 	void runSeconds(float duration_seconds);
 	void runMicroseconds(uint32_t duration);
@@ -95,40 +82,60 @@ public:
 	void runReplaySeconds(float duration_seconds);
 	void runReplayMicroseconds(uint32_t duration);
 
-	void startBaro(){ _baro.start(); }
-	void stopBaro(){ _baro.stop(); }
+	void startBaro() { _baro.start(); }
+	void stopBaro() { _baro.stop(); }
+
+	void startGps() { _gps.start(); }
+	void stopGps() { _gps.stop(); }
 
-	void startGps(){ _gps.start(); }
-	void stopGps(){ _gps.stop(); }
+	void startFlow() { _flow.start(); }
+	void stopFlow() { _flow.stop(); }
 
-	void startFlow(){ _flow.start(); }
-	void stopFlow(){ _flow.stop(); }
+	void startRangeFinder() { _rng.start(); }
+	void stopRangeFinder() { _rng.stop(); }
 
-	void startRangeFinder(){ _rng.start(); }
-	void stopRangeFinder(){ _rng.stop(); }
+	void startExternalVision() { _vio.start(); }
+	void stopExternalVision() { _vio.stop(); }
 
-	void startExternalVision(){ _vio.start(); }
-	void stopExternalVision(){ _vio.stop(); }
+	void startAirspeedSensor() { _airspeed.start(); }
+	void stopAirspeedSensor() { _airspeed.stop(); }
 
-	void startAirspeedSensor(){ _airspeed.start(); }
-	void stopAirspeedSensor(){ _airspeed.stop(); }
+	void setGpsLatitude(const double latitude);
+	void setGpsLongitude(const double longitude);
+	void setGpsAltitude(const float altitude);
 
 	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
+
 	void simulateOrientation(Quatf orientation);
 
 	void loadSensorDataFromFile(std::string filename);
 
-	Imu _imu;
-	Mag _mag;
-	Baro _baro;
-	Gps _gps;
-	Flow _flow;
+	Airspeed    _airspeed;
+	Baro        _baro;
+	Flow        _flow;
+	Gps         _gps;
+	Imu         _imu;
+	Mag         _mag;
 	RangeFinder _rng;
-	Vio _vio;
-	Airspeed _airspeed;
+	Vio         _vio;
+
+protected:
+
+private:
+	void setSensorDataToDefault();
+	void setSensorDataFromReplayData();
+	void setSensorRateToDefault();
+	void setSingleReplaySample(const sensor_info& sample);
+	void startBasicSensor();
+	void updateSensors();
+
+	std::shared_ptr<Ekf> _ekf {nullptr};
+
+	std::vector<sensor_info> _replay_data {};
 
 	bool _has_replay_data {false};
-	std::vector<sensor_info> _replay_data;
+
 	uint64_t _current_replay_data_index {0};
+	uint64_t _time {0};	// microseconds
 
 };

test/test_EKF_basics.cpp

@@ -41,18 +41,13 @@
 
 class EkfBasicsTest : public ::testing::Test {
  public:
-	EkfBasicsTest(): ::testing::Test(),
-	_ekf{std::make_shared<Ekf>()},
-	_sensor_simulator(_ekf),
-	_ekf_wrapper(_ekf) {};
-
-	std::shared_ptr<Ekf> _ekf;
-	SensorSimulator _sensor_simulator;
-	EkfWrapper _ekf_wrapper;
-
-
-	// Duration of initalization with only providing baro,mag and IMU
-	const uint32_t _init_duration_s{7};
+	EkfBasicsTest():
+		::testing::Test(),
+		_ekf{std::make_shared<Ekf>()},
+		_ekf_wrapper(_ekf) ,
+		_sensor_simulator(_ekf)
+	{
+	};
 
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
@@ -65,6 +60,27 @@ class EkfBasicsTest : public ::testing::Test {
 	void TearDown() override
 	{
 	}
+
+	std::shared_ptr<Ekf> _ekf {nullptr};
+	EkfWrapper _ekf_wrapper;
+	SensorSimulator _sensor_simulator;
+
+	// Duration of initalization with only providing baro,mag and IMU
+	const uint32_t _init_duration_s{7};
+
+protected:
+	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;
+
+private:
+
 };
 
 
@@ -188,56 +204,85 @@ TEST_F(EkfBasicsTest, accelBiasEstimation)
 		<< "gyro_bias = " << gyro_bias(0) << ", " << gyro_bias(1) << ", " << gyro_bias(2);
 }
 
-TEST_F(EkfBasicsTest, reset_ekf_global_origin)
+TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
 {
-	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);
+	_latitude_new  = 15.0000005;
+	_longitude_new = 115.0000005;
+	_altitude_new  = 100.0;
 
 	_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;
+	_sensor_simulator.setGpsLatitude(_latitude_new);
+	_sensor_simulator.setGpsLongitude(_longitude_new);
+	_sensor_simulator.setGpsAltitude(_altitude_new);
+	_sensor_simulator.runSeconds(2);
 
-	_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new);
-	_ekf->getEkfGlobalOrigin(origin_time, latitude, longitude, altitude);
+	_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);
+	EXPECT_DOUBLE_EQ(_latitude, _latitude_new);
+	EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
+	EXPECT_NEAR(_altitude, _altitude_new, 0.01f);
 
-	// After the change of origin, the pos and vel innovations should stay small
-	_sensor_simulator.runSeconds(1);
-	sleep(1);
+	_latitude_new  = -15.0000005;
+	_longitude_new = -115.0000005;
+	_altitude_new  = 1500.0;
+
+	_ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new);
+	_ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude);
+
+	EXPECT_DOUBLE_EQ(_latitude, _latitude_new);
+	EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
+	EXPECT_FLOAT_EQ(_altitude, _altitude_new);
 
 	float hpos = 0.f;
 	float vpos = 0.f;
 	float hvel = 0.f;
 	float vvel = 0.f;
 
+	// After the change of origin, the pos and vel innovations should stay small
 	_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
 
+	EXPECT_NEAR(hpos, 0.f, 0.05f);
+	EXPECT_NEAR(vpos, 0.f, 0.05f);
+
 	EXPECT_NEAR(hvel, 0.f, 0.02f);
 	EXPECT_NEAR(vvel, 0.f, 0.02f);
+}
+
+TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
+{
+	_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);
+
+	_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);
+
+	EXPECT_DOUBLE_EQ(_latitude, _latitude_new);
+	EXPECT_DOUBLE_EQ(_longitude, _longitude_new);
+	EXPECT_FLOAT_EQ(_altitude, _altitude_new);
+
+	float hpos = 0.f;
+	float vpos = 0.f;
+	float hvel = 0.f;
+	float vvel = 0.f;
+
+	// After the change of origin, the pos and vel innovations should stay small
+	_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
 
 	EXPECT_NEAR(hpos, 0.f, 0.05f);
 	EXPECT_NEAR(vpos, 0.f, 0.05f);
+
+	EXPECT_NEAR(hvel, 0.f, 0.02f);
+	EXPECT_NEAR(vvel, 0.f, 0.02f);
 }
 
 // TODO: Add sampling tests