EKF3. Use barometer and GPS for Altitude (POSZ)

postelzhuk · June 25, 2024, 8:43am

Just in case if someone interested.

Parameter EK3_OGN_HGT_MASK doesn’t work as expected until this bug fix:

github.com/ArduPilot/ardupilot

EK3_OGN_HGT_MASK bit 3 incorrect behavior

opened 01:17PM - 12 Jun 23 UTC

closed 12:36AM - 23 May 24 UTC

pavloblindnology

BUG EKF

## Bug report **Issue details** If 3rd bit of `EK3_OGN_HGT_MASK` is set to…gether with 1st bit (EK3_OGN_HGT_MASK=5) and vertical position is taken from barometr (default parameters), copter climbs periodically indefinitely. The plot below shows 10x SITL simulation with real altitude (barometer or gps) jumping at regular intervals, while the controller (and EKF) altitude remaining constant. The real alt increases as copter periodically thinks it's below desired altitude (100m) and simply turns on the throttle to keep it. ![alt](https://github.com/ArduPilot/ardupilot/assets/27725335/5ffef0ee-26b1-4998-8d79-1557df87edcc) The reason is in `readGpsData` here https://github.com/ArduPilot/ardupilot/blob/514434193d7c4dba259180db4a9aa83391c7cf01/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp#L693 When 3rd bit is set, Ardupilot doesn't use `ekfGpsRefHgt` to convert GPS alt to local alt. But it still updates `ekfGpsRefHgt` in `correctEkfOriginHeight`. And the innovation used in the latter is constantly non-zero. The jumps in alt are introduced by the 5-sigma check here. If the check is removed, the alt rises smoothly and with acceleration. https://github.com/ArduPilot/ardupilot/blob/514434193d7c4dba259180db4a9aa83391c7cf01/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp#L817 It seems the cure is to always use `ekfGpsRefHgt` to convert GPS alt to local alt in `readGpsData`. Tested and it worked ok. **Version** Copter-4.3.7 **Platform** [ ] All [ ] AntennaTracker [*] Copter [ ] Plane [ ] Rover [ ] Submarine **Airframe type** octa **Hardware type** SITL **Logs** [BIN log](https://drive.google.com/file/d/1L9tBvGNUPdPz3cMmc6Q-fqI0JJzFv-RY/view?usp=sharing)

I tried SITL with Plane-4.4 and Plane-4.5. EK3_OGN_HGT_MASK parameter doesn’t help with baro drift at all.
From master branch (25 Jun 2024). This pull request is merged: Fix infinite climb bug when using EK3_OGN_HGT_MASK by peterbarker · Pull Request #26917 · ArduPilot/ardupilot · GitHub

EK3_OGN_HGT_MASK, 5 does help with SLOW baro drifts:

self.set_parameters({"SIM_BARO_GLITCH": deltaTime * 0.1}, verbose=False)

Ardupilot corrects Altitude by GPS data.

For sudden simulated baro glitches EK3_OGN_HGT_MASK, 5 doesn’t help. Altitude is not corrected at all. Even from master branch with this fix: Fix infinite climb bug when using EK3_OGN_HGT_MASK by peterbarker · Pull Request #26917 · ArduPilot/ardupilot · GitHub