fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 05:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: set global origin method

Browse Source 
EKF: add convergence after global origin reset test
This commit is contained in:
Daniel Agar
2021-02-19 08:59:47 -05:00
parent 81937370ac
commit 63f64b57c1
7 changed files with 147 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/gps.cpp
+2 -6
View File
@@ -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
+53
View File
@@ -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