TECS climb rate overshoot when using low speedweight

MartinSollie · October 22, 2019, 4:47pm

Hi, I am working on performing automated landings in a moving net. We do this by using Guided mode to implement LOS guidance using a separate onboard computer, following a glideslope with several segments into the net. For this we need accurate altitude control, but do not care as much about the airspeed, and therefore we set the TECS SPDWEIGHT parameter to 0 or close to it.

Despite many attempts at tuning the TECS controller we still have problems with climb rate overshoot. This is especially visible when looping the landing plan, having a steep climb directly after passing through the target. See this plot of the logged TECS h, hdem (actual and desired height) and dh, dhdem (actual and desired climb rate):

The log for this flight is also linked at the bottom of this post. This flight used a SPDWEIGHT of 0.2. The problem is clearly shown at the beginning and end of the climb. This is also a problem for the landing as it causes us to hit about 0.5m above the target with the glide slope segments shown here (caused by the change in path angle at approx 932s). The integral term of the controller makes the height reach the desired if given enough time where the desired climb rate does not change.

I have been digging in the TECS code, and my hypothesis for the cause of this at this point is that the feedforward part of the pitch control does not properly account for the speedweight parameter, and that this does not occur for the default speedweight of 1. Essentially, if SPDWEIGHT is set to 0, the desired pitch is calculated as

where TAS is airspeed, TC is TIME_CONST, k_damp is PTCH_DAMP, k_i is INTEG_GAIN and the subscript d denotes the desired value. The factor 2 in all terms comes from

float SPE_weighting = 2.0f - SKE_weighting;

(here) where SKE_weighting is the speedweight which is 0. For all terms where the time constant is included, the extra factor of 2 is no problem, as we just use the time constant for tuning anyway, but this is not the case for the feedforward . If this is a linearization of the required path angle for a given speed and climb rate, I don’t see how there should be a factor 2 included. I have also looked at the change in the value of the integral term with changes in desired climb rate, and the integral change is not far off from changing by half as much as the feed forward term, perhaps indicating that the extra pitch added by the factor 2 in the feedforward is compensated for by the integral.

I would appreciate some feedback on this, whether it is possible to remove the problem by changing parameters or if this is actually a bug in the TECS controller.

MartinSollie · October 28, 2019, 11:21am

These are the parameters used
X8_eggemoen_3.9.9.param (14.6 KB)

To better illustrate the problem, I have plotted the logged desired pitch and the contributions of each controller term below, scaled to degrees. This is the same section of time as plotted in the original post. While the speedweight is 0.2 in this data, the airspeed is well controlled using the throttle so a decent approximation is the controller equation above with the factor 2 replaced by 1.8. Doing this the sum of the components fit the logged desired pitch well. The more correct feedforward term is also plotted.

It can be seen that the FF-term grows rapidly as the climb begins (t=942s), and the P and D terms work to cancel this. Only when the I-term is able to “take over” is the P term reduced. The integrator value resulting from this is however not correct for level flight (from t=960), which makes the P-term again having to cancel this error until the integrator has reached the correct value.

MartinSollie · November 11, 2019, 10:09am

Hi again,
We did some more investigations on this, in SITL. Using the default parameters for the rascal, we did the following:

Set TECS_SPDWEIGHT to 0
Set GLIDE_SLOPE_MIN to 0

tecs_debug.parm (30.8 KB)

The following plots compare the height response for Ardupilot master with a small change in TECS to prove the point. The unchanged code gives this result:

While the controller is not very well tuned for this UAV model by default and gives some oscillations, the main problem is the error in height after changes in the flight path angle. We then tried replacing the line

github.com

ArduPilot/ardupilot/blob/master/libraries/AP_TECS/AP_TECS.cpp#L858






// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
// During flare a different damping gain is used
float gainInv = (_TAS_state * timeConstant() * GRAVITY_MSS);
float temp = SEB_error + SEBdot_dem * timeConstant();


float pitch_damp = _ptchDamp;
if (_landing.is_flaring()) {
    pitch_damp = _landDamp;
} else if (!is_zero(_land_pitch_damp) && _flags.is_doing_auto_land) {
    pitch_damp = _land_pitch_damp;
}
temp += SEBdot_error * pitch_damp;


if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {

with

float temp = SEB_error + (SEBdot_dem/2.0) * timeConstant();

where we divide the feedforward term in the pitch controller by the value of SPE_weighting, which is 2 when TECS_SPDWEIGHT is 0. After this division the pitch feedforward is an approximation of the flight path angle in radians, without any scaling, . Dividing by SPE_weighting in general is a problem, as it will be zero if TECS_SPDWEIGHT is set to 2 (i.e. during a takeoff). This code change gives this result:

The height follows the reference a lot better in this case. When plotting the contributions of each controller term, for each of the two cases, the problem is also visible. Note that the desired pitch and the sum of controller components have basically the same value in the plot, as they should. In the unchanged code, the integrator changes its value significantly when the flight path angle changes:

With the code change the integrator changes a lot less, and it seems to mostly be due to the controller not being very well tuned. The integrator value after transients is close to the same both when descending and climbing.

Any thoughts on this?

The logs can be found here:

iampete · November 11, 2019, 10:52am

looks like a improvement to me, although I no TECS expert, can you PR your change?

MartinSollie · November 11, 2019, 1:21pm

I am not sure simply dividing the feedforward term like we did here is the general solution to the problem, especially for TECS_SPDWEIGHT > 1. As mentioned, if TECS_SPDWEIGHT is set to 2 then SPE_weighting is 0, so it cannot be divided by. In that case the pitch controller should only care about airspeed anyway. The change we made is specific to TECS_SPDWEIGHT set to 0, but it shows the current problem/bug with pitch feedforward in TECS.

iampete · November 11, 2019, 1:31pm

A PR or a github issue is probably the best path to get some TECS experts to take a look

krisgry · November 11, 2019, 6:19pm

Thanks! Submitted as a PR, as advised: https://github.com/ArduPilot/ardupilot/pull/12822