Quadcopter PID Controller Implementation - Derivative Filter

Hi,

I have been working on UAV project for the past several months. I built the model in Simulink using Cascade PID Control, which works pretty well in simulation. However, upon implementation, I am facing several problems. Primary with the derivative action.

Here's a video of what I got so far after coding everything in an Arduino Due. Instead of using just a normal PID control on the angle, I replace the derivative term with the negative derivative of the process variable. That way, I can just use the Euler Angle Rate instead of calculating the rate and face the exploding noise problem by dividing by a small time step. I was not able to use the gain as used in Simulink because the system get too oscillatory. https://www.youtube.com/watch?v=6O_xMvynXNs

As seen in the video, the system is not really responding well. Therefore, I think I need to do a Cascade Control with one on the Angle Rate and another on the Angle. But to do that I need to calculate the error derivative, which is very noisy. So I need a derivative filter.

Here's the Simulink's PID Block. I don't really understand how the derivative term work since the derivative is never really calculated. Instead an integration term is used instead as part of the filter?

Anyway, I tried implementing that part at the Angle level so I have something to compare to. The code is as such:

// Single Pole Derivative Filter
filter_integral += 0.5*((filter_gain * Kd*error) + filter_old)*dt;

// PID Output
return Kp*error + Ki*integral + filter_gain*(Kd*error - filter_integral);

But that don't really work at all. The Derivative term is very unstable and the quadcopter became uncontrollable.

Can someone explain to me what I did wrong?

Thanks!

Answer 1

[John Petersen]

Apply your derivative gain to the angle rate error and the proportional gain to the angle error. Angle rate error would be (desired angle rate - actual angle rate measurement) Angle error would be (desired angle - actual angle measurement)

[Yu Hin Hau]

So that would be a PIP' Controller right? I did that before I sub in the negative angle rate for the derivative term. The performance in both are similar, at least in testing... it's not oscillating, but it's not very responsive like in the video...

Answer 2

[Arkadiy Turevskiy]

As you say yourself, calculating pure derivative of a noisy signal is not a good idea.

Transfer function of a pure derivative is s .

Simulink implementation you show instead has transfer function (for derivative path) of : s*N/(s+N)

You can see that by deriving the transfer function for derivative path:

y=N(x-1/s*y)

y+N/s*y=Nx

y*(s+N)/s=Nx

y=s*N/(s+N) x

Here y is signal from "Filter Coefficient" block and x is the signal from derivative gain.

So instead of transfer function s , Simulink implements s*N/(s+N) , which is close to s for low frequencies, but is then filtering out noise at higher frequencies.

If you generate code from PID block, you get this (sample time is 0.01, chosen arbitrarily for this example):

FilterCoefficient = (Kd * error - Filter_DSTATE) * N;
Out = (Kp * error + Integrator_DSTATE) + FilterCoefficient;
Integrator_DSTATE += Ki * error * 0.01;
Filter_DSTATE += 0.01 * FilterCoefficient;

[Yu Hin Hau]

OH okay, now I see where that transfer function came from. So effectively, we are adding a pole to the derivative and changing the gain?

I don't get how to write the code for -> s*N/(s+N), the block diagram format seems to make more sense intuitively.

Anyway, isn't the code I posted and yours pretty much equivalent? Except I use non-constant sample time, trapezoidal rule for integration and doing the Filter_DSTATE before the FilterCoefficient.

So if I implemented that derivative filter correctly, then the problem would probably on tuning the N term? How do I manually tune that? Or might be the EKF on the IMU smoothing by also delaying the data? But I receive data at around 450 Hz...