Repository navigation
Conversation
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 880 byte (0.04 %)]px4_fmu-v6x [Total VM Diff: 872 byte (0.04 %)]Updated: 2026-05-28T13:09:40 |
dakejahl
left a comment
There was a problem hiding this comment.
Could you motivate why the drift detection is an external sibling of _baro_b_est rather than an extension of it?
If there's a reason the existing bias estimator + its isOffsetDetected() check isn't usable here, that'd be worth a comment in the code; if there isn't, I'd prefer the detection live inside _baro_b_est with an exposed accessor, and have EKF2::UpdateGpsSample call shiftAltOrigin when the bias estimator's status crosses a threshold. See inline comments for specifics.
Posted by Claude on behalf of @dakejahl
adedee3 to
22a358d
Compare
… gnss altitude drift correction
…ude estimate is drifting
7ad23d8 to
4054bd6
Compare
4054bd6 to
29c3d3f
Compare
| float32 delta_heading # Heading delta caused by latest heading reset [rad] | ||
| uint8 heading_reset_counter # Index of latest heading reset | ||
| bool heading_good_for_control | ||
| bool altitude_good_for_local_control |
There was a problem hiding this comment.
Add a comment please. Something like.
| bool altitude_good_for_local_control | |
| bool altitude_good_for_local_control # Set true if altitude can be used for local control |
There was a problem hiding this comment.
That would be another absolutely useless comment.
bool - Set true if
altitude_ - altitude
good_for_ - can be used for
local_control - local control
pure redundancy without a single piece of information added about why this could be useful.
There was a problem hiding this comment.
Fair call @MaEtUgR.
So the rule is "fields should be named as far as possible to be self-documenting, and comments are only needed in order to capture non-obvious behaviour"?
I don't mind redundant comments because they show that there is nothing special - while the absense of a comment may well mean the developer didn't think documentation was important.
dakejahl
left a comment
There was a problem hiding this comment.
I'm not convinced this is the right approach. We're ignoring distance sensor and any other altitude sources. Baro is already very problematic due to propwash and ground effect.
|
@dakejahl for any vehicle operating at higher altitudes, or even moving slightly in the horizontal direction, using a distance sensor would be fairly ineffective. Since this is a voting system, disturbed barometer measurements (e.g. due to prop wash or ground effect) would very likely be rejected, because the GPS velocity integral and GPS altitude generally agree with each other. |
…udeDriftDetector`
…ood_for_lock to true otherwise a mode making use of it would be be backwards compatible
…eset by the order of members
MaEtUgR
left a comment
There was a problem hiding this comment.
I tried to rename and restrcuture a bit to hopefully make the EKF2 less cluttered and interface easy to understand but I still have open questions.
- Didn't yet grasp the detection logic yet. Maybe we can add a simple test to make it more clear.
- Can't we go without an additional uORB message just for the drift? Maybe the better answer is no.
- The drift message I think should contain a cumulative value such that it stays accurate even if a message is missed by the consumer.
| home.alt += gnss_altitude_drift.altitude_offset; | ||
| home.z -= gnss_altitude_drift.altitude_offset; |
There was a problem hiding this comment.
I'm concerned with this difference only approach. I obviously misunderstood the meaning of the field commenting accumulated altitude offset from drifting to a more accurate solution. What if the consumer misses some gnss_altitude_drift message? Then it cannot track the drift anyemore.
Can't we publish the accumulated drift and make it easy to keep track of? 🤔
| @@ -0,0 +1,2 @@ | |||
| uint64 timestamp # [us] time since system start | |||
There was a problem hiding this comment.
Great start. Short description and optional long description?
…so smoothing subclasses don't overwrite the NaN
| float _gps_eph{0.f}; | ||
| float _gps_epv{0.f}; | ||
| uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; | ||
| uORB::Subscription _gnss_altitude_drift_sub{ORB_ID(gnss_altitude_drift)}; |
There was a problem hiding this comment.
To support multi-ekf, should this be a multi subscription with estimator_selector_status logic like src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp:134?
There was a problem hiding this comment.
Good point, I haven't checked that. Does it still work with the single vehicle_gps_position subscription because that's consolidated at the publication side and break with gnss_altitude_drift because there's one instance per EKF? If so then that would need to be addressed, if not it would be a perpendicular issue and I'd suggest addressing multi-EKF separately. 🤔
There was a problem hiding this comment.
Yes I think you're right.
I'm guessing that with multi EKF, it would be difficult to break because all EKFs only differ by IMU and/or mag, not by baro or GPS, meaning that all instances of gnss_altitude_drift should be about the same other than timing differences.
…nd Baro compile flags for consistency
There was a problem hiding this comment.
I checked again, from my side it's good to go now. The multi-EKF concern needs to be checked though to not break functionality.
I'm not sure why it exceeds kakute flash? I double-checked and my last commit making the defines consistent doesn't change the binary for the kakute board and if it would could only make it smaller.
EDIT: CI rightfully builds against main and the flash addition was merged there...
| gnss_altitude_drift_s gnss_altitude_drift{}; | ||
| gnss_altitude_drift.timestamp = hrt_absolute_time(); | ||
| gnss_altitude_drift.altitude_offset = _gnss_altitude_drift_detector.accumulatedAltitudeOffset(); | ||
| _gnss_altitude_drift_pub.publish(gnss_altitude_drift); |
|
While testing I found that, when --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
+++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
@@ -103,6 +103,12 @@ void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
void FlightTaskManualAltitudeSmoothVel::_setOutputState()
{
+ // Reset position setpoint to current position while altitude is unlocked
+ // to prevent a jump in setpoint due to smoothing when it re-locks
+ if (!_altitude_good_for_lock && PX4_ISFINITE(_position(2))) {
+ _smoothing.setCurrentPosition(_position(2));
+ }
+
_jerk_setpoint(2) = _smoothing.getCurrentJerk();
_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
_velocity_setpoint(2) = _smoothing.getCurrentVelocity();I've been testing by running I'll try and collect some real world examples of drift for tuning. |
|
I think it's worth adding in a max time that we will wait for stability before locking altitude again, maybe in the flight task. That seems preferable to permanently losing altitude lock in the case of some failure. |
|
After more testing and tuning, I've found that it's difficult to detect drift fast enough to avoid unintended movement without causing false positives or rejecting altitude lock for long periods of time. I have some examples with 2 minute of slow GPS altitude drift, and some where it drifts over 3 meters in 10 seconds. I'm going to try making the following changes:
It seems to me that resetting the GPS altitude this way is more "correct" than trying to control the drone in a way where the drift won't matter. False positives also feel much less risky when it's just falling back to baro. |
We need proper ground-effect detection and fusion gating (or temporary variance inflation). There's also the thrust affect on baro pressure which pulls the Z estimate during throttle transients. See #26924 I think it will be hard for this baro-gnss-drift-detection to work reliably without considering the effects of thrust and ground-effect. My 2 cents. |
|
@dakejahl I do see those effects on baro, which is why I'm hesitant about home adjustment based on baro. My current iteration primarily uses the GPS velocity as a means of rejecting slow drift and just requires that the baro agrees in direction and be closer to the estimated drift than it is to the ekf (like the OG PR). |
|
I'd try to systematically induce the position jump (toggle on SBAS after takeoff/hover?) and then post-analyze the log. You can check and see if GNSS vel is reliably independant fom position and correlated with baro-derivative and accel-integral as a means of identifying "gps position drift windows". Maybe that's what you're already doing? |
|
This pull request has been automatically closed due to 120 days of inactivity. If you would like to continue, please feel free to reopen it or submit a new PR. |
EKF2 GNSS altitude drift detector with covariance decorrelation and control-side velocity-only fallback
Solved Problem
With
EKF2_HGT_REF=GNSS, slow GPS altitude drift correction pulls the fused vertical state along with it. The vehicle silently climbs or descends to track it, even when baro and vertical-velocity integration both say the altitude hasn't changed.Prior mitigation in
HomePositiononly ran in a short post-takeoff window and only nudgedhome.alt.Solution
Move the detector into EKF2 and signal downstream consumers via a new
vehicle_local_positionflag. While the flag is set, the EKF decorrelates the vertical position state from the rest of the state vector so baro corrections can pull pos(z) without leaking into vel(z) or accel-bias, and the altitude flight task drops position control on z.Detector (
EKF2::GpsAltDriftDetector): 20 s sliding window at ~1 Hz ofd1 = ekf_amsl - baro_lpfd2 = ekf_amsl - vel_integralTrigger when
|Δd1|and|Δd2|both exceed a threshold AND|Δ(d1 - d2)|is smaller than either: baro and vel-integral agree, the fused altitude is the outlier. One-cyclehit_pendinghysteresis filters single-sample outliers. PublishesGpsAltitudeDriftCorrection.Detector reference is the fused AMSL (
getLatLonAlt().altitude()), not rawgps.altitude_msl_m, so the detector sees the altitude the system is actually acting on.EKF response (only runs
in_air && height_sensor_ref == GNSS):vehicle_local_position.altitude_good_for_local_controlreflecting the detector flag.false,Ekf::decorrelateAltPos()zeros the off-diagonals on the z-position state every GPS sample (P.uncorrelateCovariance<1>(State::pos.idx + 2)). This breaks the covariance coupling between z-pos and z-vel / accel-bias so the GPS-driven pos correction doesn't poison neighbouring states; baro fusion then pulls pos cleanly back toward truth.Control response:
FlightTaskManualAltitudereads the flag and, while it's false, sets_position_setpoint(2) = _dist_to_ground_lock = NANso the controller falls back to vertical velocity control only (introduced together with this change in the simplification of the unlock logic).Expected outcome by mode:
Alternatives
Earlier iteration absorbed the detected offset into the local origin altitude. Each detection event then produced an instantaneous step in
lpos.ref_altandlpos.z, which the position controller saw as a position error and tried to chase, producing visible altitude jumps in flight. Dropped in favour of the covariance-decorrelation approach above, which keeps the published frame continuous and lets baro do the rebalancing.Changelog Entry
Test coverage
Tested on hardware with artificial GNSS altitude drift injected (applied to
vehicle_gps_position).global mission: estimate and controller follows GNSS even during drift-correction
position-mode: estimate follows drift correction while controller only follows velocity estimate to stay "static". The initial 1m drift is the threshold window until the drift gets detected:
local mission: in controller we cant detect what mission type it originally was. Therefore we cant ignore the position setpoint which leads to the "wave" behavior. I still think this is much better compared to the drift we had previously.
