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

WindEstimator: use vehicle heading instead of ground course for initialisation 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-01-14 18:15:58 +01:00 committed by Daniel Agar
parent 1a4b2b37bb
commit 0ce44cebb7
3 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -39,7 +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 matrix::Vector2f &velIvar, const float tas_meas,
const matrix::Quatf q_att)
{ {
// 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
@ -50,8 +51,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &
const float v_n = velI(0); const float v_n = velI(0);
const float v_e = velI(1); const float v_e = velI(1);
// estimate heading from ground velocity const float heading_est = matrix::Eulerf(q_att).psi();
const float heading_est = atan2f(v_e, v_n);
// initilaise wind states assuming zero side slip and horizontal flight // 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_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est);
@ -138,13 +138,13 @@ 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::Vector2f &velIvar, const matrix::Quatf &q_att)
{ {
matrix::Vector2f velIvar_constrained = { math::max(0.01f, velIvar(0)), math::max(0.01f, velIvar(1)) }; 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); _initialised = initialise(velI, velIvar_constrained, true_airspeed, q_att);
return; return;
} }
@ -197,7 +197,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); _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed, q_att);
} }
// 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
@ -219,7 +219,7 @@ 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 matrix::Quatf &q_att)
{ {
if (!_initialised) { if (!_initialised) {
_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length()); _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
return; return;
} }
@ -291,7 +291,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()); _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
} }
// 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

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

@ -58,7 +58,7 @@ 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::Vector2f &velIvar, 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 matrix::Quatf &q_att);
bool is_estimate_valid() { return _initialised; } bool is_estimate_valid() { return _initialised; }
@ -123,7 +123,8 @@ 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 matrix::Vector2f &velIvar, const float tas_meas,
const matrix::Quatf q_att);
void run_sanity_checks(); void run_sanity_checks();
}; };

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

@ -82,7 +82,7 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
// airspeed fusion (with raw TAS) // airspeed fusion (with raw TAS)
const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}}; 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)}); _wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)}, q);
// sideslip fusion // sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, vI, q); _wind_estimator.fuse_beta(time_now_usec, vI, q);