fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 04:33: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/sensor_simulator/ekf_logger.cpp
+1 -1
View File
@@ -58,7 +58,7 @@ void EkfLogger::writeState()
_file << time;
if(_state_logging_enabled)
{
matrix::Vector<float, 24> state = _ekf_wrapper.getState();
matrix::Vector<float, 24> state = _ekf->getStateAtFusionHorizonAsVector();
for(int i = 0; i < 24; i++)
{
_file << "," << state(i);
test/sensor_simulator/ekf_wrapper.cpp
+1 -63
View File
@@ -160,74 +160,12 @@ bool EkfWrapper::isWindVelocityEstimated() const
return control_status.flags.wind;
}
Vector3f EkfWrapper::getPosition() const
{
float temp[3];
_ekf->get_position(temp);
return Vector3f(temp);
}
Vector3f EkfWrapper::getVelocity() const
{
float temp[3];
_ekf->get_velocity(temp);
return Vector3f(temp);
}
Vector3f EkfWrapper::getAccelBias() const
{
float temp[3];
_ekf->get_accel_bias(temp);
return Vector3f(temp);
}
Vector3f EkfWrapper::getGyroBias() const
{
float temp[3];
_ekf->get_gyro_bias(temp);
return Vector3f(temp);
}
Quatf EkfWrapper::getQuaternion() const
{
return _ekf->get_quaternion();
}
Eulerf EkfWrapper::getEulerAngles() const
{
return Eulerf(getQuaternion());
}
matrix::Vector<float, 24> EkfWrapper::getState() const
{
float state[24];
_ekf->get_state_delayed(state);
return matrix::Vector<float, 24>{state};
return Eulerf(_ekf->getQuaternion());
}
matrix::Vector<float, 4> EkfWrapper::getQuaternionVariance() const
{
return matrix::Vector<float, 4>(_ekf->orientation_covariances().diag());
}
Vector3f EkfWrapper::getPositionVariance() const
{
return Vector3f(_ekf->position_covariances().diag());
}
Vector3f EkfWrapper::getVelocityVariance() const
{
return Vector3f(_ekf->velocity_covariances().diag());
}
Vector2f EkfWrapper::getWindVelocity() const
{
float temp[2];
_ekf->get_wind_velocity(temp);
return Vector2f(temp);
}
Quatf EkfWrapper::getVisionAlignmentQuaternion() const
{
float temp[4];
_ekf->get_ev2ekf_quaternion(temp);
return Quatf(temp);
}
test/sensor_simulator/ekf_wrapper.h
-11
View File
@@ -85,19 +85,8 @@ public:
bool isWindVelocityEstimated() const;
Vector3f getPosition() const;
Vector3f getVelocity() const;
Vector3f getAccelBias() const;
Vector3f getGyroBias() const;
Quatf getQuaternion() const;
Eulerf getEulerAngles() const;
matrix::Vector<float, 24> getState() const;
matrix::Vector<float, 4> getQuaternionVariance() const;
Vector3f getPositionVariance() const;
Vector3f getVelocityVariance() const;
Vector2f getWindVelocity() const;
Quatf getVisionAlignmentQuaternion() const;
private:
std::shared_ptr<Ekf> _ekf;
test/test_EKF_airspeed.cpp
+4 -4
View File
@@ -88,9 +88,9 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
const Vector3f vel = _ekf_wrapper.getVelocity();
const Vector2f vel_wind_earth = _ekf_wrapper.getWindVelocity();
const float height_before_pressure_correction = _ekf_wrapper.getPosition()(2);
const Vector3f vel = _ekf->getVelocity();
const Vector2f vel_wind_earth = _ekf->getWindVelocity();
const float height_before_pressure_correction = _ekf->getPosition()(2);
const Dcmf R_to_earth_sim(_quat_sim);
EXPECT_TRUE(matrix::isEqual(vel, simulated_velocity_earth));
@@ -108,7 +108,7 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
_sensor_simulator.runSeconds(20);
const float height_after_pressure_correction = _ekf_wrapper.getPosition()(2);
const float height_after_pressure_correction = _ekf->getPosition()(2);
// height increase means that state z decrease due to z axis pointing down
const float expected_height_after_pressure_correction = height_before_pressure_correction -
expected_height_difference;
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));
test/test_EKF_externalVision.cpp
+5 -5
View File
@@ -103,7 +103,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
TEST_F(EkfExternalVisionTest, visionVarianceCheck)
{
const Vector3f velVar_init = _ekf_wrapper.getVelocityVariance();
const Vector3f velVar_init = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
_sensor_simulator._vio.setVelocityVariance(Vector3f{2.0f,0.01f,0.01f});
@@ -111,7 +111,7 @@ TEST_F(EkfExternalVisionTest, visionVarianceCheck)
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(4);
const Vector3f velVar_new = _ekf_wrapper.getVelocityVariance();
const Vector3f velVar_new = _ekf->getVelocityVariance();
EXPECT_TRUE(velVar_new(0) > velVar_new(1));
}
@@ -131,17 +131,17 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.startExternalVision();
const Vector3f velVar_init = _ekf_wrapper.getVelocityVariance();
const Vector3f velVar_init = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
_sensor_simulator.runSeconds(4);
// THEN: velocity uncertainty in y should be bigger
const Vector3f velVar_new = _ekf_wrapper.getVelocityVariance();
const Vector3f velVar_new = _ekf->getVelocityVariance();
EXPECT_TRUE(velVar_new(1) > velVar_new(0));
// THEN: the frame offset should be estimated correctly
Quatf estimatedExternalVisionFrameOffset = _ekf_wrapper.getVisionAlignmentQuaternion();
Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
estimatedExternalVisionFrameOffset.canonical()));
}
test/test_EKF_fusionLogic.cpp
+9 -9
View File
@@ -136,25 +136,25 @@ TEST_F(EkfFusionLogicTest, rejectGpsSignalJump)
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
// WHEN: Having a big horizontal position Gps jump coming from the Gps Receiver
Vector3f pos_old = _ekf_wrapper.getPosition();
Vector3f vel_old = _ekf_wrapper.getVelocity();
Vector3f accel_bias_old = _ekf_wrapper.getAccelBias();
const Vector3f pos_old = _ekf->getPosition();
const Vector3f vel_old = _ekf->getVelocity();
const Vector3f accel_bias_old = _ekf->getAccelBias();
_sensor_simulator._gps.stepHorizontalPositionByMeters(Vector2f{10.0f, 0.0f});
_sensor_simulator.runSeconds(2);
// THEN: The estimate should not change much in the short run
// and GPS fusion should be stopped after a while.
Vector3f pos_new = _ekf_wrapper.getPosition();
Vector3f vel_new = _ekf_wrapper.getVelocity();
Vector3f accel_bias_new = _ekf_wrapper.getAccelBias();
Vector3f pos_new = _ekf->getPosition();
Vector3f vel_new = _ekf->getVelocity();
Vector3f accel_bias_new = _ekf->getAccelBias();
EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f));
EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f));
EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f));
_sensor_simulator.runSeconds(10);
pos_new = _ekf_wrapper.getPosition();
vel_new = _ekf_wrapper.getVelocity();
accel_bias_new = _ekf_wrapper.getAccelBias();
pos_new = _ekf->getPosition();
vel_new = _ekf->getVelocity();
accel_bias_new = _ekf->getAccelBias();
EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f));
EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f));
EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f));
test/test_EKF_initialization.cpp
+5 -5
View File
@@ -64,7 +64,7 @@ class EkfInitializationTest : public ::testing::Test {
void initializedOrienationIsMatchingGroundTruth(Quatf true_quaternion)
{
Quatf quat_est = _ekf_wrapper.getQuaternion();
const Quatf quat_est = _ekf->getQuaternion();
EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion));
}
@@ -86,8 +86,8 @@ class EkfInitializationTest : public ::testing::Test {
void velocityAndPositionCloseToZero()
{
Vector3f pos = _ekf_wrapper.getPosition();
Vector3f vel = _ekf_wrapper.getVelocity();
const Vector3f pos = _ekf->getPosition();
const Vector3f vel = _ekf->getVelocity();
EXPECT_TRUE(matrix::isEqual(pos, Vector3f{}, 0.001f));
EXPECT_TRUE(matrix::isEqual(vel, Vector3f{}, 0.001f));
@@ -95,8 +95,8 @@ class EkfInitializationTest : public ::testing::Test {
void velocityAndPositionVarianceBigEnoughAfterOrientationInitialization()
{
Vector3f pos_var = _ekf_wrapper.getPositionVariance();
Vector3f vel_var = _ekf_wrapper.getVelocityVariance();
const Vector3f pos_var = _ekf->getPositionVariance();
const Vector3f vel_var = _ekf->getVelocityVariance();
const float pos_variance_limit = 0.2f;
EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(1)" << pos_var(0);