VTOL planes are getting lift from the wing when flying in MC mode at
high speed. They (and some other drones) also get extra drag when
climbing and descending at high speed, corrupting the hover thrust
estimate.
To avoid this, two speed thresholds (vertical and horizontal) are defined
above which the sensitivity of the estimator is decreased by linearly
increasing the observation noise.
The condition to get valid requires a low variance and test ratio. but
to stay valid, only the variance is required.
Co-authored-by: Daniel Agar <daniel@agar.ca>
- parameter updates can be quite expensive because they trigger nearly all modules to reload all of their parameters immediately
- limit modules from updating faster than once per second
Without range finder, the validity flag goes to false quite quickly and
if can be that a vehicle never starts HTE is the takeoff is too slow.
In this specific context of takeoff detection, since
the exact value is not important, we can safely ignore the validity flag.
the _landed flag only is not enough to tell if the drone is still touching
the ground, so an additional check based on the altitude AGL is required.
to have this in_air detection correctly updated, the module needs to be
scheduled on the vehicle_local_position message as no setpoint is
produced in non-assisted modes.
In the predition step, the process variance was erroneously
multiplied by dt instead of dt^2. The default values are adjusted
accordingly to keep the same tuning for the default loop rate of 50Hz
- track and publish validity based on hover thrust variance, innovation test ratio, and hysteresis
- only publish on actual updates or becoming inactive
- fix dt (previous timestamp wasn't being saved)
- use local position timestamp (corresponding) to accel data rather than current time to avoid unnecessary timing jitter
- check local position validity before using
- mc_hover_thrust_estimator: move from wq:lp_default -> wq:nav_and_controllers to ensure the hover thrust estimator runs after the position controller and uses the same vehicle_local_position data
- land_detector: check hover thrust estimate validity and adjust low throttle thresholds if hover thrust is available
- mc_pos_control: only use hover thrust estimate if valid
- Use a low-passed value of the signed innovation test ratio to trigger
the state variance boost. The threshold of 0.2 has been chosen using log
replay and simulation scenarii.
- Do not reset the learned accel noise during a state variance boost.
After a few tests, this does not seem to help at all.
- Continue to learn the accel noise even if the measurement got rejected
to avoid ignoring sudden changes of noise
- Lower the acceleration noise time constant and increase min/max
values to avoid learning quickly a small variance that could temporarly
destabilize the filter
- Update filter time constants. Increasing the speed of the residual lpf
improves the quality of the learned accel noise
* MC_HTE: unitialize with hover_thrust parameter
* MC_HTE: constrain hover thrust setter between 0.1 and 0.9
* MC_HTE: integrate with land detector and velocity controller
* MCHoverThrustEstimator: Always publish an estimate even when not fusing measurements. This is required as the land detector and the position controller need to receive a hover thrust value.
* MC_HTE: use altitude agl threshold to start the estimator
local_position.z is relative to the origin of the EKF while dist_bottom
is above ground
Co-authored-by: bresch <brescianimathieu@gmail.com>
BazookaJoe1900