[WIP] L1 enhancements: handling small loiter radii and high winds

[tstastny]

Corresponding ECL PR: PX4/PX4-ECL#494

This PR does the following:

Handling small loiter radii

• Replace L1 PD controller with adaptive period on original L1 logic for adequately tracking small loiter radii. why?: L1 was designed to track circles, and the original paper described the criteria for choosing a period which guarantees convergence to a circle with a given radius. Reverting to original L1 (more specifically the L2+ formulation) keeps the logic throughout the l1 controller more consistent, straight forward, and avoids any other complications from the current PD / L1 switching behavior. Further, the PD anyway suffers the same problem of not tracking circles when not tuned for it. Here the adaptive logic handles this.

Handling high winds

• Remove ground speed undershoot. why?: this logic is overly conservative and ends up unnecessarily adding very large airspeed references in high winds. This PR replaces this logic with more efficient geometric considerations. (see more details below).
• Add wind estimate subscription.
• Use wind estimate in l1 guidance for handling case of wind speed > nominal airspeed reference. why?: the current wind robustification logic sets a hard switch from ground speed to airspeed vector after some arbitrarily defined minimum ground speed threshold, this causes large nasty jumps in the commanded roll reference, and further doesn't fully address the issue specific to the wind orientation.
• Add minimum ground speed parameter (used in l1 adaptations). @Antiheavy
• Revert to previous ground speed only formulation if wind estimate is invalid (default behavior).
• Update corresponding ecl submodule.

For more general information on the logic / background, additional information not specific to the PX4 implementation here can be found in https://arxiv.org/abs/1804.04209 and https://ieeexplore.ieee.org/document/7963609/ . I will update the prior link soon with the ground speed minimum formulation.

Replacing ground speed undershoot
The primary issue with ground speed undershoot as currently implemented is that it considers the wrong angle, the "air-to-ground" angle doesn't consider the orientation or magnitude of the wind, and thus results in very large airspeed increments when not necessary. See in the following figure that e.g. when the current airspeed is close to the minimum airspeed (could be the case for example during landings), the ground speed undershoot (which is added to the airspeed reference) approaches the minimum airspeed when flying in head wind. This means the airspeed reference effectively doubles, when really all that would be necessary is a few m/s increase to maintain some minimum forward ground speed.

This minimum forward ground speed can be achieved most efficiently utilizing the bearing feasibility function developed in the papers cited above (see a nice plot and description on the arXiv brief). This is just a geometric relationship (considering the wind triangle -- i.e. airspeed vector + wind speed vector = ground speed vector) which only increases the airspeed reference as much as necessary to maintain the given forward ground speed (where forward ground speed is defined as the projection of ground speed on the airspeed vector, so negative values mean the aircraft is flying backwards with respect to the ground). The plots below show the effect of this function and the resulting airspeed increments commanded for the case of various wind speeds, airspeed nominal reference = 12 m/s, and airspeed max = 16 m/s. The plots assume the aircraft is following the corresponding orientation commands from the wind robustified l1 controller. The forward ground speed is maintained up until such point that the airspeed maximum does not have any more reserve.

Simulations

All simulations were performed in MATLAB with a simple 2D coordinated turn model with representative first order roll and airspeed response dynamics (see https://arxiv.org/pdf/1804.04209.pdf for a more detailed description of the model). All guidance gains (e.g. operator defined period=25 and damping=0.707) were kept the same for each control formulation for a fair comparison. Nominal airspeed reference was defined to 10 m/s.

Tracking small radii with the adaptive period in the standard L1 formulation can be seen in the following simulation. A 20m radius is tracked. The PD control approach currently implemented does not achieve this.

Note the effective period automatically adapts (reduces) to track the circle.

Handling of high winds is compared in the next simulation. Here a 50m radius loiter is commanded, however, the wind speed (12 m/s here) is greater than the airspeed. The maximum allowed airspeed reference is set to 15 m/s. Four control formulations are displayed:

1. the current PX4 logic
2. new logic with airspeed incrementing disabled
3. new logic with airspeed incrementing enabled and min ground speed = 0 m/s
4. new logic with airspeed incrementing enabled and min ground speed = 3 m/s

The current PX4 logic has large jumps in roll setpoint due to the hard switch based on the "air-ground" angle and arbitrarily defined 3 m/s ground speed threshold.

The new logic shows each of three modes, 1) run-away mitigation (attempt to reduce the run-away to a minimum), 2) run-away prevention (bring run-away to a halt and hold position), and 3) min ground speed maintenance -- note the forward ground speed does not go below the commanded minimum (if the maximum airspeed would be lower, then the logic would attempt to maintain as much of the minimum as possible, then default to each of the lesser solutions). Airspeed references and forward ground speeds are shown below along with the "bearing feasibility". (note the lagged airspeed response is not shown to de-clutter things, but has a time constant of 1s).

HITL logs (x-plane, hilstar)

Log for small loiter radii (first commit): https://review.px4.io/plot_app?log=07bac744-e5dd-4ca4-abc8-524125878682 . Verified general WP following still works (should not have changed), and both large and small circles are tracked properly with reformulated loiter logic -- the small circle used the automatic period adaptation now present in the controller.

Log for high winds (first+second commit): https://review.px4.io/plot_app?log=dba557a7-8abb-44ec-bd13-43698d6ddf2e . Testing in very high winds (approx. 14 m/s with nominal airspeed reference = 13 m/s). Note my laptop is not powerful enough to send data to x-plane as fast as the EKF always wants -- so when the winds were getting increased, sometimes the GPS position was reset do to the lag in messages -- but after the initial increase in wind speed, things were stable enough in the EKF to test the functionality. Log timeline:

1. stabilized take-off and climb to altitude (also stayed in stabilized while increasing the wind speed)
2. once wind speeds were high enough, auto engaged
3. aircraft "falls" back into first loiter, attempting to follow as best it can while flying with tail wind (very fast!) then as the feasibility function reduces to zero (infeasible tracking), the aircraft turns against the wind to minimize the run-away from the loiter.
4. params are first set to not increment the airspeed, which means the aircraft is slowly running away at this point.
5. airspeed incrementing is then enabled with minimum ground speed set to zero, the aircraft then increases its airspeed reference to match that of the wind speed and maintain a zero ground velocity (run-away prevention).
6. the minimum ground speed parameter is then changed to a positive value (and the airspeed max also increased to allow the extra reference), and the airspeed reference is increased to maintain this minimum ground speed and follow the loiter. Note on the down-wind segment of the loiter, the airspeed reference is decreased again, as there is no need to waste throttle when the ground speed minimum is not being violated.
7. on the second time around the first loiter - the airspeed max is decreased again to 16 to demonstrate that the minimum ground speed of course cannot be maintained in the head wind leg now, but that the controller maintains as much of this minimum speed as it can -- here 2 m/s of the 3 that are commanded.
8. some general WP following is now commanded -- straight lines may also be tracked with minimum ground speed maintained.

Open questions on implementation:

• The airspeed increment is calculated in a 2D approximate of "true airspeed" (i.e. speed relative to the airmass, *not indicated). At the moment, this increment is just directly added to the *indicated airspeed setpoint for TECS -- to be completely correct, this should probably be converted from TAS to IAS before getting added, thought it won't make a huge difference at low altitudes and is anyway only incrementing the total setpoint. -- thoughts? I could make the conversion quite quickly in one more commit.
• Related to the bullet above, but in my opinion not completely necessary with the small angles considered, is that yet another conversion from 2D to the total airspeed could be done based on the current flight path angle.
• Is the radius already limited to some value greater than zero anywhere else in the navigator or position controller? If not, a minimum can be placed in the ecl controller to avoid the potential of the L1 ratio getting set to zero here ecl_l1_pos_controller.cpp#L265. If L1 ratio = 0, this causes a singularity in the acceleration command equation.
  answer: yes it is constrained in the mission block. navigator/mission_block.cpp#L508
• The navigate_heading function was left untouched. Mainly because I was unclear on what the actually goal of the navigate heading function is -- i.e. we should be clear the difference between "heading" and "course" and "yaw". At the moment, it is only used as a yaw controller in fw pos ctrl. This is fine, but perhaps the ground speed based logic for defining the lateral-acceleration doesn't really make sense in that case, as it is anyway controlling something about the air-mass relative (or assuming no sideslip -- body) axis. Probably better to use the airspeed as the nav speed for yaw control. If one wants to use this for course control (e.g. follow a bearing, ground relative), then these same wind robust adaptations can be applied, but would require a little bit of change to the function -- i.e. one would need to specify what exactly the input args are used for.
• navigate_level_flight function was also left untouched -- this is not currently used anywhere in the fw pos ctrl cpp. However, it may cause erroneous airspeed increases if someone does use this and then also calls the airspeed incrementing function. One way to deal with this would be to place a flag in the navigate level flight function that disables any airspeed increment output. But I wanted some other opinions on this, as the other approach could be that it is the responsibility of whoever uses these functions to call them properly.
• I wasn't sure what the ground rover module is doing. So I simply added a "zero" wind vector to it to ensure that functionality there has not changed at all.
• implications for people not using airspeed sensors -- we should probably think about how this should be handled here. @CarlOlsson @dagar
• Should EKF2_WIND_NOISE be increased to capture some gusting? Or should guidance calculate it's own wind estimate from ground speed, TAS, and yaw (perhaps with some complementary filter on TAS)? -- unless the very low-passed wind estimate is being used elsewhere, better IMHO to do the prior. @priseborough

Further work:

• This PR only covers the case that the prescribed airspeed maximum is sufficient in the given cases (and that no extra throttle is available even if one wanted to increase the airspeed reference further). If an aircraft has more thrust available for faster airspeeds in even higher wind scenarios - this should be a sequential PR to this and handle that case at the level of TECS. My proposal would be to make a separate guidance specific airspeed maximum (i.e. the maximum airspeed one can command in over-wind scenarios above the TECS specific airspeed max which is set for climb performance and probably should not change, e.g. FW_L1_AIRSP_MAX). Then some likely linear increase in the output thrust from TECS should be used to achieve this airspeed (as @CarlOlsson mentioned in Feature Request: improved minimum ground speed controller for Fixed Wing #10078). @priseborough any thoughts on this?

This is quite a big PR, so any help testing this for corner cases etc would be very much appreciated. Also happy to get feedback on anything I've missed / not considered while implementing. @dagar @Antiheavy @CarlOlsson @priseborough

[Antiheavy]

Cool stuff. We are happy to help flight test.

[LorenzMeier]

Could you please rebase? Thanks!

[CarlOlsson]

@tstastny This looks super cool!
Regarding the increase in max thrust for very high wind speeds and separate parameters for mission ascend/descend speed, lets sync up with everyone interested when this is in (Y)

[chamois94]

I could try the new algorithm (airspeed increment enabled and 5 m/s min ground speed) in SITL (with such a powerful wind (half the maximum allowed airpeed)) and I encoutered a problem when the wind estimate is not correct:
UAV was in loiter mode around a point and I applied a very quick 180° rotation to the wind vector (I know it's not physically possible but it was to test the robutsness of the control loop).
In this case the UAV ran away in a 90° direction from the wind and can never come back even if it has enough power to. The problem is that in this case the EKF will never be able to converge to a good estimation but does not know it (the wind ekf process was set to 1.0). I know it seems logical but maybe we should add a safety in this case ? Looking at the wind covariance can maybe help but not enough I think.

[tstastny]

The problem is that in this case the EKF will never be able to converge to a good estimation but does not know it

Thanks for testing! Yes - the guidance of course needs somewhat decent wind estimates. There is a bool there now that disables using the wind estimate in the logic if the estimate is "invalid" - but you are right that this does not help in the case that the estimator believes things are ok. Another (independent) issue at the moment (and listed on the open questions bullet above) is that I took out the ground speed undershoot completely, so there is actually no handling of this when the wind estimate is invalid.

First thing I'll add back in (as discussed on the other thread PX4/PX4-ECL#494 (review) with Paul), is (for the invalid wind estimate case) some form of ground speed undershoot utilizing the yaw angle and ground speed vector. This is not efficient, but the only failsafe possible when the wind estimates are not good. This will basically be what was there before, but I can at least get rid of the binary switch at the ground speed threshold to make the roll reference commands a bit smoother.

The second part is a bit more open on how to handle it, @priseborough do you have any intuition on how best to interpret the EKF outputs in these cases to the decide when not to use the wind estimate
(deem it invalid) within the position control loop? One could possibly check the result (as a sanity check) with current airspeed, yaw angle, and ground speed vectors -- but this is anyway what the EKF should be fusing (minus the low pass filter). So at that point, I guess the other option would be playing with the amount of low passing actually desired in the filter. -- I also thought about the difference between plugging in the TAS and yaw into the controller (and calculating wind vector from those values) instead of the ground speed and wind speed estimate vectors. But the decision for the latter was ultimately made assuming this was already being fused in a nicer way and filtered within the EKF. Maybe this was a bad assumption (at least some of the time). >>EDIT: I added another bullet to the "open questions" on this - I will play with increasing the EKF2_WIND_NOISE a bit in HIL. Checked this in a flight test yesterday with a slightly increased value, was better for capturing some gusts, but want to try increasing it more.

The more I think about this though -- I'm worried that many px4 users do not spend the time to properly calibrate their airspeed sensors (I've seen lots of log files uploaded with clearly underestimated true airspeeds). This will obviously lead to erroneous wind estimates.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[tstastny]

I wont have time to work on cleaning up this PR again until about a month from now (end of Feb). But I hope there is still interest. We have been using some version of this logic on our aircraft for the past 6 mo and it works quite well (is relaxing to see the aircraft behaving in a predictable manner in very high windy days). Sorry for the large delay.. just too many other priority projects in the last months. Anyway - I have an idea on how to hopefully satisfy all comments here once I get back into it.

[Antiheavy]

We are definitely interested in improved fixed wing performance assuming the concerns can be addressed.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[sfuhrer]

@tstastny do you think it would be possible to split this up into separate parts to bring them in more easily?

[tstastny]

@tstastny do you think it would be possible to split this up into separate parts to bring them in more easily?

I think it depends on what is desired as default vs advanced functionality. I discussed with @dagar some time ago about whether or not having separate libraries for different controllers would be a better idea, and then just interfacing them in the standard fw_pos_ctrl_l1 etc files with conditional params. This way no one's running setup gives them any surprises, but anyone is still able to use the new features if they want.

On the small loiter radii -- that could for sure be it's own PR. But again, depends on if it is desired as default or not.

Regarding @CarlOlsson 's comment on increasing the maximum airspeed in cruise flight (even if it exceeds the maximum airspeed set to make TECS run properly), this one could also be it's own separate PR. I did something simple for my own implementation where I just made a new "max cruise" airspeed setting that is only used when needed to compensate high winds (but keeps the TECS tuning unchanged). I did *not do very much analysis of this though. Was mostly just trying to fly level.. so this would need it's own investigation and experimentation on how to properly implement.

Windy augmentations could also be it's own PR. But would likely need some more considerations with the wind estimation. I.e. to make the windy controller run properly I needed to tune up the wind noise to something like 1m/s/s to capture/reject shorter term gusts. Also, depending again on whether or not we go for a separate library approach vs augmenting the current L1 (or both) .

• https://arxiv.org/pdf/1804.04209.pdf has the logic for L1 augmentations (assuming the PD switching logic has been replaced)
• My newer paper https://arxiv.org/pdf/1908.01381.pdf has a better logic (slightly different control structure) with additional functionality (e.g. keeping on track with zero ground speed as one option).
• Corresponding video showcasing flight results for all four wind compensation modes: https://youtu.be/oM690LO29kM

[LorenzMeier]

@sfuhrer The PR as-is looks more or less manageable. Let's discuss if this needs rather a concerted effort by you and Roman to get things validated and merged.

[Antiheavy]

Add minimum ground speed parameter (used in l1 adaptations). @Antiheavy

note that this part went in separately as an independent PR some time ago: #11381

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[moreba1]

@tstastny
Can we implement and use different L1 period and Damping factor for loiter? i think mission and loiter must have different navigation. for example big l1 period for loiter and smaller l1_period for mission navigation.

[LorenzMeier]

Closing as stale. @sfuhrer @RomanBapst Please review what from this could be brought in still.

[tstastny]

@sfuhrer @RomanBapst -- maybe hit me up on slack and we can discuss how these things may still get contributed in a timely fashion, I have coincidentally been "midnighting" some stability analysis for an improved version of all this (according to further developments from my PhD thesis) in the past week or so (off work hours). TL;DR - allows users not to need to change their current L1 tuning.. and is better in every other way. ;) Would just want to coordinate with you two to appropriately plan where and how code goes from the start.

Can we implement and use different L1 period and Damping factor for loiter? i think mission and loiter must have different navigation. for example big l1 period for loiter and smaller l1_period for mission navigation.

@moreba1, not sure that bigger periods for loitering will help tracking.. I would actually assume the opposite. Though, you need to be careful depending on your vehicle's roll delay, as this acts as a lower bound on the period you can select while keeping path following stability. The other posts about being careful with this PR were correct, that you need to know your system and be certain about the minimum period bound before adapting the period in flight. Something that would be quite easy to determine, however, is exactly this roll time constant, via simple stabilized roll steps and a first order ID. Then one can exactly determine the lowest period they are allowed to set. (via L2+ paper from Curry et. al. ACC, 2003 - this is just about identical logic to what is implemented in PX4).