Asked by Yu Hin Hau
on 11 Dec 2014

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 by John Petersen
on 11 Dec 2014

Yu Hin Hau
on 11 Dec 2014

Sign in to comment.

Answer by 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;

Yu Hin Hau
on 11 Dec 2014

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...

Sign in to comment.

Opportunities for recent engineering grads.

Apply Today
## 0 Comments

Sign in to comment.