Browse Source

AirspeedValidator: fuse true airspeed before sideslip (#742)

- the states can be initialised more accurately using an airspeed measurement
and the wind estimator uses the first measurement to initialise

Signed-off-by: RomanBapst <bapstroman@gmail.com>
release/1.12
Roman Bapst 4 years ago
parent
commit
842d98d572
  1. 8
      src/lib/airspeed_validator/AirspeedValidator.cpp

8
src/lib/airspeed_validator/AirspeedValidator.cpp

@ -77,12 +77,14 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air @@ -77,12 +77,14 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
Quatf q(att_q);
// sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, vI, 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)});
// sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, vI, q);
}
}

Loading…
Cancel
Save