AP_L1_Control: add lateral acceleration feedback (w/ EKF fixes)#32266
Draft
rubenp02 wants to merge 13 commits intoArduPilot:masterfrom
Draft
AP_L1_Control: add lateral acceleration feedback (w/ EKF fixes)#32266rubenp02 wants to merge 13 commits intoArduPilot:masterfrom
rubenp02 wants to merge 13 commits intoArduPilot:masterfrom
Conversation
Introduced an adaptive scale factor (_lat_acc_k) that learns the effective roll-to-lateral-acceleration gain from measured lateral acceleration and applies it in nav_roll_cd. This helps keep turns and circle tracking accurate when the coordinated-turn model is imperfect (e.g. sideslip/uncoordinated flight), and greatly improves navigation performance in aircraft with poorly tuned or no yaw control. The estimator is gated to avoid learning from low-speed/noisy conditions and is low-pass filtered using a new LACC_K_TC parameter. It is updated in the waypoint, loiter and heading-hold modes. Key changes: - Added LACC_K_TC parameter to configure the estimator time constant. Can be disabled by setting it to 0. - Applied _lat_acc_k in the nav_roll_cd coordinated-turn mapping, acting as a feedback correction on the effectiveness of the model. - Added _update_lat_acc_gain, which estimates a gain sample as measured_lat_accel / model_lat_accel, and low-pass filters it using the configured time constant. - Call the estimator from waypoint, loiter, and heading-hold updates. This also adds a dependency on AP_FixedWing to reuse the configured minimum groundspeed parameter when deciding whether the learning logic is reliable.
Added an RLAG streaming log message to report the adaptive roll-to-lateral- acceleration gain (_lat_acc_k), including raw and clamped gain samples, blend factor, demanded, modeled and measured lateral acceleration, and a status bitmask describing all gating decisions. In addition to this, _update_lat_acc_gain now avoids early returns for logging purposes, making the handling of some cases more explicit and its execution time more consistent.
Updated Plane tests that used hardcoded loiter radius correction factors (REALLY_BAD_FUDGE_FACTOR) that were previously needed for small loiters to pass. This correction is no longer necessary due to the addition of feedback to the navigation lateral acceleration -> roll model, which ensures that loiters of all sizes are flown at the correct radius.
Added NAVL1_LIM_BANK to the tests that require accurate loiter navigation and didn't previously have it. It is always set to 60, for consistency with other tests that already used it, and because it is a reasonable value for the default ROLL_LIMIT_DEG of 65.
Replaced instances of RTL_RADIUS with WP_LOITER_RAD in tests where the latter is the one that is used.
Reworked synthetic sideslip fusion to be robust when the zero-sideslip assumption is violated (e.g. sustained sideslip/uncoordinated flight), which was causing non-wind states to be corrected in the wrong direction and increasing EKF drift. This is especially important on aircraft that may not coordinate turns (e.g. flying wings). The issue was most visible in dead reckoning, where sustained sideslip could accumulate large position divergence quickly. Synthetic sideslip observation variance is now scaled by normalized innovation magnitude, so the fusion is smoothly down-weighted as inconsistency grows. When normalized innovation is outside the gate, non-wind state corrections are inhibited while wind-state correction is still allowed.
Reworked synthetic sideslip fusion to be robust when the zero-sideslip assumption is violated (e.g. sustained sideslip/uncoordinated flight), which was causing non-wind states to be corrected in the wrong direction and increasing EKF drift. This is especially important on aircraft that may not coordinate turns (e.g. flying wings). The issue was most visible in dead reckoning, where sustained sideslip could accumulate large position divergence quickly. Synthetic sideslip observation variance is now scaled by normalized innovation magnitude, so the fusion is smoothly down-weighted as inconsistency grows. When normalized innovation is outside the gate, non-wind state corrections are inhibited while wind-state correction is still allowed.
Contributor
Contributor
|
.... ah. You're patching the EKFs here. Can you explain your findings, please? |
Contributor
Author
The EKF makes a zero-sideslip assumption (w/ moderate variance) which makes it sensitive to non-zero sideslip (as is the case with the default SITL frame, most flying wings, and probably most poorly tuned user aircraft). In normal flight this seems to be just fine, but when dead reckoning it makes for significantly increased divergence. Furthermore, #32252 increases sideslip enough due to higher roll enough to fail the dead reckoning autotest most of the time. I've tried to mitigate this in the EKF by modulating the variance according to how reasonable the zero-sideslip assumption appears to be, and limiting update to just the wind states if innovation goes beyond a 2-sigma threshold. This appears to work (marginal improvement in There's additional context in #32252. The script I'm using for my local testing can be also be found there. |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
#32252 rebased on top of #32070 to see if it fixes the DeadReckoning test.