Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

For discussion - use of noise-robust differentiators for DTerm in PIDs #2087

Closed
wants to merge 12 commits into from

Conversation

@martinbudden
Copy link
Contributor

martinbudden commented Apr 26, 2016

For discussion I've implemented a set of noise-robust differentiators for the DTerm.

These were formulated by Pavel Holoborodko and are described at http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/

Essentially these combine differentiation with noise suppression and zero time lag in a computationally efficient manner.

Of course the only way to see if they are any good for DTerm calculation is to actually try them out. @digitalentity is already using a variant in iNav.

@borisbstyle , you may be interested in these for Betaflight.
@ledvinap , @hydra , @joshuabardwell , I'd certainly value your thoughts on these.

The differentiators are configurable by filter length, from N=1 which is just ordinary numeric differentiation to N=5.

Cleanflight necessarily uses one-sided filters, and these have some disadvantages compared with centred filters, in that they need to be longer and they amplify noise in the "mid-frequency range". How that effects Cleanflight is to be seen.

The coefficients are customisable to give different filter characteristics and Pavel has given some indication on his web page that he might be willing to help with such customisation, but I haven't contacted him yet.

@martinbudden martinbudden force-pushed the martinbudden:pid_robust branch from 5a1f75b to 56503ea Apr 26, 2016
@digitalentity

This comment has been minimized.

Copy link
Contributor

digitalentity commented Apr 26, 2016

I'm really happy how Pavel Holoborodko's formula works. D-term is now totally noise-free and can be used without any filtering (and essentially without delay).

@martinbudden martinbudden force-pushed the martinbudden:pid_robust branch from 56503ea to 445718e Apr 27, 2016
@ledvinap

This comment has been minimized.

Copy link
Contributor

ledvinap commented Apr 27, 2016

Yes, this may be great improvement.

I would prefer to implement differentiation as generic filter. Both old averaging code and Pavels are FIR filters, so why not implement as one. Computing requirements are quite low (at least for single-digit tap count) and it will greatly simplify testing new ideas (Sawitsky-Golay is one alternative).
Biquad and PT1 is IIR, but we may optionally include one biquad stage (with small overhead for PT1 - set unused coefficients to 1).

Making the code generic and mathematically correct will IMO help a lot in long term.

@ledvinap

This comment has been minimized.

Copy link
Contributor

ledvinap commented Apr 27, 2016

BTW: old averaging filter ( [1/3 0 0 -1/3] ) has notch in transfer frequency at 0.33 Fs (330Hz). This is close to common motor frequency, so under right conditions it may work better than smooth Holobrodko filters. Savitzky–Golay filter may have similar advantage (trading smoothness for stopband performance)

@borisbstyle

This comment has been minimized.

Copy link
Member

borisbstyle commented Apr 27, 2016

I will have a look and do some testing with it. It is a nice research project.

I would also rather see this wrapped in a function with other filters than in the pid controller itself, but lets test it first.

@martinbudden

This comment has been minimized.

Copy link
Contributor Author

martinbudden commented Apr 27, 2016

OK, I've updated this to use a FIR filter for the differentiation. The filter implementation is a bit "quick and dirty" and could certainly do with tidying up, but the overall principle is demonstrated.

// N=6: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4
// N=7: h[0] = 7/32, h[-1] = 1/2, h[-2] = -1/32, h[-3] = -3/4, h[-4] = -11/32, h[-5] = 1/4, h[-6] = 5/32
// first coefficient is the divisor
static const int nrdCoefficents2[] = { 1, 1, -1};

This comment has been minimized.

Copy link
@ledvinap

ledvinap Apr 27, 2016

Contributor

Int has to be converted to float anyway, so store coefs as floats now
Also scaling may be applied to coefs directly, no need for division (and expensive float division)

(edited)

@@ -689,7 +689,9 @@ const clivalue_t valueTable[] = {
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { PID_MIN, PID_MAX } , PG_PID_PROFILE, offsetof(pidProfile_t, D8[PIDVEL])},

{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } , PG_PID_PROFILE, offsetof(pidProfile_t, yaw_p_limit)},
{ "dterm_cut_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = {0, 500 } , PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_hz)},
{ "dterm_lowpass_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 } , PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lpf_hz)},
{ "dterm_nrd", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, PID_MAX_NRD } , PG_PID_PROFILE, offsetof(pidProfile_t, dterm_noise_robust_differentiator)},

This comment has been minimized.

Copy link
@borisbstyle

borisbstyle Apr 27, 2016

Member

nrd is kind of hard to understand IMO. Why not dterm_ndifferentiator or something like that?

// Calculate derivative using 5-point noise-robust differentiator without time delay (one-sided or forward filters)
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
// h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
delta = 5*gyroRate + 2*lastRate[axis][0] - 8*lastRate[axis][1] - 2*lastRate[axis][2] + 3*lastRate[axis][3];

This comment has been minimized.

Copy link
@borisbstyle

borisbstyle Apr 27, 2016

Member

So rewrite is not configurable. It is hardcoded to 5point differentiator? Why the choice for that?

if (pidProfile->dterm_lpf_hz) {
// Dterm low pass filter
DTerm = delta * 3; // Keep same scaling as unfiltered DTerm
#ifdef USE_PID_BIQUAD_FILTER

This comment has been minimized.

Copy link
@borisbstyle

borisbstyle Apr 27, 2016

Member

IMO biquad for dterm can fully be removed. It is too much and it easily puts D and P out of phase. pt1 is a 1 point filter which is good enough in case when filter wanted

This comment has been minimized.

Copy link
@ledvinap

ledvinap Apr 27, 2016

Contributor

2nd order filter (biquad) with double bandwidth may be better then first order one and delay will be similar ...

int32_t unittest_pidMultiWiiRewriteCore_PTerm[3];
int32_t unittest_pidMultiWiiRewriteCore_ITerm[3];
int32_t unittest_pidMultiWiiRewriteCore_DTerm[3];
}

static const float luxPTermScale = 1.0f / 128;
static const float luxITermScale = 1000000.0f / 0x1000000;
static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 256;
static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 512;

This comment has been minimized.

Copy link
@borisbstyle

borisbstyle Apr 27, 2016

Member

Yes I meant 512 indeed the last time :D

@martinbudden

This comment has been minimized.

Copy link
Contributor Author

martinbudden commented Apr 27, 2016

Updated as per suggestions.

@borisbstyle , the only reason rewrite isn't configurable is that I haven't done it yet. I generally find the best coding approach is "deep then wide" - i.e. go deeply into a particular instance of a problem so that the problem is well understood and then generalise to other instances. In this case that means getting luxfloat fully working and then pulling the code over into rewrite (which should be fairly simple).

@borisbstyle

This comment has been minimized.

Copy link
Member

borisbstyle commented Apr 27, 2016

@martinbudden
Yes of course. I will test this soon and report back what I see compared to other approaches.

Also I would propose to ditch the MW23 as the defaults don't match it. Only thing you lose is the multiwii "super expo" implementation. I or even you can simply add a PR where super expo can be a mode in rewrite and luxfloat.
There are no further differences in MW23 pid controller.

@martinbudden

This comment has been minimized.

Copy link
Contributor Author

martinbudden commented Apr 27, 2016

@borisbstyle , I've been looking into the "super expo" implementation. One thing I'd like to do alongside that is improve the anti-windup protection. @digitalentity has a good implementation of anti-windup in iNav and I was planning on moving that over before adding in the "super expo" stuff.

I look forward to hearing the results of your flight testing.

@borisbstyle

This comment has been minimized.

Copy link
Member

borisbstyle commented Apr 27, 2016

@martinbudden
I changed mine too with something simple but solid. Check my latest commits

Super expo needs to ignore Iterm at certain point as the error is being manipulated otherwise it will wind up anyways.

@digitalentity

This comment has been minimized.

Copy link
Contributor

digitalentity commented Apr 27, 2016

@martinbudden, @borisbstyle
I'm not an acro pilot, but N=5 seems like an optimal choice. I don't feel a difference when changing from 5 to 7 or 8, but I feel noticable difference when changing from 3 to 5.

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Apr 28, 2016

I'm curious about phase shift when n=5, how much at what frequencies? e.g. at 50, 100, 150Hz?
Also the 'low pass' effect - Pavel's approach seems to be an efficient way of performing the equivalent of filtering then differentiating. Is there some kind of equivalent filtering frequency for the n=5 solution? It seems that functionally dterm should be responsive and without meaningful phase shift up to around 100Hz.

@ledvinap

This comment has been minimized.

Copy link
Contributor

ledvinap commented Apr 28, 2016

@ctzsnooze : Here is response and delay of all new fiters:
diff-magnitude
diff-groupdelay

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Apr 28, 2016

Thank you! Wow! n=5 does seem ideal for us.
By100Hz there is very littlephase shift, maybe only 10-15 degrees, nearly perfect linear reproduction of dTerm to that point, and group delay of no consequence. Awesome!
After 100Hz maybe phase shift becomes an issue - at 200Hz maybe 70 degrees. And inherent dTerm gain continues up to about 250Hz, rather high.
On that basis would it be fair to say that with this code, the incoming gyro data should be LPF filtered at 80 - 100Hz, in practice, to reduce excessive dTerm gain on motor noise frequencies > 160-300, and to keep to keep derived Pterm and Dterm in phase?

@digitalentity

This comment has been minimized.

Copy link
Contributor

digitalentity commented Apr 28, 2016

I did empirical tests when choosing the length in INAV's PID controller. I also concluded N=5 to be optimal. Lower doesn't have enough noise suppression, higher gives significant delay in D-term that was causing oscillations.

@ledvinap

This comment has been minimized.

Copy link
Contributor

ledvinap commented Apr 28, 2016

@ctzsnooze : It's not important where you filter the data, all filter will introduce delay and phase shift.
Using common filter has some performance advantage, but otherwise the filter may be merged into differentiator.

Here is Savitzky-Golay filter added for comparison (I can synthesize them easily - http://www.mathworks.com/matlabcentral/fileexchange/30299-savitzky-golay-smooth-differentiation-filters-and-filter-application/content/savitzkyGolay.m) , with zero group delay (at DC) and 2 sample delay (-2 variant). The cost of zero group delay at DC considerable (effect on Holoborodko's differentiators will be similar. It is equivalent to cascade of filter (with positive DC delay) and (~ high pass) filter with negative group delay). No free lunch here ...

diff-magnitude-2
diff-groupdelay-2

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Apr 28, 2016

The new proposal looks much better! :-)
Out of curiosity how would comparable graphs of our current delta calculation followed by say current IIR dTerm filtering at say 100Hz?

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Apr 28, 2016

I'm trying to translate the 'group delay in samples' concept to 'functional delay in seconds'... could you assist with that?
I guess this depends on how many loops are needed to get enough samples to compute the output? If 5 samples are needed, and group delay is close to 1 sample at 100Hz, does that sort of mean the 'functional' time delay can be through of as being around 5ms - or to ask differently, how would the group delay numbers above compare to a simple classical RC filter time constant?
Also... I'm curious about what happens to the frequency response / phase shift if loop time changes? Current delta calculations are loop time (well, target loop time) compensated. Given the variability in user's set loop time, we need something either loop time compensated or loop time independent.

@martinbudden

This comment has been minimized.

Copy link
Contributor Author

martinbudden commented Apr 28, 2016

@ctzsnooze , I think talking about delta calculations being loop time compensated is an incorrect way of looking at things and is probably a hangover form MultiiWii which just did it incorrectly. The DTerm is a differential and so by definition is loop time independent. The basic computations this: we measure the change in gyro rate (dR) by the calculation:

dR = gyroRate - lastGyroRate

then we divide by dT to get the derivative dR/dT

Of course the overall PID (i.e. PTerm + ITerm + DTerm) is not normalised for the loop time, so changing the loop time may require retuning, and I think there is a case for normalising the overall PID - something for a later day though.

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Apr 28, 2016

OK that's true - sorry! Yes, there's no 'delay' in our current dTerm calculation. However we currently typically use dTerm filtering - an IIR - to ameliorate high frequency noise amplitude gain of the current dTerm calculation. This filter changes frequency response and introduces phase delay. I could be wrong. I was curious to see graphs of how the current arrangement - set to a typical or comparable value - compares to the newer dTerm calculation.
But then I started thinking about the influence of loop time on the existing filter vs the newer proposal for dTerm (which functionally includes a filter). Like how does changing the loop time influence actual cutoff frequencies, delay and/or phase shift.

@martinbudden

This comment has been minimized.

Copy link
Contributor Author

martinbudden commented Apr 28, 2016

@ledvinap , Pavel Holoborodko provided two sets of filter coefficients:

  1. "Precise on 1, x", that is gives exact differentials for constant and linear functions.
  2. "Precise on 1, x, x^2", that is gives exact differentials for constant, linear and quadratic functions

I, somewhat arbitrarily, used set two. The coefficients for set one are:

1/4  (h[0] = 1, h[-1] = 1, h[-2] =-1, h[-3] =-1)
1/8  (h[0] = 1, h[-1] = 2, h[-2] = 0, h[-3] =-2, h[-4] =-1)
1/16 (h[0] = 1, h[-1] = 3, h[-2] = 2, h[-3] =-2, h[-4] =-3, h[-5] =-1)
1/32 (h[0] = 1, h[-1] = 4, h[-2] = 5, h[-3] = 0, h[-4] =-5, h[-5] =-4, h[-6] =-1)
1/64 (h[0] = 1, h[-1] = 5, h[-2] = 9, h[-3] = 5, h[-4] =-5, h[-5] =-9, h[-6] =-5, h[-7] =-1)

Can you do your graphical magic with these coefficients to see how they compare with the current ones?

@martinbudden

This comment has been minimized.

Copy link
Contributor Author

martinbudden commented Apr 28, 2016

@ctzsnooze , my apologies if I was over-pedantic. Continuing in that vein, there actually is a delay in the current raw DTerm calculation, because we calculate dR = gyroRate - lastGyroRate we are actually approximating the derivative at a time dT/2 in the past, so there is a delay of half a loop time. Note that the n=2 case is equivalent to simple differentiation and you can see this delay of 0.5 in @ledvinap 's graphs.

I'm not sure how much delay the biquad filter currently used introduces, but it is simple to work out how much delay the moving average filter introduces - it's half the length of the moving average, so an 8 point moving average will introduce a delay of 4*dT. Note that the filter with N=3 is equivalent to a simple 2 point moving average and introduces a delay of 1, again as seen in @ledvinap 's graphs.

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented Apr 28, 2016

It's strange that for n=3 the delay is fixed regardless of frequency but for n=5 it varies?

Betaflight currently uses a simple IIR, not a biquad filter, for filtering calculated d_term. That's why I was curious to see graphs of the current vs proposed solution, and to know how loop time affected both.

@ledvinap

This comment has been minimized.

Copy link
Contributor

ledvinap commented Apr 28, 2016

@ctzsnooze : All graphs are based on 1kHz sampling frequency; 1 sample is 1ms for group delay.

For symmetrical FIR filter group delay is constant and is directly delay of filter.
Zero group delay at DC is interesting, because for linear slope there will be no delay (except where changing direction).

You should try playing with Octave (or Matlab) a bit ...

@martinbudden

This comment has been minimized.

Copy link
Contributor Author

martinbudden commented May 3, 2016

@ctzsnooze , we seem to have come full circle on using the biquad filter for DTerm. We started with the pT1 filter because @borisbstyle was using that in betaflight, having previously tried biquad. Then at your suggestion we stared looking at biquad, but now we've seen the graphs, the consensus seems to be going against using the biquad filter for DTerm. Would you agree?

@borisbstyle

This comment has been minimized.

Copy link
Member

borisbstyle commented May 3, 2016

I vote for IIR. Did some more testing today and it indeed seemed better.

Besides that all these ESC firmwares are getting better too and have their filtering.

For example I tried the latest blheli 14.5 and it was night and day difference with previous versions.

@ctzsnooze

This comment has been minimized.

Copy link
Contributor

ctzsnooze commented May 4, 2016

Yep, the first order IIR to linearise dTerm should be all that's needed. I'd the cutoff relatively high, somewhere above software gyro biquad, like maybe 50% higher just for testing.

As Boris points out, since motors can't respond to very high frequencies anyway, our main concern with very high frequency noise really is aliasing causing folding down to frequencies that they actually can respond to. 8k sampling and 8k loop times seem to be the solution there, and the biquad seems ideal the primary gyro filter in that it doesn't seem to introduce significant aliasing and is quite steep.

I flew yesterday with gyro software biquad filter at 70 and IIR on dTerm at 105 on two different power setups, 8k8k with LB30 multishot RS2205 2600's on 4S, and 8k4k LB20 SS2204 2300's on the other, both multishot, wow, talk about smooth! Motors cool as could possibly be, flight responsiveness great. Multishot is way smoother (haven't used BLHeli 14.5 yet). And agree entirely with Boris that ESC developments are very positive. I think I could set filters higher still without problems, given how smooth these ESCs are getting and that dTerm is now working properly since it has much less relative phase shift.

@martinbudden

This comment has been minimized.

Copy link
Contributor Author

martinbudden commented May 4, 2016

@borisbstyle , @ctzsnooze , OK, I'll ditch the option to set a biquad filter on DTerm.

To be clear, is the filter you guys are using on the DTerm the pt1 filter, or have you implemented a different filter?

And presumably no-one is using the moving average?

As for defaults, I'm inclined to set them quite high, say:

  1. soft_gyro_lpf_hz = 80
  2. dterm_lpf_hz = 110

Does that seem reasonable?

@borisbstyle

This comment has been minimized.

Copy link
Member

borisbstyle commented May 4, 2016

@martinbudden
Yes just remove anything else and only keep IIR in there. Even averaging can be removed.

I did quite some testing last week with different scenarios and IIR was indeed by far giving the best tune / P behaviour.

I am not sure about the cutoffs yet and especially 110hz on dterm. I am still experimenting with that on different copters.
So far 80 for gyro and 70 for dterm has served me well, but will try to find the limits.

I also suggest changing names to soft_gyro_lowpass and dterm_lowpass.

I also noticed that Yaw axis is sensitive to noise on P. That is because yaw P is much higher than Roll and pitch and you get more noise amplification. Besides that yaw is much more active anyway.
I suggest adding a yaw_lowpass, which just fitlers the yaw P or pid_sum.

@martinbudden martinbudden force-pushed the martinbudden:pid_robust branch 5 times, most recently from 2f01dac to 12e4e35 May 6, 2016
@martinbudden martinbudden force-pushed the martinbudden:pid_robust branch from 12e4e35 to 74b7af6 May 10, 2016
@martinbudden

This comment has been minimized.

Copy link
Contributor Author

martinbudden commented Sep 28, 2016

Closing this as discussion has run its course.

martinbudden pushed a commit to martinbudden/cleanflight that referenced this pull request Jan 12, 2017
Removed VCP default to using second serial (UART1 in most cases) to MSP
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
5 participants
You can’t perform that action at this time.