diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 8de58a8782..bcc1f918d4 100644 --- a/EKF/ekf_helper.cpp +++ b/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(NAN); double current_lon = static_cast(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; diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index 9a15e605d3..40229b786c 100644 --- a/test/sensor_simulator/gps.cpp +++ b/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(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(lon_new * 1e7); _gps_data.lat = static_cast(lat_new * 1e7); } diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h index 8ff60d87b9..d5d50860fe 100644 --- a/test/sensor_simulator/gps.h +++ b/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 diff --git a/test/sensor_simulator/sensor_simulator.cpp b/test/sensor_simulator/sensor_simulator.cpp index 8b836607ca..fbb4c1ed4c 100644 --- a/test/sensor_simulator/sensor_simulator.cpp +++ b/test/sensor_simulator/sensor_simulator.cpp @@ -2,15 +2,15 @@ SensorSimulator::SensorSimulator(std::shared_ptr 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(latitude * 1e7); + _gps.setLatitude(lat); +} + +void SensorSimulator::setGpsLongitude(const double longitude) +{ + int32_t lon = static_cast(longitude * 1e7); + _gps.setLongitude(lon); +} + +void SensorSimulator::setGpsAltitude(const float altitude) +{ + int32_t alt = static_cast(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, diff --git a/test/sensor_simulator/sensor_simulator.h b/test/sensor_simulator/sensor_simulator.h index 6bd41b2c70..37e1ed2be2 100644 --- a/test/sensor_simulator/sensor_simulator.h +++ b/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 sensor_data {}; + uint64_t timestamp {}; + enum measurement_t {IMU, MAG, BARO, GPS, AIRSPEED, RANGE, FLOW, VISION, LANDING_STATUS} sensor_type = IMU; + std::array sensor_data {}; }; class SensorSimulator { -private: - std::shared_ptr _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); ~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 {nullptr}; + + std::vector _replay_data {}; bool _has_replay_data {false}; - std::vector _replay_data; + uint64_t _current_replay_data_index {0}; + uint64_t _time {0}; // microseconds }; diff --git a/test/test_EKF_basics.cpp b/test/test_EKF_basics.cpp index 7e92c1a2b6..0a53e89453 100644 --- a/test/test_EKF_basics.cpp +++ b/test/test_EKF_basics.cpp @@ -41,18 +41,13 @@ class EkfBasicsTest : public ::testing::Test { public: - EkfBasicsTest(): ::testing::Test(), - _ekf{std::make_shared()}, - _sensor_simulator(_ekf), - _ekf_wrapper(_ekf) {}; - - std::shared_ptr _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_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 {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