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

Expand test framework and test cases (#685)

Browse Source 
* Fix comment

* Ekf wrapper for testing

Add utility function for accessing information in the ekf object

* Add step function for Gps sensor

* Add RangeFinder and Flow to simulated sensors

* Add first fusion logic tests

* Add units to function name

* Use EXPECT_TRUE

* Adding missing qualifiers

* Improve EXPECT_ calls

* Improve naming
This commit is contained in:
kritz
2019-12-17 11:35:45 +01:00
committed by Mathieu Bresciani
parent 6c25ac5731
commit 532c9abd4a
23 changed files with 679 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_basics.cpp
+31 -40
View File
@@ -36,16 +36,19 @@
#include <memory>
#include "EKF/ekf.h"
#include "sensor_simulator/sensor_simulator.h"
#include "sensor_simulator/ekf_wrapper.h"
class EkfInitializationTest : public ::testing::Test {
public:
EkfInitializationTest(): ::testing::Test(),
_ekf{std::make_shared<Ekf>()},
_sensor_simulator(_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{2};
@@ -54,7 +57,7 @@ class EkfInitializationTest : public ::testing::Test {
void SetUp() override
{
_ekf->init(0);
_sensor_simulator.run_seconds(_init_duration_s);
_sensor_simulator.runSeconds(_init_duration_s);
}
// Use this method to clean up any memory, network etc. after each test
@@ -68,7 +71,7 @@ TEST_F(EkfInitializationTest, tiltAlign)
{
// GIVEN: reasonable static sensor data for some duration
// THEN: EKF should tilt align
EXPECT_EQ(true,_ekf->attitude_valid());
EXPECT_TRUE(_ekf->attitude_valid());
}
TEST_F(EkfInitializationTest, initialControlMode)
@@ -108,25 +111,19 @@ TEST_F(EkfInitializationTest, initialControlMode)
TEST_F(EkfInitializationTest, convergesToZero)
{
// GIVEN: initialized EKF with default IMU, baro and mag input
_sensor_simulator.run_seconds(4);
_sensor_simulator.runSeconds(4);
float converged_pos[3];
float converged_vel[3];
float converged_accel_bias[3];
float converged_gyro_bias[3];
_ekf->get_position(converged_pos);
_ekf->get_velocity(converged_vel);
_ekf->get_accel_bias(converged_accel_bias);
_ekf->get_gyro_bias(converged_gyro_bias);
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};
// THEN: EKF should stay or converge to zero
for(int i=0; i<3; ++i)
{
EXPECT_NEAR(0.0f,converged_pos[i],0.001f);
EXPECT_NEAR(0.0f,converged_vel[i],0.001f);
EXPECT_NEAR(0.0f,converged_accel_bias[i],0.001f);
EXPECT_NEAR(0.0f,converged_gyro_bias[i],0.001f);
}
EXPECT_TRUE(matrix::isEqual(pos, ref, 0.001f));
EXPECT_TRUE(matrix::isEqual(vel, ref, 0.001f));
EXPECT_TRUE(matrix::isEqual(accel_bias, ref, 0.001f));
EXPECT_TRUE(matrix::isEqual(gyro_bias, ref, 0.001f));
}
TEST_F(EkfInitializationTest, gpsFusion)
@@ -135,7 +132,7 @@ TEST_F(EkfInitializationTest, gpsFusion)
// WHEN: setting GPS measurements for 11s, minimum GPS health time is set to 10 sec
_sensor_simulator.startGps();
_sensor_simulator.run_seconds(11);
_sensor_simulator.runSeconds(11);
// THEN: EKF should fuse GPS, but no other position sensor
filter_control_status_u control_status;
@@ -171,29 +168,23 @@ TEST_F(EkfInitializationTest, accleBiasEstimation)
{
// GIVEN: initialized EKF with default IMU, baro and mag input for 2s
// WHEN: Added more sensor measurements with accel bias and gps measurements
Vector3f accel_bias = {0.0f,0.0f,0.1f};
Vector3f accel_bias_sim = {0.0f,0.0f,0.1f};
_sensor_simulator.startGps();
_sensor_simulator.setImuBias(accel_bias, Vector3f{0.0f,0.0f,0.0f});
_sensor_simulator.run_seconds(10);
_sensor_simulator.setImuBias(accel_bias_sim, Vector3f{0.0f,0.0f,0.0f});
_sensor_simulator.runSeconds(10);
float converged_pos[3];
float converged_vel[3];
float converged_accel_bias[3];
float converged_gyro_bias[3];
_ekf->get_position(converged_pos);
_ekf->get_velocity(converged_vel);
_ekf->get_accel_bias(converged_accel_bias);
_ekf->get_gyro_bias(converged_gyro_bias);
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};
// THEN: EKF should estimate bias correctelly
for(int i=0; i<3; ++i)
{
EXPECT_NEAR(0.0f,converged_pos[i],0.001f) << "i: " << i;
EXPECT_NEAR(0.0f,converged_vel[i],0.001f) << "i: " << i;
EXPECT_NEAR(accel_bias(i),converged_accel_bias[i],0.001f) << "i: " << i;
EXPECT_NEAR(0.0f,converged_gyro_bias[i],0.001f) << "i: " << i;
}
// THEN: EKF should stay or converge to zero
EXPECT_TRUE(matrix::isEqual(pos, zero, 0.001f));
EXPECT_TRUE(matrix::isEqual(vel, zero, 0.001f));
EXPECT_TRUE(matrix::isEqual(accel_bias, accel_bias_sim, 0.001f));
EXPECT_TRUE(matrix::isEqual(gyro_bias, zero, 0.001f));
}
// TODO: Add sampling tests