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

AirspeedSelector: airspeed scale estimation improvements and robustification

Browse Source 
- run airspeed scale estimation always, not in dedicated mode
- add option to apply scale automatically, with extra feasibility check
- add airspeed scale for all 3 possible airspeed instances
- clean up parameters
- add check for data stuck (non-changing airspeed data)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-10-22 17:00:35 +02:00
parent 8e8c6efd66
commit 625f556b0e
7 changed files with 286 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/wind_estimator/WindEstimator.cpp
+24 -26
View File
@@ -56,7 +56,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &
// initilaise wind states assuming zero side slip and horizontal flight
_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est);
_state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_est);
_state(INDEX_TAS_SCALE) = 1.0f;
_state(INDEX_TAS_SCALE) = _scale_init;
// compute jacobian of states wrt north/each earth velocity states and true airspeed measurement
float L0 = v_e * v_e;
@@ -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 = _tas_scale_p_var;
float q_k_tas = _disable_tas_scale_estimate ? 0.f : _tas_scale_p_var;
float SPP0 = dt * dt;
float SPP1 = SPP0 * q_w;
@@ -160,28 +160,32 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
const float v_e = velI(1);
const float v_d = velI(2);
const float k_tas = _state(INDEX_TAS_SCALE);
// calculate airspeed from ground speed and wind states (without scale)
const float airspeed_predicted_raw = sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) +
(v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + v_d * v_d);
// compute kalman gain K
const float HH0 = sqrtf(v_d * v_d + (v_e - _state(INDEX_W_E)) * (v_e - _state(INDEX_W_E)) + (v_n - _state(
INDEX_W_N)) * (v_n - _state(INDEX_W_N)));
const float HH1 = k_tas / HH0;
// compute state observation matrix H
const float HH0 = airspeed_predicted_raw;
const float HH1 = _state(INDEX_TAS_SCALE) / HH0;
matrix::Matrix<float, 1, 3> H_tas;
H_tas(0, 0) = HH1 * (-v_n + _state(INDEX_W_N));
H_tas(0, 1) = HH1 * (-v_e + _state(INDEX_W_E));
H_tas(0, 2) = HH0;
matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
// compute innovation covariance S
const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
// compute Kalman gain
matrix::Matrix<float, 3, 1> K = _P * H_tas.transpose();
K /= S(0, 0);
// compute innovation
const float airspeed_pred = k_tas * sqrtf((v_n - _state(INDEX_W_N)) * (v_n - _state(INDEX_W_N)) + (v_e - _state(
INDEX_W_E)) *
(v_e - _state(INDEX_W_E)) + v_d * v_d);
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;
// innovation variance
@@ -259,13 +263,17 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5;
H_beta(0, 2) = 0;
// compute kalman gain
matrix::Matrix<float, 3, 1> K = _P * H_beta.transpose();
// compute innovation covariance S
const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;
// compute Kalman gain
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);
@@ -333,16 +341,6 @@ WindEstimator::run_sanity_checks()
return;
}
// check if we should inhibit learning of airspeed scale factor and rather use a pre-set value.
// airspeed scale factor errors arise from sensor installation which does not change and only needs
// to be computed once for a perticular installation.
if (_enforced_airspeed_scale < 0) {
_state(INDEX_TAS_SCALE) = math::max(0.0f, _state(INDEX_TAS_SCALE));
} else {
_state(INDEX_TAS_SCALE) = _enforced_airspeed_scale;
}
// attain symmetry
for (unsigned row = 0; row < 3; row++) {
for (unsigned column = 0; column < row; column++) {
src/lib/wind_estimator/WindEstimator.hpp
+14 -11
View File
@@ -70,7 +70,9 @@ public:
bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
uint64_t &time_meas_rejected, bool &reinit_filter);
float get_tas_scale() { return _state(INDEX_TAS_SCALE); }
// invert scale (CAS = IAS * scale), protect agains division by 0, constrain to [0.1, 10]
float get_tas_scale() { return math::constrain(1.f / (_state(INDEX_TAS_SCALE) + 0.0001f), 0.1f, 10.f); }
float get_tas_scale_var() { return _P(2, 2); }
float get_tas_innov() { return _tas_innov; }
float get_tas_innov_var() { return _tas_innov_var; }
float get_beta_innov() { return _beta_innov; }
@@ -88,10 +90,8 @@ public:
void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
void set_beta_gate(uint8_t gate_size) {_beta_gate = gate_size; }
// Inhibit learning of the airspeed scale factor and force the estimator to use _enforced_airspeed_scale as scale factor.
// Negative input values enable learning of the airspeed scale factor.
void enforce_airspeed_scale(float scale) {_enforced_airspeed_scale = scale; }
void set_scale_init(float scale_init) {_scale_init = math::constrain(1.f / (scale_init + 0.0001f), 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 {
@@ -100,7 +100,7 @@ private:
INDEX_TAS_SCALE
}; ///< enum which can be used to access state.
matrix::Vector3f _state; ///< state vector
matrix::Vector3f _state{0.f, 0.f, 1.f};
matrix::Matrix3f _P; ///< state covariance matrix
float _tas_innov{0.0f}; ///< true airspeed innovation
@@ -118,15 +118,18 @@ private:
uint8_t _tas_gate{3}; ///< airspeed fusion gate size
uint8_t _beta_gate{1}; ///< sideslip fusion gate size
float _scale_init{1.f};
uint64_t _time_last_airspeed_fuse = 0; ///< timestamp of last airspeed fusion
uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion
uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction
uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected
uint64_t _time_last_beta_fuse = 0; ///< timestamp of last sideslip fusion
uint64_t _time_last_update = 0; ///< timestamp of last covariance prediction
uint64_t _time_rejected_beta = 0; ///< timestamp of when sideslip measurements have consistently started to be rejected
uint64_t _time_rejected_tas =
0; ///<timestamp of when true airspeed measurements have consistently started to be rejected
0; ///< timestamp of when true airspeed measurements have consistently started to be rejected
bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
float _enforced_airspeed_scale{-1.0f}; ///< by default we want to estimate the true airspeed scale factor (see enforce_airspeed_scale(...) )
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);