AP_L1_Control: add lateral acceleration feedback

[rubenp02]

Note:
This PR supercedes #30438. More context in that thread!

Description

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.

Before vs after

These comparisons are from SITL simulations using the default fixed-wing model and all parameters set to their default values, except for NAVL1_LIM_BANK, which has been set to 65 degrees, matching ROLL_LIMIT_DEG to provide accurate loiter radius compensation with height.

Before

After

Before, but with SITL model tweaked to have 0 sideslip

For this test, the c_y_b coefficient in SIM_Plane.h has been set to 0, so the model doesn't see any adverse-yaw forces:

This proves that, for the most part, the issue had to do with turn coordination. However, just asking users to better tune their yaw controlers isn't enough, as some aircraft (e.g. most flying wings) don't have active turn coordination mechanismsm, but they should still be expected to navigate correctly.

Old integral term-based solution (#30438)

[rubenp02]

@tridge I have tried this with high sim wind (15 and 20 m/s) and it works just as "well" as master. But I haven't been able to replicate your problematic loiter shape exactly. Either way, I doubt this can possibly have any issues with wind. Maybe really low frequency and high intensity lateral turbulence can have a slight effect, but it can be tuned out.

[rubenp02]

A couple other notes:

• Heavy use of magic numbers here, I know. I even considered adding a block of constexprs at top to at least embrace them. Please let me know what you think about this and if it's unacceptable.
• The issue wasn't a lack of an integral term, even if adding one on AP_L1_Control: Add integral term to loiter navigation #30438 seemed to fix it. The loiter controller probably doesn't need one, at least to handle cross-track error, because of the centripetal term. The integral term only seemed to work, but in reality it was winding the lateral acceleration demand way up, forcing the (much lower) achieved acceleration demand to track the original, pre-integral-term one.
• This also doesn't have to concern itself with resetting an integral term on mode/submode (loiter tracking vs capture) changes, which is a major win since the old method fixed the cross-track error but significantly worsened loiter capture:

[rubenp02]

Forgot to remove the REALLY_BAD_FUDGE_FACTOR from the tests, which this is meant to do away with. I also had to update a couple other tests for this to pass.

Finally, I noticed that I had a breaking bug which only really showed up in left hand loiters/turns, which my test plans didn't have many of. The fix has in turn made the loiter entry behaviour just a little worse:

Since the main difference is that now the feedback is also learning from the initial left hand turn used to capture the loiter, this might even be better and only look worse because of tuning, but I haven't had the time to properly test it. It more closely resembles the 0 sideslip "ground truth" that can be found in the description of this PR, which I see as a good sign.

[magicrub]

instead of all the code to bring this param in which forces it forever-ish to be plane-specific, why not just pass it in /w update_waypoint() / update/loiter() ? You could also just set it occasionally like we set stall_speed and max_speed in AP_ADSB which keeps it vehicle-type agnostic and limits the library's dependencies

[rubenp02]

L1 guidance is plane-specific, so I thought it was a reasonable dependency. I agree that it's a bit overkill to pass the entire AP_FixedWing object just to optionally get the rarely set GROUNDSPEED_MIN param., but my changes @ #30439 (which I'm going to update soon) use a few others and at that point I think it's warranted.

I'll look into what you mentioned about AP_ADSB.

[tridge]

test results in SITL look good, I'd like @Georacer or @priseborough to comment on the maths

[tridge]

let's just use 5.0 here, and remove aparm

[tridge]

ping @IamPete1

[rubenp02]

Since the live testing in today's dev call didn't really show that much of an improvement, I have done further testing using @tridge's setup (same mission + sim params + aircraft params, except where otherwise noted). Here are my findings:

Note:
All tests are using NAVL1_LIM_BANK = ROLL_LIMIT_DEG = 65, so L1 attempts to properly track the loiter path, and the LOITER_UNLIM has been explicitly given a radius of WP_LOITER_RAD, which doesn't functionally change the plan but allows the GCS to render the circle. Both changes are just for ease of visual comparison and presumably don't have a significant effect on the actual results.

Scenario	master	This PR	Notes
Tridge's windy loiter setup			On the right side of the loiter, the wind slows the aircraft so much that it barely needs to generate any track lateral acceleration, so sideslip is nearly 0 and this PR's scale factor doesn't apply. On the left side, however, the little time the adaptive scale factor has to work with is enough to make the navigation ever so slightly tighter. This is what was tested in the dev call and is pretty much the worst case scenario.
No wind, default trim			Here, master has the aircraft following a slightly larger circle than intended because of the lack of turn coordination of the default SITL config. This PR easily compensates for it.
No wind, rudder trimmed to 1600 µs (proverse yaw)			1600 µs of rudder is about what's needed in the default SITL config to coordinate this specific loiter, so we've got very similar results from both. Presumably, this PRs scale factor is doing very little to no work here. We'll see when logging is added! Loiter entry is also improved slightly as the adaptive scale has enough time to learn just from that short initial left turn as well (and that's while fighting level-flight adverse-yaw, which the system assumes doesn't exist).
No wind, rudder trimmed to 1650 µs (strong proverse yaw)			Here, master has the aircraft following a slightly smaller circle than intended, because of the uncompensated properse yaw. This PR easily compensates for it. Loiter entry improvement is much more evident here as well.
No wind, rudder trimmed to 1450 µs (adverse yaw)			This is very similar to the "No wind, default trim" case but with even more adverse yaw. Same conclusions as there.

All logs: https://filebin.net/vxsab7yvlbuo8zfe

[IamPete1]

Could the EKF's side slip angle estimate be used directly? Here its effectively trying to learn a value? If the cause is only side slip could we just have a constant side slip coefficient that a user could set as part of initial tune/setup?

Slightly tangential, but I wonder if such a side slip coefficient might also allow the EKF to do a better job of dead reckoning.

[rubenp02]

Could the EKF's side slip angle estimate be used directly? Here its effectively trying to learn a value? If the cause is only side slip could we just have a constant side slip coefficient that a user could set as part of initial tune/setup?

I tried both and they work but don't seem as robust to me. Just adding the SSA estimate to the model in nav_roll_cd isn't as stable as this and doesn't close the loop, so if there are other higher order effects not being modeled (which I'd expect them to be), loiters still won't be fully accurate. The key point is that this isn't correcting for sideslip specifically, it’s adapting to the actual roll-to-lateral-acceleration effectiveness by feeding feedback into the previously open loop inverse model.

Having a constant coefficient (making _lat_acc_k a config. constant rather than learned) works really well in SITL (comes out to pretty much exactly 0.75), and in fact once this system learns a _lat_acc_k it doesn't change it much in practice, but I'd say that IRL this is more likely to be a slow airframe parameter rather than a fully constant one. This seems to be true for our rudderless V-Raptor, where with a constant coeff. we still saw some loiter tracking inconsistencies, but ever since we've been running a version of this, it has just worked. It also has the advantage that it gets tuned on its own, so it's perfect for the majority of users that just run the default config (which are exactly the kind of users which will have no or really poorly tuned yaw damping). And you also can't run a wildly incorrect tune that destroys your navigation. Finally, this option also wouldn't close the loop.

[rubenp02]

Having a constant coefficient (making _lat_acc_k a config. constant rather than learned) works really well in SITL (comes out to pretty much exactly 0.75), and in fact once this system learns a _lat_acc_k it doesn't change it much in practice, but I'd say that IRL this is more likely to be a slow airframe parameter rather than a fully constant one. This seems to be true for our rudderless V-Raptor, where with a constant coeff. we still saw some loiter tracking inconsistencies, but ever since we've been running a version of this, it has just worked. It also has the advantage that it gets tuned on its own, so it's perfect for the majority of users that just run the default config (which are exactly the kind of users which will have no or really poorly tuned yaw damping). And you also can't run a wildly incorrect tune that destroys your navigation. Finally, this option also wouldn't close the loop.

By the way, if this ends up being a constant airframe parameter (in SITL it seems to be), then maybe we can have it autotuned by default, with an option to fix it to an arbitrary value. Similar to ARSPD_AUTOCAL, but fine to keep enabled all the time.

Edit: I've thought about this a bit more and my take is that it's pretty likely to be slow changing rather than constant (at least IRL), with variables like propwash, CG, control surfaces trim, flap settings, etc. affecting it. But I might be wrong here, maybe this is a question for someone with more aeronautical expertise.

[IamPete1]

Maybe we just need that logging and to do some testing. One disadvantage of the current implementation is that it has to be re-learned every flight. Maybe the airspeed ratio analogy is a good one, we could have a parameter for the coefficient which would mean that its roughly correct when you first launch, then a param to enable/disable learning. Some may want to leave it on all the time and others may prefer that its set correctly and never changes.

My only real concern about a constantly learning is that it might go wrong in some edge case and end up causing a issue. Using airspeed ratio as a example if you were learning all the time and has some GPS issues you may learn a bad ratio which would cause a under/over speed.

I guess we could also just put a PID from lateral acceleration demand to roll angle with a FF based on the current calculation. I guess one difference between that and the this is that this assumes the vehicle behaves the same way in both left and right turns where as a PID would have to wait for the I term to swap signs.

[NDevDrone]

I guess we could also just put a PID from lateral acceleration demand to roll angle with a FF based on the current calculation. I guess one difference between that and the this is that this assumes the vehicle behaves the same way in both left and right turns where as a PID would have to wait for the I term to swap signs.

Funny you mention it, I was playing around with the idea of implementing an incremental nonlinear dynamic inversion law (INDI Lec) earlier today after I came across this discussion that was referenced in the other PR, which I think was this vintage google group discussions on the L1 controller. I didn't do anything crazy, just took Ruben's adaptation law, 'the base' and just added a feedback term delta_phi to it:

• Base: phi_ff = atan(a_dem / (g*cos(theta)*k_hat))
• New: phi_cmd = phi_ff + delta_phi, where
  delta_phi = (Kp*e_a + Ki*int(e_a dt)) / (g*cos(theta)*k_hat*sec(phi_ff)^2)
  withe_a = a_dem - a_meas_filt.

So while this probably would not help with the high non-minimum phase airframe scenario Paul refers to in the discussion thread, it could help improve the effective stability margin of the L1 controller by reducing the demanded to achieved lateral acceleration lag assuming the aircraft is tuned properly and has enough turn authority.

I did a very rough not so scientific A/B test where I change environment params at different points (after a few turns in loiter, and then a few turns in RTL before doing autoland. After some screwing around, I used the parameters for the test:

• Common params for each case: NAVL1_LACC_K_TC=4, i.e. the estimated gain from the lat accel feedback action, NAVL1_LACC_FB_TC=0.5
• Base: NAVL1_LACC_FB_P=0, NAVL1_LACC_FB_I=0
• New: NAVL1_LACC_FB_P=0.1, NAVL1_LACC_FB_I=0.02

The test itself was roughly takeoff, after some time fly to a waypoint with NAV_LOITER_UNLIM (R=120 m), I then changed wind during the loiter from a baseline 8 m/s (dir=360, turb=0.5, tc=2.0) to gusty winds from a different direction 20 m/s (dir=270, turb=2.0, tc=0.8) at a point, then back to the baseline after a few more turns.

Comparing the lateral accel law base (Ruben's commit) to the same with the addition of the PI feedback delta_phi for a few turns, I get during gusty wind turns an absolute mean error reduction 13.7 -> 12.0 m (-12%), a root mean square error reduction of 19.3 -> 15.3 m (-21%), a max outwards error reduction of +75.9 -> +50.1 m (-34%). Nets out to improved tracking for within ±10 m of the target 73.4% -> 79.2% of the time. So its something!

In comparison, for the lower wind speed portion on just the first 3 turns, reductions of MAE 13.4 -> 12.4 m, RMSE 18.4 -> 15.6.

Logs here
__

I appreciate the addition of yet another PID would likely be overkill so not clear if its worth to add, I could put it on top of this PR should it get merged to keep things straightforward.

[tridge]

Peter and I flew this PR again today, no issues found (was a low wind day)

[NDevDrone]

I made a variant with an added first-order lateral accel feedback term here https://github.com/NDevDrone/ardupilot/tree/l1-latacc-adaptive-first-order-inversion in case you want to try that out in SITL @rubenp02 @tridge

[rubenp02]

Added logging. Couldn't possibly fit the existing NTUN message, so I made another one specifically for this and therefore decided to log everything. Do tell me if it's too much! Has the additional benefit of keeping the execution time of the method much more consistent, which I assume is irrelevant but I guess conceptually nicer.

I also reverted the increased timeouts I had for some tests, as I'm going to try and find the root cause. All current test patches are, AFAIK, legitimate corrections to bad logic.

[rubenp02]

Maybe we just need that logging and to do some testing. One disadvantage of the current implementation is that it has to be re-learned every flight. Maybe the airspeed ratio analogy is a good one, we could have a parameter for the coefficient which would mean that its roughly correct when you first launch, then a param to enable/disable learning. Some may want to leave it on all the time and others may prefer that its set correctly and never changes.

Agreed, but it learns almost instantly upon first starting to turn, and currently I think it's good that it's constantly adapting because I'm inclined to believe that despite the learned gain being pretty much a constant in SITL, IRL it will keep changing due to aircraft config (throttle setting due to propwash if puller, flaps and other geometry/configuration changing controls, changes in CG for aircraft with payloads, etc.). It's also cheap to calculate, so nothing lost there either that you could meaningfully get back by disabling it.

As a middle ground, maybe copy the SERVO_AUTO_TRIM architecture? To provide periodic rather than continuous updates, and a simple way to force a static value if the user absolutely wants to.

My only real concern about a constantly learning is that it might go wrong in some edge case and end up causing a issue. Using airspeed ratio as a example if you were learning all the time and has some GPS issues you may learn a bad ratio which would cause a under/over speed.

I understand the concern but it's pretty easy to guard and bounded to +-50% of the model output. In practice this seems pretty conservative (in SITL at least, it absolutely won't help much if your rudder trim is seriously out of whack). You're still always guaranteed aerodynamically safe turns in the intended direction.

I guess we could also just put a PID from lateral acceleration demand to roll angle with a FF based on the current calculation. I guess one difference between that and the this is that this assumes the vehicle behaves the same way in both left and right turns where as a PID would have to wait for the I term to swap signs.

I intentionally didn't make this into a full PID because I considered it would be too complex, yet another opportunity for bad/no tuning, and because by turning this into a full blown PID, even if you include it as a FF, you lose a bit of the elegance of the model-based approach, which is pretty accurate in almost every case in a canonical and properly tuned aircraft. I came up with this specific controller for these reasons.