fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 00:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

WindEstimator: remove option to disable scale estimation

Browse Source 
The situation where this would be desired is unclear, plus it's basically
the same as setting ASPD_SC_P_NOISE to a very small value.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-10-22 17:00:35 +02:00
parent b38bf23d6e
commit 73fe300c00
6 changed files with 8 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/wind_estimator/WindEstimator.cpp
+1 -9
View File
@@ -116,7 +116,7 @@ WindEstimator::update(uint64_t time_now)
_time_last_update = time_now;
float q_w = _wind_p_var;
float q_k_tas = _disable_tas_scale_estimate ? 0.f : _tas_scale_p_var;
float q_k_tas = _tas_scale_p_var;
float SPP0 = dt * dt;
float SPP1 = SPP0 * q_w;
@@ -180,10 +180,6 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
K /= S(0, 0);
if (_disable_tas_scale_estimate) {
K(2, 0) = 0.f;
}
// compute innovation
const float airspeed_pred = _state(INDEX_TAS_SCALE) * airspeed_predicted_raw;
_tas_innov = true_airspeed - airspeed_pred;
@@ -270,10 +266,6 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
matrix::Matrix<float, 3, 1> K = _P * H_beta.transpose();
K /= S(0, 0);
if (_disable_tas_scale_estimate) {
K(2, 0) = 0.f;
}
// compute predicted side slip angle
matrix::Vector3f rel_wind(velI(0) - _state(INDEX_W_N), velI(1) - _state(INDEX_W_E), velI(2));
matrix::Dcmf R_body_to_earth(q_att);
src/lib/wind_estimator/WindEstimator.hpp
-3
View File
@@ -85,7 +85,6 @@ public:
void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; }
void set_scale_init(float scale_init) {_scale_init = 1.f / math::constrain(scale_init, 0.1f, 10.f); }
void set_disable_tas_scale_estimate(bool disable_tas_scale_estimate) {_disable_tas_scale_estimate = disable_tas_scale_estimate; }
private:
enum {
@@ -123,8 +122,6 @@ private:
bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
bool _disable_tas_scale_estimate{false};
// initialise state and state covariance matrix
bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas);