Recently, I have been working on a flight control board for my Quadcopter to learn about control system. The end goal is to be able to send a coordinate and have the Quadcopter fly there autonomously. I did some simulation in Simulink with Cascade PID Controller, which work pretty well.

However, when I start implementing it, just attitude control at the moment, I start running into all sort of troubles. Mainly time delay and noise.

So here's a video of what I got so far. Sorry for the quality, crappy $29 phone from Walmart on Black Friday lol.

In the video, I implemented a basic PID Controller. However, instead of taking the derivative of the error, I use the negative derivative of the process variable (angle) instead, assuming that the target angle should be relatively constant. This way, I can by pass the exploding noise issue (divide by small time) by using the IMU angular rate data.

I didn't really make an online tuner, so the video shown is just some rough tuning. Mainly I have trouble with the derivative term. I need it to dampen out the Proportional term. However, by using high derivative gain, the system oscillate and sometime looses control. Using lower derivative gain, the system still oscillate, by more like swinging around as shown in the video.

So I think I need to do a cascade control here by having one PID control the angular rate, then another to control the angle. However, that mean I must use the real derivative term of (error - error_old) / (time - time_old). Since the control loop run at a relatively high frequency, around 450 Hz, any noise in the error will increase drastically by dividing a small value. Therefore, I think I need a derivative filter.

I did some research and found that a first order low pass filter is usually what people use. However, I don't really understand how it work and how to implement that. So here's what MATLAB use in its PID block.

From this, it look like they never actually take the derivative of the error signal. I assume that's part of the filtering? Or the filter cancel that s block out?

Anyway, I tried to implement this on the attitude controller so that I have something to compare. I use both MATLAB's auto-tune calculated value and also tried some manual tuning, it's nothing even close at all. I mean it make things a lot worse. The quadcopter just simply uncontrollable.

Here's the code I used for the derivative action:

```
// 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);
```

I tried using the negative angular rate instead of the error, it's better in the sense that at least it is controllable. But not much improvement compare to the one without the filter.

Any suggestions will be greatly appreciated!