Position Controller PIDs: missing POS I + inconsistencies

guglie · August 2, 2019, 10:00am

I’ve added position Z and target position Z to PSC log and confirmed that there’s a constant offset between altitude target and position of the drone.

Moreover on Z I can see much more delay than on X,Y.
Cheating a bit with the scaling of the graph it’s clear that the performance on PosY (blue) against TargetPosY is way better than PosZ (red) against TargetPosZ (yellow).

I hope that this is only a matter of PSC parameters tuning, but I’ve already tried a lot of values, the only way I could reduce delay was setting a very high POSZ P but it gives big overshoot.

This is obtained in SITL setting target position and velocity at high frequency in GUIDED on copter 3.6.10.
I’ve tried also setting AHRS_EKF_TYPE = 10 to have ground truth position with similar results.

@rmackay9 @Leonardthall can you help on this? What are the differencies between horizontal and vertical controllers?

guglie · August 2, 2019, 8:09pm

I succeded to reduce/remove the offset, but not the delay.

Log file here

rmackay9 · August 2, 2019, 11:25pm

I suspect the offset has something to do with the difference between altitude above home and altitude-above-ekf origin. I don’t have any idea where that offset would creep in and I suspect it’s a logging issue not an actual altitude hold issue.

Leonardthall · August 3, 2019, 12:14am

The delay is a normal part of the current Alt Hold controller. This is one of the things that will be removed in the rewrite.

The offset in altitude is a result of the mismatch between the velocity output of the EKF and the altitude accuracy of the EKF.

You said you fixed that part. How did you do that?

guglie · August 5, 2019, 10:42am

Thanks @rmackay9 and @Leonardthall

To remove the offset I

changed PSC_ACCZ_I from 0 to 1 
changed PSC_ACCZ_P from 0.5 to 0.7
changed PSC_POSZ_P from 1 to 1.5
changed PSC_VELZ_P from 2 to 5

(I know, ACCZ_I was 0 because in theory is the same as VELZ_P right? Indeed I can achieve same results leaving ACCZ_I = 0 and increasing VELZ_P more)

But there’s still another 10cm offset between PSC.PZ and position from EKF

Why? Is it for the “mismatch between the velocity output of the EKF and the altitude accuracy of the EKF”?

Leonardthall · August 5, 2019, 11:27am

Ahh, this should have been set to 1 to start with.

No, if you don’t have the I term the aircraft will require a constant offset in velocity to counter the constant offset that should be taken up by the I term.

Why? Is it for the “ mismatch between the velocity output of the EKF and the altitude accuracy of the EKF ”?

Yes that is likely.

guglie · August 28, 2019, 10:47am

Hi @Leonardthall,

I’ve tested the position controller a lot sending a setpoint at high frequency in Guided.
How can I reduce the “normal delay” (nearly 1 second) in the current Alt Hold controller?
Is there some filtering going on?

Here’s a plot with target altitude in red and current altitude in blue of vertical movements with increasing speed.

To remove the early slowdown (mostly descending) I need a position I term, right?
Or is there some other parameter I can tune?

Here are some of the parameters I’m using
LOIT_ACC_MAX,980.665
LOIT_BRK_ACCEL,500
LOIT_BRK_DELAY,0
LOIT_BRK_JERK,500
LOIT_SPEED,1250
PSC_ACCZ_D,0
PSC_ACCZ_FF,0
PSC_ACCZ_FILT,20
PSC_ACCZ_I,1
PSC_ACCZ_IMAX,1000
PSC_ACCZ_P,0.5
PSC_ACC_XY_FILT,2
PSC_ANGLE_MAX,0
PSC_POSXY_P,1
PSC_POSZ_P,1
PSC_VELXY_D,0.5
PSC_VELXY_D_FILT,5
PSC_VELXY_FILT,5
PSC_VELXY_I,1
PSC_VELXY_IMAX,1000
PSC_VELXY_P,2
PSC_VELZ_P,5
WPNAV_ACCEL,1000
WPNAV_ACCEL_Z,1500
WPNAV_RADIUS,0
WPNAV_SPEED,1000
WPNAV_SPEED_DN,1500
WPNAV_SPEED_UP,1500

Are the following “hidden parameters” related?
POSCONTROL_STOPPING_DIST_UP_MAX = 300cm
POSCONTROL_STOPPING_DIST_DOWN_MAX = 200cm
POSCONTROL_LEASH_LENGTH_MIN = 100cm
POSCONTROL_VEL_ERROR_CUTOFF_FREQ = 4hz
POSCONTROL_THROTTLE_CUTOFF_FREQ = 2hz
POSCONTROL_ACCEL_FILTER_HZ = 2hz
POSCONTROL_JERK_RATIO = 1

By the way, which method can I use to find the optimal frequency to send the target position and velocity?

The quadcopter I’m using is small and very agile. I’m flying indoor with a precise indoor localization system (~5cm accuracy, 100ms delay).

Leonardthall · August 28, 2019, 11:11am

I would need a log of the plot above to comment.

guglie · August 28, 2019, 12:21pm

Sorry, I forgot it

Another interesting thing is that it seems there is almost no delay in the altitude control for landing.

guglie · August 29, 2019, 4:05pm

I’ve done the same test with horizontal movement.

Quite surprised by the amount of overshoot and overcompensation from braking when speed increases. Didn’t noticed that in more complex movements (but slower) like the ones in the first graph above.

How can I reduce that keeping a fast response?

Another strange thing are the little spikes (showed by the red arrow) in the target position (PSC.TPY, green) compared to the guided target pos (GUID.pY, cyan). This divergence is not observable in the vertical test log.

Note that I’m commanding target position and velocity.

Anubis · August 30, 2019, 6:55pm

I think this is different than the vertical problem. In this horizontal test, the acceleration targets are being followed, but the velocity targets are way off. To me, this means that the (horizontal) velocity controller is not tuned well. Have you tried tuning out those overshoots?

Leonardthall · August 31, 2019, 11:19am

The ctun logging is turned off.

Until I rewrite the alt hold controller the only way to reduce the delay is to tune the alt hold controller up tighter. So you will need to increase:
PSC_POSZ_P
PSC_VELZ_P

guglie · September 2, 2019, 9:19am

Thank you @Leonardthall, Here’s 2 logs with CTUN enabled.
One with PSC_POSZ_P = 1.2 and PSC_VELZ_P = 6

And another with PSC_POSZ_P = 1.4 and PSC_VELZ_P = 7

On the horizontal, do you know how I can reduce the overbraking shown in the above graph?

@Anubis Thanks, so do you think I have to reduce PSC_VELXY_P and PSC_VELXY_I?

I see now that target velocity (PSC.TVY, blue) is way different from commanded velocity
(GUID.vY, red).

Does it take into account corrections for position target as well?
How is velocity target blended with position target?

Leonardthall · September 2, 2019, 12:04pm

Sorry, PID logging isn’t turned on either.

You can try increasing the VEL XY D term. In general the pos controller is tuned to be good for everybody so there is lots of room to tune it up further.

guglie · September 2, 2019, 3:12pm

@Leonardthall here’s a log of vertical movement with CTUN and PID log messages with POSZ_P = 1.4 and VELZ_P = 7

here horizontal movement (with CTUN and PID) with VELXY_D = 0.7

Do you think I can increase even more those parameters?

Can I raise the following parameters to have faster filters? How much?
PSC_VELXY_D_FILT,5
PSC_VELXY_FILT,5
PSC_ACC_XY_FILT,2
PSC_ACCZ_FILT,20

Leonardthall · September 3, 2019, 12:03am

Sorry @guglie this is driving me crazy. Just set these logging parameters please.

Do the test one more time for me. Sorry but I need full resolution logs to understand what is going on here.

You can take VELXY_D above 1. Like any D term you can increase it until you see oscillation in the position controller.

I think the guided input command has steps in it making things not work as well as they should in the controller.

I can look further with the next (and hopefully final) log.

guglie · September 3, 2019, 10:37am

Hi @Leonardthall, ahaha I’m sorry, I’ve set up like you said (almost, I removed RCIN, CAMERA and OPTFLOW).

With all those messages the logs have gaps, even if I increase LOG_FILE_BUFSIZE.
And I think that there are artifacts with the frequency of the logs and the frequency of the commands.

Thank you so much!
I’ll recap some of the questions in my mind:

Why the horizontal brake moves the drone back so much? Do you think those are the beginning of big oscillations from Position Controller PIDs or I have to modify LOIT_BRK_* parameters?
Target velocity (PSC.TVY) is way different from commanded velocity (GUID.vY). Does it take into account corrections for position target as well?
What are those little spikes in PSC.TPY that make it different from GUID.pY?
Can I raise the following parameters to have faster filters? How much?
PSC_VELXY_D_FILT,5
PSC_VELXY_FILT,5
PSC_ACC_XY_FILT,2
PSC_ACCZ_FILT,20
I want to tweak this “hidden parameters”, in particular the filter frequencies that to me seems very low, can you give some hint on this?
POSCONTROL_STOPPING_DIST_UP_MAX = 300cm
POSCONTROL_STOPPING_DIST_DOWN_MAX = 200cm
POSCONTROL_LEASH_LENGTH_MIN = 100cm
POSCONTROL_VEL_ERROR_CUTOFF_FREQ = 4hz
POSCONTROL_THROTTLE_CUTOFF_FREQ = 2hz
POSCONTROL_ACCEL_FILTER_HZ = 2hz
POSCONTROL_JERK_RATIO = 1

Leonardthall · September 3, 2019, 2:45pm

With all those messages the logs have gaps, even if I increase LOG_FILE_BUFSIZE.
And I think that there are artifacts with the frequency of the logs and the frequency of the commands.

If you put a fast SD card into the autopilot then you won’t have a problem.

Ok, so I can confirm that there is no delay in the AltHold reaction. However I can see that the guided messages coming in are not smooth and this is causing spikes in the Altitude control.

and 3. I am sorry but I don’t know what is happening here.
Yes, this will include the correction to the position error.
Yes you may but you will see little impact going over 5 Hz because that is already a pretty quick response. So for the most part you will be increasing noise.
for a small quad there may be some small increase in speed but it will be minor.

I am sorry that I am not able to help you out too much here as guided is one of the few modes I did not write the control interface for. It is on my list of things to rewrite but time is the key.

Your guided inputs appear to freeze for sample at a time and this adds to the noise on Alt Hold. I would ensure your commands are as good as possible.

I am confused as to why the overshoot is happening. It may be caused by I term build up but I don’t get to see the PID loops as they run in the position controller. You could try dropping it by a factor of 10 and seeing if it makes any difference. There is a chance that this is something to do with the way guided gives the commands but I would need to go over the code in detail and I can’t do that right now.

I am sorry I can’t help more.

guglie · September 17, 2019, 1:26am

Thanks @Leonardthall, I’ve fixed that.
Here are the resulting logs:

What do you think about it? The delay / problems are still there:

Vertical

Horizontal

Thanks, ok for PSC_VELXY_D_FILT and PSC_VELXY_FILT that are already at 5. Raising PSC_ACC_XY_FILT from 2 to 5 can be a good idea?
PSC_ACCZ_FILT is already a lot higher by default, 20hz. Why? How I can tune it?

Ok I’ll make a test doubling these
POSCONTROL_VEL_ERROR_CUTOFF_FREQ = 4hz
POSCONTROL_THROTTLE_CUTOFF_FREQ = 2hz
POSCONTROL_ACCEL_FILTER_HZ = 2hz

What about POSCONTROL_JERK_RATIO ?

Maybe I can ask these questions to @rmackay9:

Why the horizontal brake moves the drone back so much?
What are those little spikes in PSC.TPY that make it different from GUID.pY?

Leonardthall · September 21, 2019, 2:33pm

As I said before. With the vertical axis of the position controller we expect a following error. All you can do is reduce it but you can’t remove it. When I rewrite the vertical controller it will look more like the xy controller where the lines are on top of each other.

On the position controller side of things there are parts of this log that don’t make sense to me. It looks like you are using custom code. What changes have you made. Please provide a link to your repo.