Quadcopter PID Controller Implementation - Derivative Filter
8 views (last 30 days)
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?
John Petersen on 11 Dec 2014
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)
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:
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;