fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 18:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

HTE: fix variance prediction (#16016)

Browse Source 
In the predition step, the process variance was erroneously
multiplied by dt instead of dt^2. The default values are adjusted
accordingly to keep the same tuning for the default loop rate of 50Hz
This commit is contained in:
Mathieu Bresciani 2020-10-27 16:56:32 +01:00 committed by GitHub
parent f36f8928e3
commit 050c9dcd3d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c
View File

@ -52,7 +52,7 @@
* @unit normalized_thrust/s
* @group Hover Thrust Estimator
*/
PARAM_DEFINE_FLOAT(HTE_HT_NOISE, 0.0005);
PARAM_DEFINE_FLOAT(HTE_HT_NOISE, 0.0036);
/**
* Gate size for acceleration fusion

4
src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp
View File

@ -45,7 +45,7 @@ void ZeroOrderHoverThrustEkf::predict(const float dt)
{
// State is constant
// Predict state covariance only
_state_var += _process_var * dt;
_state_var += _process_var * dt * dt;
_dt = dt;
}
@ -133,7 +133,7 @@ inline bool ZeroOrderHoverThrustEkf::isLargeOffsetDetected() const
inline void ZeroOrderHoverThrustEkf::bumpStateVariance()
{
_state_var += 1e3f * _process_var * _dt;
_state_var += 1e3f * _process_var * _dt * _dt;
}
inline void ZeroOrderHoverThrustEkf::updateLpf(const float residual, const float signed_innov_test_ratio)

4
src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp
View File

@ -84,7 +84,7 @@ public:
void resetAccelNoise() { _acc_var = 5.f; };
void predict(float _dt);
void predict(float dt);
void fuseAccZ(float acc_z, float thrust, status &status_return);
void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); }
@ -102,7 +102,7 @@ private:
float _gate_size{3.f};
float _state_var{0.01f}; ///< Initial hover thrust uncertainty variance (thrust^2)
float _process_var{0.25e-6f}; ///< Hover thrust process noise variance (thrust^2/s^2)
float _process_var{12.5e-6f}; ///< Hover thrust process noise variance (thrust^2/s^2)
float _acc_var{5.f}; ///< Acceleration variance (m^2/s^3)
float _dt{0.02f};