Attitude control not reaching input reference

Hi there,

While performing experiments to examine the behavior of the attitude closed loop in Arducopter, I found that the attitude reference, in this case a roll input, is not attained properly: there is a steady state offset (see attached figures below). This phenomenon is observed on both of my setups: a DJI F450 and a custom 7 inch quad, both equipped with a Pixhawk Cube Black. In SITL, however, this steady state error is completely absent.

I am providing a step input through mavros from a companion computer (Nvidia Jetson TX2) in guided, and read the roll value through mavros as well. Arducopter version is 3.7.0-dev (https://github.com/unl-nimbus-lab/ardupilot, basically 3.6 but with an added mode to support thrust commands in guided mode, which is not being used in this experiment).

My question is: do you have suggestions to get rid of this phenomenon and have the attitude controller reach the reference? Increasing the gain on the angle controller yields smaller offsets, but large oscillations.
I find it a bit weird that this behavior occurs, as the angle controller should provide an error to the angular velocity loop, which in turn has an integrator in it (I is definitely not set to 0, at the bottom I attached the used .param file), so I would expect it to, at least slowly, converge to the reference.

F450 (for ATC_ANG_RLL_P ranging from 4.5 up to 12):


Custom frame (for ATC_ANG_RLL_P ranging from 3 up to 6):

SITL (default):

Used parameters on the custom frame:
params.param (14.2 KB)

Have a look at the log message PIDR.I. Is the I term reaching its IMAX of 0.5? If so, you need to increase ATC_RAT_ROLL_IMAX.
If it’s not reaching IMAX, but is plateauing at some lower value, you might benefit from increasing ATC_RAT_ROLL_I. I know it’s non-zero, but it could be that your F450 is a bit heavier on the left side, requiring a larger I term to get rid of the steady state error

Thank you for your reply @phinchey. I had to do some new experiments because the PID signals weren’t logged yet, so now I have some results and I saw that the integrative action is far below the IMAX.
To check what you said about the drone being heavier on one side, I did the same test in the other direction and the result is the same, as I had expected because both frames are close to perfectly symmetrically loaded.

Increasing ATC_RAT_ROLL_I does seem to have a positive effect:
low I (0.0013):
low_I

higher I (0.0040):
high_I

I will try with some more tunings for the rates loop and the angle loop, but I’m afraid the disturbance will never be fully rejected.
My best guess is now that the disturbance increases with the velocity, such that the integrator of the inner loop is not enough to counteract it on a step response. I think an integrator on the outer loop would be the most effective solution.

Can you attach a .BIN log? I’d like to look at the rate response.
That’s not only a steady-state error, but the error is actually increasing quite a lot, and the rate P term is not correcting. Additionally, the rise time of 0.2s it not brilliant. I think you can increase your rate P term quite a lot. Your D term is extremely low, so don’t worry if you have to increase that to keep the P-induced overshoot / oscillation down. I wouldn’t be surprised if you increased P by 5x and D by 10x or more.

I will get you the most recent .bin file on Monday or one of the next days (still have to download those logs and don’t have the Pixhawk here right now).

Yes I agree, I also expect this 7" quad to be a lot more agile than the F450 as it’s a lighter, smaller and a much more powerful setup. The thing is I have experience with tuning in other flight controller software such as Kiss and Betaflight, but I’m not used to Arducopter, which is why I’m not familiar with the filter settings used. I noticed a lot of them (INS_GYRO_FILTER, INS_ACCEL_FILTER, MOT_THST_EXPO, ATC_INPUT_TC…) affect the vibrations where at first I thought it was due to the PIDs, which is probably why they are set so low.
P x5 would really surprise me, because if that was possible then it would have to fly super sluggish right now which it doesn’t, but I will definitely follow your advice and try to increase it together with D as well as I and update here what happens.

Here’s a link to the log:
https://drive.google.com/file/d/1epee3ObomjCXe5T_eRa-OoADZRVkYIz6/view?usp=sharing
Not sure if it contains the signals you want to view, if not just let me know what to change and I’ll do a new test.

I’m thinking maybe this 0.2s is not entirely due to rise time, but also partly through communication delay between the TX2 and the Pixhawk (as the beginning is really flat, just a delay)…

You’re right, the rise time is not that slow - I assumed the companion computer was inputting a step, but the input itself has a rise time and the response is not too far behind it.
The overshoot is about 20%, which is on the upper end of acceptable, but I think you could benefit from more rate D to dampen out some of the resulting oscillation.

I think the I term still isn’t strong enough to overcome whatever stabilising force is acting on your UAV. Keep increasing I until it’s fixed or you hit the max value. You have plenty of room to increase

Update: I recorded some longer responses, and with sufficiently high P, I and D eventually the roll angle evolves to the reference value - quite slowly though. The dip at 1~2 s I can’t get rid of.

In the figure below I increased the rate PIDs up to P=0.090, I=0.080, D=0.0020. For this setup this is extremely high: I hear a nasty rattle and get oscillations in acro mode, but I just recorded it anyway for the sake of investigation.

The highest I can get them while still sounding and looking smooth and flying very agile in acro are P=0080, I=0.040, D=0.0009. Increasing any of them leads to a rattle and/or oscillations. I noticed that I needed to set the FILT value for roll and pitch to 40 to get these values oscillation-free (although the tuning process instructions advise ATC_RAT_PIT_FILT~= 20-30 for 7" props).

high_pids

P.S. Increasing P by 5x and D by 10x didn’t work out - it started shaking very badly, but like, very badly.