# Quadcopter PID Controller Implementation - Derivative Filter

15 views (last 30 days)

Show older comments

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!

##### 0 Comments

### Answers (2)

John Petersen
on 11 Dec 2014

Arkadiy Turevskiy
on 11 Dec 2014

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;

### See Also

### Products

### Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!