-
Notifications
You must be signed in to change notification settings - Fork 14.2k
Description
Describe problem solved by the proposed feature
There are quite a few open issues regarding altitude stabilization:
- [Bug] Altitude estimation issues #24653
- [Bug] Unstable height estimates #24058
- [Bug]Altitude Control Failure, Unable to Maintain Stable Height #24390
- [Bug] Multi EKF vehicle local positions starting with large offset on quad indoors #24206
All of the above describe a common problem of using laser rangefinders to with altitude stabilization and takeoff - both indoor and outdor:
- Confusion regading the usage of EKF2_HGT_REF, EKF2_RNG_CTRL and MPC_ALT_MODE, particuarly for indoor fligths
- Inconsistent altitude hold while in HOLD mode and LRF is available
- Inconsistent takeoff altitude when taking off if Baro is the primary height reference input.
Describe your preferred solution
As highlighted by @dakejahl in #25206, ideally we'd like to use the LRF for:
- Improve Altitude Hold stabilization while hovering. Mitigate disturbances from wind gusts, GPS multipath, and vibration.
- Fixed wing landing. Terrain Height is important for glide slope.
- Multi-Copter takeoff height accuracy. GPS accuracy is poor while on the ground and barometer suffers from ground effect.
- Multi-Copter landing. Same as above.
- Optical Flow. Velocity estimate relies on Range to convert from Pixel_Vx/Vy to Body_Vx/Vy
First step: rework of the LRF kinematics consistency (KC) checks
When checking for KC of LRF measurements, only the acceleromenter measurement shall be used to avoid bad baro data to invalidate LRF measuremetns, at the same time, if bad LRF measurements ends up degrading vehicle altitude estimation we should not use such data to validate new LRF measurements
Motivation
You can only trust the accelerometers for KC checks
Second step: stop using LRF for altitude aiding - only use it for vertical velocity aid
For outdoor usage, user shall always use EKF2_HGT_REF set to BARO or GPS while LRF is only used as additional source of vertical velocity observation
Motivation
Terrain altitude changes and without terrain altitude data LRF cannot provide altitude data as the the EKF expects it (namely, w.r.t. the ekf origin).
On the other hand, when the rigth conditions applies (vehicle is not moving horizontally and it is not tilted too much), then the LRF provides solid and reliable vertical velocity measurement which can help altitude estimation.
What this step aims to accomplish
Hovering under LRF range should result in an altitude estimate being extra stable
Possible variations
Use LRF to aid vertical position when hovering: the new logic shall make the EKF switch EKF2_HGT_REF to LRF in a way that there is not discontinuitiy in position.
Third step: on takeoff, EKF2_HGT_REF shall use LRF
Regardless the configured EKF2_HGT_REF, if good LRF is available on the first takeoff, then the local altitude should match (up to sensor offset) the LFR readings. As sson as good LRF is not good (vehicle starts moving horizontally, LRF goes out of range, etc) the main height reference shall go back to the desired one.
Fourth step: make altitude hold a control problem and decouple it from vehicle altitude if LRF is available
When hovering, if good distance to the ground is available, the drone shall always use that data to keep its altitude constant.
Describe possible alternatives
None at the moment but any suggention is welcome. The main objectives here are to improve altitude hold and achieve better takeoffs when LRF is available. How to reach these goals is still open to discussion.
Additional context
Multicopters have been considered as primary users of rangefinders in the above reasonings. In no FW operation you can assume flat terrain and therefore use LRF to aid altitude (position or velocity).
Indoor applications have not been considered. For indoor applications users shall just set EKF2_HGT_REF to LRF. Uneven terrain in indoor terrain is completely out of scope.