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?