dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

WindEstimator: reworked filter initialisation 

- 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
src/lib/wind_estimator/WindEstimator.cpp
View File

@ -39,8 +39,8 @@
#include "WindEstimator.hpp"
bool
WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
const matrix::Quatf &q_att)
WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
const float tas_meas, const float tas_variance)
{
// do no initialise if ground velocity is low
// 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;
}
const float v_n = velI(0);
const float v_e = velI(1);
if (PX4_ISFINITE(tas_meas) && PX4_ISFINITE(tas_variance)) {
// 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
_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) = _scale_init;
const float cos_heading = cosf(heading_rad);
const float sin_heading = sinf(heading_rad);
// compute jacobian of states wrt north/each earth velocity states and true airspeed measurement
float L0 = v_e * v_e;
float L1 = v_n * v_n;
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;
// rotate wind velocity into earth frame aligned with vehicle yaw
const float Wx = _state(INDEX_W_N) * cos_heading + _state(INDEX_W_E) * sin_heading;
const float Wy = -_state(INDEX_W_N) * sin_heading + _state(INDEX_W_E) * cos_heading;
matrix::Matrix3f L;
L.setZero();
L(0, 0) = L4;
L(0, 1) = L0 * L3 + L6;
L(1, 0) = L1 * L3 + L6;
L(1, 1) = L4;
L(2, 2) = 1.0f;
_P(INDEX_W_N, INDEX_W_N) = tas_variance * sq(cos_heading) + heading_variance * sq(-Wx * sin_heading - Wy * cos_heading)
+ initial_wind_var_body_y * sq(sin_heading);
_P(INDEX_W_N, INDEX_W_E) = tas_variance * sin_heading * cos_heading + heading_variance *
(-Wx * sin_heading - Wy * cos_heading) * (Wx * cos_heading - Wy * sin_heading) -
initial_wind_var_body_y * sin_heading * cos_heading;
_P(INDEX_W_E, INDEX_W_N) = _P(INDEX_W_N, INDEX_W_E);
_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
// and measured airspeed
_P.setZero();
_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;
// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
_P(INDEX_W_N, INDEX_W_N) += hor_vel_variance;
_P(INDEX_W_E, INDEX_W_E) += hor_vel_variance;
_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
_time_rejected_tas = 0;
@ -124,13 +126,12 @@ WindEstimator::update(uint64_t time_now)
void
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) {
// 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;
}
@ -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) {
// only reset filter if _tas_innov_var gets unfeasible
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
@ -176,10 +177,11 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
}
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) {
_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;
}
@ -251,7 +253,7 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
if (meas_is_rejected || 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

12
src/lib/wind_estimator/WindEstimator.hpp
View File

@ -60,8 +60,9 @@ public:
void update(uint64_t time_now);
void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI,
const matrix::Vector2f &velIvar, const matrix::Quatf &q_att);
void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, 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 float hor_vel_variance,
const matrix::Quatf &q_att);
bool is_estimate_valid() { return _initialised; }
@ -129,8 +130,11 @@ private:
bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
// initialise state and state covariance matrix
bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
const matrix::Quatf &q_att);
bool initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
const float tas_meas = NAN, const float tas_variance = NAN);
void run_sanity_checks();
// return the square of two floating point numbers
static constexpr float sq(float var) { return var * var; }
};

6
src/modules/airspeed_selector/AirspeedValidator.cpp
View File

@ -81,11 +81,11 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
Quatf q(att_q);
// airspeed fusion (with raw TAS)
const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}};
_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)}, q);
const float hor_vel_variance = lpos_evh * lpos_evh;
_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q);
// sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, vI, q);
_wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q);
}
}

4
src/modules/airspeed_selector/airspeed_selector_main.cpp
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);
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;