Yaw Controller High Pass Filter Discrepancy

[jmtobin-uh]

The current high pass filter in the yaw controller is calculated by the following (line 123): float rate_hp_out = 0.9960080f * _last_rate_hp_out + rate_hp_in - _last_rate_hp_in;

However, based on this equation (source: hpf wikipedia):

Shouldn't the alpha, in this case the constant 0.9960080 (how was this chosen?), be applied to all values, not simply the last rate output of the high pass filter? Making the line: float rate_hp_out = 0.9960080f *( _last_rate_hp_out + rate_hp_in - _last_rate_hp_in);

Version 4.1

[IamPete1]

The line is here: https://github.com/ArduPilot/ardupilot/blob/b0aa456daad5e732d9477481a055f5fa1acd8a77/libraries/APM_Control/AP_YawController.cpp#L119-L123

Your right in that it does not match the Wiki equation. Maybe @tridge or @priseborough will know if that was by design or mistake.

[lthall]

That eqation you gave is not correct. This is the correct equation: y[i] := y[i-1] + α * (x[i] - y[i-1])

[amilcarlucas]

y[i] := y[i-1] + α * (x[i] - y[i-1])

You certainly mean: y[i] := y[i-1] + α * (x[i] - x[i-1])

[lthall]

no: y[i] := y[i-1] + α * (x[i] - y[i-1])

This is basicly the last sample pluss a percentage of the error between our last sample and our current reading.

[jmtobin-uh]

Even so, neither of those represent what is happening in the actual code.

As it stands the variables are as follows:

y[i] = float rate_hp_out 
y[i-1] = _last_rate_hp_out
x[i] = rate_hp_in 
x[i-1] = _last_rate_hp_in;
a = 0.9960080

Currently, in the code we have, float rate_hp_out = 0.9960080f * _last_rate_hp_out + rate_hp_in - _last_rate_hp_in; which equates to, y[i] = a * y[i-1] + x[i] - x[i-1]

@lthall seems to be saying that with an equation of y[i] = y[i-1] + a * (x[i] - y[i-1]) we should see, float rate_hp_out = _last_rate_hp_out + 0.9960080f * (rate_hp_in - _last_rate_hp_out );

@amilcarlucas suggested y[i] = y[i-1] + a * (x[i] - x[i-1]) which would make the code, float rate_hp_out = _last_rate_hp_out +0.9960080f *( rate_hp_in - _last_rate_hp_in);

There still seems to be a issue here regardless of the suggested solution, unless what is written in the code already IS correct.

[lthall]

OOOOPPPPSSSSSSS, Sorry. High pass filter, not low pass filter! Oh my bad!!

y[i] := α × y[i−1] + α × (x[i] − x[i−1])

Very sorry everybody.

[jmtobin-uh]

No problem @lthall.

So for the current implementation, y[i] = a × y[i−1] + b × (x[i] − x[i−1])

a is 0.9960080 and b is 1?

[lthall]

I agree that it looks like the brackets are missing: y[i] := α × y[i−1] + α × (x[i] − x[i−1]) y[i] := α × (y[i−1] + (x[i] − x[i−1]))

float rate_hp_out = 0.9960080f _last_rate_hp_out + rate_hp_in - _last_rate_hp_in; float rate_hp_out = 0.9960080f ( _last_rate_hp_out + rate_hp_in - _last_rate_hp_in);

[jmtobin-uh]

Got it. Wont necessarily change much, but glad we found it.

[tridge]

I've assigned to @priseborough who wrote this code

[priseborough]

The difference is essentially one of the transformation used to go from S to Z domain. Both this and the suggested implementation give unity gain at the high frequency asymptote which is what is important. The difference is the crossover frequency. If we run a test using a sine input at the design cross over frequency fc = 1/(2PiTau) where Tau = (alpha * dt)/(1.0-alpha) for dt = 0.02 and alpha = 0.9960080 we get a time constant Tau of 4.99 seconds. From memory the alpha value was obtained using a numerical package for a cross over time constant of 5 seconds at 50Hz. Interestingly if a exponential transform is used, the value for alpha should be exp(-0.02/5.0) = 0.996.

Simulating these difference equations for the filter as implemented gives an attenuation at the cross-over frequency of 2.98dB which is close to the expected attenuation of 3dB.

If we make the change suggested by @lthall we get an attenuation of 3.02 dB so in terms of corner frequency accuracy, there is nothing to choose from between them.

The real issue is that the formulation assumes a 50Hz update rate and that is no longer a valid assumption for planes. I'll provide an update that corrects that assumption.