Px4-autopilot: FW altitude control overshoot

[jinchengde]

FW altitude change, there is overshoot control

Looking at the pitch control, that is good for setpoint

the log attach is
Flight Review

and also discuss in
DroneCode Discussion

[RomanBapst]

@jinchengde You need to have a look at the TECS topic on the log in order to understand what is going on.
From a quick glance at the log it looks like the actual height rate is overshooting the demanded one by far.

Can you try to reduce FW_T_HRATE_FF to see if that improves the situation?

[Antiheavy]

@RomanBapst @dagar I thought this was a know problem with FW altitude control? We were able to reduce the overshoot effect slightly by adjusting the TECS tuning, but the pronounced overshoot is still there every time we have a large altitude change.

Here is one of our random flight logs as a reference:
https://review.px4.io/plot_app?log=7c1cf5af-38f4-4f05-8924-8a26f7ac16ac

[jinchengde]

@RomanBapst @dagar
I also test in SITL gazebo plane and it also have overshoot during automatic takeoff, here is the video for you reference
SITL Gazebo Plane
No effect even if I set FW_T_HRATE_FF to zero

[tubeme]

@RomanBapst @dagar We've had the same problem. It was even worse in 1.7.3 It works in both directions, ie. in steep pitch down. It seems like when you release the pitch stick to level the flight it takes some 1-2 sec for the craft to react, level and start follow its altitude set-point. The problem was visible in auto mission modes as well in 1.7.3 but now in 1.8 the mission flight got all the things better regarding the hrate settings and altitude hold. But still we have it visible during altitude mode flight.

[dagar]

@priseborough any suggestions?

[jinchengde]

@RomanBapst @dagar

set FW_T_HRATE_FF to 0.6
https://logs.px4.io/plot_app?log=be9d4f9b-5116-43a0-965c-04acabf17c73

set FW_T_HRATE_FF to 0.4
https://logs.px4.io/plot_app?log=7be20f90-1e71-4896-bbd0-b0ffab32ab4e

the last one looks more smoothly but still have sharp in altitude control

[priseborough]

The input shaping applied to the height and height rate demand inside the TECS _update_height_setpoint function

https://github.com/PX4/ecl/blob/master/tecs/tecs.cpp#L250-L257

can be improved by making the time constant of LPF able to be explicitly set via a parameter. To eliminate overshoot this cannot be much less than the TECS time constant FW_T_TIME_CONST.

This will need to be done in conjunction with other changes to avoid steady state errors during constant rate climb and descent manouevres, rather than rely on the user adjusting a FW_T_HRATE_FF parameter.

I will come up with some changes this week.

[priseborough]

A second order filter structure with critical damping should enable the vertical acceleration, rate and controller bandwidth limits to be respected whilst maintaining zero steady state error during constant rate climb and descent.

[tubeme]

@priseborough Look at what we came upon while tuning the hrate_ff gain.

In 1.8 we see much better response but still the same behavior on manual control.

https://github.com/PX4/Firmware/issues/8941#issuecomment-371755785

[priseborough]

I've done some Matlab simulation work on an alternative algorithm for the input shaping. It works for ramps and steps, noting that with step inputs, any use of a predictor or integrator term to eliminate height error must result in overshoot as neither TECS or the input shaping filter have future knowledge of when the ramp will level off and the vehicle dynamics cannot perform the infinite acceleration required to do so without overshoot.

Step Demand Example

Ramp Demand Example

Ramp Demand Example - Integrator used to eliminate height offset

The only way we are going to be able to track ramp inputs without height offsets and overshoot is to incorporate move some responsibility for input shaping in the waypoint logic so that TECS is not asked to perform the physically impossible task of tracking a height profile with non continuous first derivative whilst maintaining zero steady state tracking error.

Matlab source code attached:

height_demand_shaping.m.txt

[LorenzMeier]

@bresch Could you please contribute here? It sounds like we should try to do what we’re doing for multicopter as well for TECS here.

[priseborough]

It is also worth noting that lag free tracking of a time varying hight input is most important during approach and flare. At other times in flight, eg climbing or descending between waypoints, some lag is normally not an issue. Other autopilot projects using the TECS algorithm work around this by applying the feed-forward correction during landing approach only. This would be an effective short term solution.

[bresch]

@priseborough I wrote a library to generate a time-optimal, jerk limited (bang-bang) trajectory based on desired velocity for multicopter velocity control mode (see the PR). Position or velocity can be used as the input of the controller and one or several derivative(s) can be used as feedforwards. We could use the same for FW manual control: the input is the desired velocity, based on the sticks.

I'm planning to do the same for multicopter waypoint navigation (like it's usually done for pick-and-place industrial robots). This could be used to generate a smooth altitude trajectory + feedforwards for TECS in auto mode; do you think it could be a correct solution?

(image from _Design of third-order uniform acceleration and deceleration trajectory based on Simulink StateFlow_, Xudong Yang, Song Wang)

[dagar]

neither TECS or the input shaping filter have future knowledge of when the ramp will level off

In an old PR that was never merged I moved the navigator altitude ramp handling into the FW position controller. https://github.com/PX4/Firmware/pull/8883

[tubeme]

@priseborough I only think we have to find a permanent solution. The plane is performing decently now despite the overshoots. There no overshoots in mission, such that we could see. At least not noticeable.

[stale[bot]]

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.

[Antiheavy]

would be great to see FW overshoot improvements someday.

[bys1123]

Is #10746 fix this? @bresch

[bresch]

@bys1123 Nope, the PR was for multicopter navigation only but the trajectory generator is in a library and could be reused here.
~However, as Paul demonstrated, a simple 2nd order critically damped input filter should fix the issue. If we don't need to have strict requirements on the maximum amplitude of the derivatives, I would just go with that filter.~
_Edit: I misunderstood what Paul said_

[Antiheavy]

as Paul demonstrated, a simple 2nd order critically damped input filter should fix the issue. If we don't need to have strict requirements on the maximum amplitude of the derivatives, I would just go with that filter.

curious, is anyone working on this already for fixed wing?

[stale[bot]]

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.

[julianoes]

@RomanBapst @sfuhrer is this something that could get testing and brought in at some point?

[logindiamant]

Dear colleagues,
Please tell, where can I find Thrust msg/value in PX4?

[julianoes]

@logindiamant your question seems off topic. Please create a new issue with more detail or ask a question in discuss.px4.io.

[logindiamant]

Peoples, who discussed this main issue publish Thrust value in logs... So, question is on topic, @julianoes

[julianoes]

Aha sorry @logindiamant. It comes from:
https://github.com/PX4/flight_review/blob/bb39d190b91bb29b5bd43997e26e31976f6b87be/plot_app/configured_plots.py#L331-L333
and therefore:
https://github.com/PX4/Firmware/blob/f9a5c91f8cc5f74ce8d2a0425f7b76d7af65a0a8/msg/actuator_controls.msg#L15

So it's actuator_controls_0[3].

[logindiamant]

Ooo, many thanks, @julianoes!!!
I created UAV model on nonlinear differential equations with input {roll, pitch, yawrate and thrust}, and just want to verify it on SITL and real fly UAVs. We plan to fuse model in our visual-inertial odometer

[stale[bot]]

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

[stale[bot]]

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

[stale[bot]]

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

[stale[bot]]

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