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

Interface: output vector quantities by "return-by-value"

Browse Source
This commit is contained in:
kamilritz
2020-03-30 13:07:00 +02:00
committed by Mathieu Bresciani
parent 72d8f91b4d
commit 2fa43419d2
16 changed files with 126 additions and 233 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_basics.cpp
+11 -11
View File
@@ -113,11 +113,11 @@ TEST_F(EkfBasicsTest, convergesToZero)
// GIVEN: initialized EKF with default IMU, baro and mag input
_sensor_simulator.runSeconds(4);
Vector3f pos = _ekf_wrapper.getPosition();
Vector3f vel = _ekf_wrapper.getVelocity();
Vector3f accel_bias = _ekf_wrapper.getAccelBias();
Vector3f gyro_bias = _ekf_wrapper.getGyroBias();
Vector3f ref{0.0f, 0.0f, 0.0f};
const Vector3f pos = _ekf->getPosition();
const Vector3f vel = _ekf->getVelocity();
const Vector3f accel_bias = _ekf->getAccelBias();
const Vector3f gyro_bias = _ekf->getGyroBias();
const Vector3f ref{0.0f, 0.0f, 0.0f};
// THEN: EKF should stay or converge to zero
EXPECT_TRUE(matrix::isEqual(pos, ref, 0.001f));
@@ -168,17 +168,17 @@ TEST_F(EkfBasicsTest, accleBiasEstimation)
{
// GIVEN: initialized EKF with default IMU, baro and mag input for 3s
// WHEN: Added more sensor measurements with accel bias and gps measurements
Vector3f accel_bias_sim = {0.0f,0.0f,0.1f};
const Vector3f accel_bias_sim = {0.0f,0.0f,0.1f};
_sensor_simulator.startGps();
_sensor_simulator.setImuBias(accel_bias_sim, Vector3f{0.0f,0.0f,0.0f});
_sensor_simulator.runSeconds(10);
Vector3f pos = _ekf_wrapper.getPosition();
Vector3f vel = _ekf_wrapper.getVelocity();
Vector3f accel_bias = _ekf_wrapper.getAccelBias();
Vector3f gyro_bias = _ekf_wrapper.getGyroBias();
Vector3f zero{0.0f, 0.0f, 0.0f};
const Vector3f pos = _ekf->getPosition();
const Vector3f vel = _ekf->getVelocity();
const Vector3f accel_bias = _ekf->getAccelBias();
const Vector3f gyro_bias = _ekf->getGyroBias();
const Vector3f zero{0.0f, 0.0f, 0.0f};
// THEN: EKF should stay or converge to zero
EXPECT_TRUE(matrix::isEqual(pos, zero, 0.01f));