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

HTE: pull status from class instead of returning struct

Browse Source
This commit is contained in:
bresch
2020-12-16 11:08:41 +01:00
committed by Julian Kent
parent 652cc4350e
commit 39251daf28
5 changed files with 55 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+11 -12
View File
@@ -164,14 +164,14 @@ void MulticopterHoverThrustEstimator::Run()
if (PX4_ISFINITE(local_pos_sp.thrust[2])) {
// Inform the hover thrust estimator about the measured vertical
// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
ZeroOrderHoverThrustEkf::status status{};
_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status);
_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
const bool valid = (status.hover_thrust_var < 0.001f) && (status.innov_test_ratio < 1.f);
const bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f)
&& (_hover_thrust_ekf.getInnovationTestRatio() < 1.f);
_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
_valid = _valid_hysteresis.get_state();
publishStatus(local_pos.timestamp, status);
publishStatus(local_pos.timestamp);
}
}
@@ -193,20 +193,19 @@ void MulticopterHoverThrustEstimator::Run()
perf_end(_cycle_perf);
}
void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime &timestamp_sample,
const ZeroOrderHoverThrustEkf::status &status)
void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime &timestamp_sample)
{
hover_thrust_estimate_s status_msg{};
status_msg.timestamp_sample = timestamp_sample;
status_msg.hover_thrust = status.hover_thrust;
status_msg.hover_thrust_var = status.hover_thrust_var;
status_msg.hover_thrust = _hover_thrust_ekf.getHoverThrustEstimate();
status_msg.hover_thrust_var = _hover_thrust_ekf.getHoverThrustEstimateVar();
status_msg.accel_innov = status.innov;
status_msg.accel_innov_var = status.innov_var;
status_msg.accel_innov_test_ratio = status.innov_test_ratio;
status_msg.accel_noise_var = status.accel_noise_var;
status_msg.accel_innov = _hover_thrust_ekf.getInnovation();
status_msg.accel_innov_var = _hover_thrust_ekf.getInnovationVar();
status_msg.accel_innov_test_ratio = _hover_thrust_ekf.getInnovationTestRatio();
status_msg.accel_noise_var = _hover_thrust_ekf.getAccelNoiseVar();
status_msg.valid = _valid;
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp
+1 -1
View File
@@ -88,7 +88,7 @@ private:
void reset();
void publishStatus(const hrt_abstime &timestamp_sample, const ZeroOrderHoverThrustEkf::status &status);
void publishStatus(const hrt_abstime &timestamp_sample);
void publishInvalidStatus();
ZeroOrderHoverThrustEkf _hover_thrust_ekf{};
src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp
+5 -17
View File
@@ -49,7 +49,7 @@ void ZeroOrderHoverThrustEkf::predict(const float dt)
_dt = dt;
}
void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust, status &status_return)
void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust)
{
const float H = computeH(thrust);
const float innov_var = computeInnovVar(H);
@@ -75,7 +75,10 @@ void ZeroOrderHoverThrustEkf::fuseAccZ(const float acc_z, const float thrust, st
updateLpf(residual, signed_innov_test_ratio);
updateMeasurementNoise(residual, H);
status_return = packStatus(innov, innov_var, innov_test_ratio);
// save for logging
_innov = innov;
_innov_var = innov_var;
_innov_test_ratio = innov_test_ratio;
}
inline float ZeroOrderHoverThrustEkf::computeH(const float thrust) const
@@ -151,18 +154,3 @@ inline void ZeroOrderHoverThrustEkf::updateMeasurementNoise(const float residual
const float P = _state_var;
_acc_var = math::constrain((1.f - alpha) * _acc_var + alpha * (res_no_bias * res_no_bias + H * P * H), 1.f, 400.f);
}
inline ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkf::packStatus(const float innov, const float innov_var,
const float innov_test_ratio) const
{
// Send back status for logging
status ret{};
ret.hover_thrust = _hover_thr;
ret.hover_thrust_var = _state_var;
ret.innov = innov;
ret.innov_var = innov_var;
ret.innov_test_ratio = innov_test_ratio;
ret.accel_noise_var = _acc_var;
return ret;
}
src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp
+9 -13
View File
@@ -70,22 +70,13 @@
class ZeroOrderHoverThrustEkf
{
public:
struct status {
float hover_thrust;
float hover_thrust_var;
float innov;
float innov_var;
float innov_test_ratio;
float accel_noise_var;
};
ZeroOrderHoverThrustEkf() = default;
~ZeroOrderHoverThrustEkf() = default;
void resetAccelNoise() { _acc_var = 5.f; };
void predict(float dt);
void fuseAccZ(float acc_z, float thrust, status &status_return);
void predict(float _dt);
void fuseAccZ(float acc_z, float thrust);
void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); }
void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; }
@@ -95,6 +86,9 @@ public:
float getHoverThrustEstimate() const { return _hover_thr; }
float getHoverThrustEstimateVar() const { return _state_var; }
float getInnovation() const { return _innov; }
float getInnovationVar() const { return _innov_var; }
float getInnovationTestRatio() const { return _innov_test_ratio; }
float getAccelNoiseVar() const { return _acc_var; }
private:
@@ -106,6 +100,10 @@ private:
float _acc_var{5.f}; ///< Acceleration variance (m^2/s^3)
float _dt{0.02f};
float _innov{0.f}; ///< Measurement innovation (m/s^2)
float _innov_var{0.f}; ///< Measurement innovation variance (m^2/s^3)
float _innov_test_ratio{0.f}; ///< Noramlized Innovation Squared test ratio
float _residual_lpf{}; ///< used to remove the constant bias of the residual
float _signed_innov_test_ratio_lpf{}; ///< used as a delay to trigger the recovery logic
@@ -131,8 +129,6 @@ private:
void updateLpf(float residual, float signed_innov_test_ratio);
void updateMeasurementNoise(float residual, float H);
status packStatus(float innov, float innov_var, float innov_test_ratio) const;
static constexpr float _noise_learning_time_constant = 2.f; ///< in seconds
static constexpr float _lpf_time_constant = 1.f; ///< in seconds
};
src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp
+29 -11
View File
@@ -47,13 +47,22 @@ using namespace matrix;
class ZeroOrderHoverThrustEkfTest : public ::testing::Test
{
public:
struct Status {
float hover_thrust;
float hover_thrust_var;
float innov;
float innov_var;
float innov_test_ratio;
float accel_noise_var;
};
ZeroOrderHoverThrustEkfTest()
{
_random_generator.seed(42);
}
float computeAccelFromThrustAndHoverThrust(float thrust, float hover_thrust);
ZeroOrderHoverThrustEkf::status runEkf(float hover_thrust_true, float thrust, float time, float accel_noise = 0.f,
float thr_noise = 0.f);
Status runEkf(float hover_thrust_true, float thrust, float time, float accel_noise = 0.f,
float thr_noise = 0.f);
private:
ZeroOrderHoverThrustEkf _ekf{};
@@ -71,19 +80,28 @@ float ZeroOrderHoverThrustEkfTest::computeAccelFromThrustAndHoverThrust(float th
return CONSTANTS_ONE_G * thrust / hover_thrust - CONSTANTS_ONE_G;
}
ZeroOrderHoverThrustEkf::status ZeroOrderHoverThrustEkfTest::runEkf(float hover_thrust_true, float thrust, float time,
ZeroOrderHoverThrustEkfTest::Status ZeroOrderHoverThrustEkfTest::runEkf(float hover_thrust_true, float thrust,
float time,
float accel_noise, float thr_noise)
{
ZeroOrderHoverThrustEkf::status status{};
Status status{};
for (float t = 0.f; t <= time; t += _dt) {
_ekf.predict(_dt);
float noisy_thrust = thrust + thr_noise * _standard_normal_distribution(_random_generator);
float accel_theory = computeAccelFromThrustAndHoverThrust(thrust, hover_thrust_true);
float noisy_accel = accel_theory + accel_noise * _standard_normal_distribution(_random_generator);
_ekf.fuseAccZ(noisy_accel, noisy_thrust, status);
_ekf.fuseAccZ(noisy_accel, noisy_thrust);
}
status.hover_thrust = _ekf.getHoverThrustEstimate();
status.hover_thrust_var = _ekf.getHoverThrustEstimateVar();
status.innov = _ekf.getInnovation();
status.innov_var = _ekf.getInnovationVar();
status.innov_test_ratio = _ekf.getInnovationTestRatio();
status.accel_noise_var = _ekf.getAccelNoiseVar();
return status;
}
@@ -94,7 +112,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticCase)
const float hover_thrust_true = 0.5f;
// WHEN: we input noiseless data and run the filter
ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 2.f);
ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, 2.f);
// THEN: The estimate should not move and its variance decrease quickly
EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-4f);
@@ -110,7 +128,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergence)
const float hover_thrust_true = 0.72f;
// WHEN: we input noiseless data and run the filter
ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, 2.f);
ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, 2.f);
// THEN: the state should converge to the true value and its variance decrease
EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 1e-2f);
@@ -129,7 +147,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testStaticConvergenceWithNoise)
const float t_sim = 10.f;
// WHEN: we input noisy accel data and run the filter
ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f);
@@ -147,7 +165,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testLargeAccelNoiseAndBias)
const float t_sim = 15.f;
// WHEN: we input noisy accel data and run the filter
ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, sigma_noise);
// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 7e-2);
@@ -167,7 +185,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testThrustAndAccelNoise)
const float t_sim = 15.f;
// WHEN: we input noisy accel and thrust data, and run the filter
ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
// THEN: the estimate should converge and the accel noise variance should be close to the true noise value
EXPECT_NEAR(status.hover_thrust, hover_thrust_true, 5e-2f);
@@ -188,7 +206,7 @@ TEST_F(ZeroOrderHoverThrustEkfTest, testHoverThrustJump)
float t_sim = 10.f;
// WHEN: we input noisy accel and thrust data, and run the filter
ZeroOrderHoverThrustEkf::status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
ZeroOrderHoverThrustEkfTest::Status status = runEkf(hover_thrust_true, thrust, t_sim, accel_noise, thr_noise);
// THEN: change the hover thrust and the current thrust (the velocity controller responds quickly)
// Note that this is an extreme jump in hover thrust
thrust = 0.3;