Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

WindEstimator: reworked filter initialisation

Browse Source 
- separate initialisation with and without airspeed

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2022-07-25 15:45:13 +03:00
committed by Silvan Fuhrer
parent a63f1b71fe
commit fbd4534edc
4 changed files with 54 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/wind_estimator/WindEstimator.cpp
+40 -38
View File
@@ -39,8 +39,8 @@
#include "WindEstimator.hpp" #include "WindEstimator.hpp"
bool bool
WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas, WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
const matrix::Quatf &q_att) const float tas_meas, const float tas_variance)
{ {
// do no initialise if ground velocity is low // do no initialise if ground velocity is low
// this should prevent the filter from initialising on the ground // this should prevent the filter from initialising on the ground
@@ -48,41 +48,43 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &
return false; return false;
} }
const float v_n = velI(0); if (PX4_ISFINITE(tas_meas) && PX4_ISFINITE(tas_variance)) {
const float v_e = velI(1); // initialise wind states assuming zero side slip and horizontal flight
_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_rad);
_state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_rad);
_state(INDEX_TAS_SCALE) = _scale_init;
const float heading_est = matrix::Eulerf(q_att).psi(); constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
const float initial_wind_var_body_y = sq(tas_meas * sinf(initial_sideslip_uncertainty));
constexpr float heading_variance = sq(math::radians(10.0f));
// initilaise wind states assuming zero side slip and horizontal flight const float cos_heading = cosf(heading_rad);
_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est); const float sin_heading = sinf(heading_rad);
_state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_est);
_state(INDEX_TAS_SCALE) = _scale_init;
// compute jacobian of states wrt north/each earth velocity states and true airspeed measurement // rotate wind velocity into earth frame aligned with vehicle yaw
float L0 = v_e * v_e; const float Wx = _state(INDEX_W_N) * cos_heading + _state(INDEX_W_E) * sin_heading;
float L1 = v_n * v_n; const float Wy = -_state(INDEX_W_N) * sin_heading + _state(INDEX_W_E) * cos_heading;
float L2 = L0 + L1;
float L3 = tas_meas / (L2 * sqrtf(L2));
float L4 = L3 * v_e * v_n + 1.0f;
float L5 = 1.0f / sqrtf(L2);
float L6 = -L5 * tas_meas;
matrix::Matrix3f L; _P(INDEX_W_N, INDEX_W_N) = tas_variance * sq(cos_heading) + heading_variance * sq(-Wx * sin_heading - Wy * cos_heading)
L.setZero(); + initial_wind_var_body_y * sq(sin_heading);
L(0, 0) = L4; _P(INDEX_W_N, INDEX_W_E) = tas_variance * sin_heading * cos_heading + heading_variance *
L(0, 1) = L0 * L3 + L6; (-Wx * sin_heading - Wy * cos_heading) * (Wx * cos_heading - Wy * sin_heading) -
L(1, 0) = L1 * L3 + L6; initial_wind_var_body_y * sin_heading * cos_heading;
L(1, 1) = L4; _P(INDEX_W_E, INDEX_W_N) = _P(INDEX_W_N, INDEX_W_E);
L(2, 2) = 1.0f; _P(INDEX_W_E, INDEX_W_E) = tas_variance * sq(sin_heading) + heading_variance * sq(Wx * cos_heading - Wy * sin_heading) +
initial_wind_var_body_y * sq(cos_heading);
// get an estimate of the state covariance matrix given the estimated variance of ground velocity // Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
// and measured airspeed _P(INDEX_W_N, INDEX_W_N) += hor_vel_variance;
_P.setZero(); _P(INDEX_W_E, INDEX_W_E) += hor_vel_variance;
_P(INDEX_W_N, INDEX_W_N) = INITIAL_WIND_VAR;
_P(INDEX_W_E, INDEX_W_E) = INITIAL_WIND_VAR;
_P(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = 0.0001f;
_P = L * _P * L.transpose(); } else {
// no airspeed available
_state.setZero();
_state(INDEX_TAS_SCALE) = 1.0f;
_P.setZero();
_P(INDEX_W_N, INDEX_W_N) = _P(INDEX_W_E, INDEX_W_E) = INITIAL_WIND_VAR;
}
// reset the timestamp for measurement rejection // reset the timestamp for measurement rejection
_time_rejected_tas = 0; _time_rejected_tas = 0;
@@ -124,13 +126,12 @@ WindEstimator::update(uint64_t time_now)
void void
WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Vector3f &velI, WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Vector3f &velI,
const matrix::Vector2f &velIvar, const matrix::Quatf &q_att) const float hor_vel_variance, const matrix::Quatf &q_att)
{ {
matrix::Vector2f velIvar_constrained = { math::max(0.01f, velIvar(0)), math::max(0.01f, velIvar(1)) };
if (!_initialised) { if (!_initialised) {
// try to initialise // try to initialise
_initialised = initialise(velI, velIvar_constrained, true_airspeed, q_att); _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
return; return;
} }
@@ -157,7 +158,7 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
if (meas_is_rejected || _tas_innov_var < 0.f) { if (meas_is_rejected || _tas_innov_var < 0.f) {
// only reset filter if _tas_innov_var gets unfeasible // only reset filter if _tas_innov_var gets unfeasible
if (_tas_innov_var < 0.0f) { if (_tas_innov_var < 0.0f) {
_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed, q_att); _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
} }
// we either did a filter reset or the current measurement was rejected so do not fuse // we either did a filter reset or the current measurement was rejected so do not fuse
@@ -176,10 +177,11 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
} }
void void
WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att) WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const float hor_vel_variance,
const matrix::Quatf &q_att)
{ {
if (!_initialised) { if (!_initialised) {
_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att); _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
return; return;
} }
@@ -251,7 +253,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
if (meas_is_rejected || reinit_filter) { if (meas_is_rejected || reinit_filter) {
if (reinit_filter) { if (reinit_filter) {
_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att); _initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
} }
// we either did a filter reset or the current measurement was rejected so do not fuse // we either did a filter reset or the current measurement was rejected so do not fuse
src/lib/wind_estimator/WindEstimator.hpp
+8 -4
View File
@@ -60,8 +60,9 @@ public:
void update(uint64_t time_now); void update(uint64_t time_now);
void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI, void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI,
const matrix::Vector2f &velIvar, const matrix::Quatf &q_att); const float hor_vel_variance, const matrix::Quatf &q_att);
void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att); void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const float hor_vel_variance,
const matrix::Quatf &q_att);
bool is_estimate_valid() { return _initialised; } bool is_estimate_valid() { return _initialised; }
@@ -129,8 +130,11 @@ private:
bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
// initialise state and state covariance matrix // initialise state and state covariance matrix
bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas, bool initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
const matrix::Quatf &q_att); const float tas_meas = NAN, const float tas_variance = NAN);
void run_sanity_checks(); void run_sanity_checks();
// return the square of two floating point numbers
static constexpr float sq(float var) { return var * var; }
}; };
src/modules/airspeed_selector/AirspeedValidator.cpp
+3 -3
View File
@@ -81,11 +81,11 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
Quatf q(att_q); Quatf q(att_q);
// airspeed fusion (with raw TAS) // airspeed fusion (with raw TAS)
const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}}; const float hor_vel_variance = lpos_evh * lpos_evh;
_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)}, q); _wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q);
// sideslip fusion // sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, vI, q); _wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q);
} }
} }
src/modules/airspeed_selector/airspeed_selector_main.cpp
+3 -1
View File
@@ -521,7 +521,9 @@ void AirspeedModule::update_wind_estimator_sideslip()
Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz); Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
Quatf q(_vehicle_attitude.q); Quatf q(_vehicle_attitude.q);
_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q); const float hor_vel_variance = _vehicle_local_position.evh * _vehicle_local_position.evh;
_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, hor_vel_variance, q);
} }
_wind_estimate_sideslip.timestamp = _time_now_usec; _wind_estimate_sideslip.timestamp = _time_now_usec;