Improving controller

- Adding constrain to integrator part of the PID
- Resetting the PID integral value when altitude command is zero
- Adding a low-pass filter before the derivative of the controller