diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 7f29df5f16..9e9da09fb9 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -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 ×tamp_sample, - const ZeroOrderHoverThrustEkf::status &status) +void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime ×tamp_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; diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index cad76b33f9..a66c289df1 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -88,7 +88,7 @@ private: void reset(); - void publishStatus(const hrt_abstime ×tamp_sample, const ZeroOrderHoverThrustEkf::status &status); + void publishStatus(const hrt_abstime ×tamp_sample); void publishInvalidStatus(); ZeroOrderHoverThrustEkf _hover_thrust_ekf{}; diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp index 435789fb86..94c13cad7b 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp +++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp @@ -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; -} diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp index b7e7efb33e..51672d0cff 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp +++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp @@ -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 }; diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp index 34d2cea07e..69e6549ab5 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp +++ b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp @@ -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;