mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 10:37:34 +08:00
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>
This commit is contained in:
@@ -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);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user