mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 08:03:34 +08:00
Add unit tests and update ekf_helper setEkfGlobalOrigin() method to allow for condition when current position is unitialized.
This commit is contained in:
mcsauder
+4
-2
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
+13
-14
@@ -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
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
|
||||
};
|
||||
|
||||
+88
-43
@@ -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(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);
|
||||
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user