Skip to content

Refactor LRF usage in EKF2 and control logic to improvealtitude stabilization #25258

@beniaminopozzan

Description

@beniaminopozzan

Describe problem solved by the proposed feature

There are quite a few open issues regarding altitude stabilization:

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.

Metadata

Metadata

Assignees

No one assigned

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions