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

GPS Yaw: fix heading initialisation and add unit tests

Browse Source 
When the antennas are not parallel to the x body axis, the GPS message
contains the angular offset but the data is already corrected in the
driver. EKF2 should then not add this offset during the initialisation.
This commit is contained in:
bresch 2020-06-30 17:12:04 +02:00 committed by Mathieu Bresciani
parent 97225fcb74
commit 7f4fedde6a
4 changed files with 71 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/gps_yaw_fusion.cpp
View File

@ -273,8 +273,8 @@ bool Ekf::resetGpsAntYaw()
return false;
}
// get measurement and correct for antenna array yaw offset
const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
// GPS yaw measurement is alreday compensated for antenna offset in the driver
const float measured_yaw = _gps_sample_delayed.yaw;
const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
resetQuatStateYaw(measured_yaw, yaw_variance, true);

4
test/sensor_simulator/gps.cpp
View File

@ -58,6 +58,10 @@ void Gps::setYaw(float yaw) {
_gps_data.yaw = yaw;
}
void Gps::setYawOffset(float yaw_offset) {
_gps_data.yaw_offset = yaw_offset;
}
void Gps::setFixType(int n)
{
_gps_data.fix_type = n;

1
test/sensor_simulator/gps.h
View File

@ -59,6 +59,7 @@ public:
void setLongitude(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);

71
test/test_EKF_gps_yaw.cpp
View File

@ -54,6 +54,9 @@ class EkfGpsHeadingTest : public ::testing::Test {
SensorSimulator _sensor_simulator;
EkfWrapper _ekf_wrapper;
void runConvergenceScenario(float yaw_offset_rad = 0.f, float antenna_offset_rad = 0.f);
void checkConvergence(float truth, float tolerance = FLT_EPSILON);
// Setup the Ekf with synthetic measurements
void SetUp() override
{
@ -67,6 +70,29 @@ class EkfGpsHeadingTest : public ::testing::Test {
}
};
void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float antenna_offset_rad)
{
// GIVEN: an initial GPS yaw, not aligned with the current one
float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + yaw_offset_rad);
_sensor_simulator._gps.setYaw(gps_heading);
_sensor_simulator._gps.setYawOffset(antenna_offset_rad);
// WHEN: the GPS yaw fusion is activated
_ekf_wrapper.enableGpsHeadingFusion();
_sensor_simulator.runSeconds(5);
// THEN: the estimate is reset and stays close to the measurement
checkConvergence(gps_heading, 0.05f);
}
void EkfGpsHeadingTest::checkConvergence(float truth, float tolerance)
{
const float yaw_est = _ekf_wrapper.getYawAngle();
EXPECT_NEAR(yaw_est, truth, math::radians(tolerance))
<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(truth);
}
TEST_F(EkfGpsHeadingTest, fusionStartWithReset)
{
// GIVEN:EKF that fuses GPS
@ -96,7 +122,9 @@ TEST_F(EkfGpsHeadingTest, fusionStartWithReset)
TEST_F(EkfGpsHeadingTest, yawConvergence)
{
// GIVEN: an initial GPS yaw, not aligned with the current one
float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
const float initial_yaw = math::radians(10.f);
float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + initial_yaw);
_sensor_simulator._gps.setYaw(gps_heading);
// WHEN: the GPS yaw fusion is activated
@ -104,9 +132,7 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
_sensor_simulator.runSeconds(5);
// THEN: the estimate is reset and stays close to the measurement
float yaw_est = _ekf_wrapper.getYawAngle();
EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.05f))
<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading);
checkConvergence(gps_heading, 0.05f);
// AND WHEN: the the measurement changes
gps_heading += math::radians(2.f);
@ -115,9 +141,40 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
// THEN: the estimate slowly converges to the new measurement
// Note that the process is slow, because the gyro did not detect any motion
yaw_est = _ekf_wrapper.getYawAngle();
EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.5f))
<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading);
checkConvergence(gps_heading, 0.5f);
}
TEST_F(EkfGpsHeadingTest, yaw0)
{
runConvergenceScenario();
}
TEST_F(EkfGpsHeadingTest, yaw60)
{
const float yaw_offset_rad = math::radians(60.f);
const float antenna_offset_rad = math::radians(80.f);
runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
}
TEST_F(EkfGpsHeadingTest, yaw180)
{
const float yaw_offset_rad = math::radians(180.f);
const float antenna_offset_rad = math::radians(-20.f);
runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
}
TEST_F(EkfGpsHeadingTest, yawMinus120)
{
const float yaw_offset_rad = math::radians(120.f);
const float antenna_offset_rad = math::radians(-42.f);
runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
}
TEST_F(EkfGpsHeadingTest, yawMinus30)
{
const float yaw_offset_rad = math::radians(-30.f);
const float antenna_offset_rad = math::radians(10.f);
runConvergenceScenario(yaw_offset_rad, antenna_offset_rad);
}
TEST_F(EkfGpsHeadingTest, fallBackToMag)