I'm trying to implement two PIDs for stabilizing quadrotor for position tracking. The inputs are $x_{d}(t), y_{d}(t), z_{d}(t)$ and $\psi_{d}(t)$. For position tracking, usually the small angle assumption is assumed. This assumption allows for acquiring $\theta_{d}$ and $\phi_{d}$. These are the results
The x-axis position is driving me crazy. After alot of attempts for tuning the PIDs, I felt something wrong is going on. Is this a normal behavior for PID controller? Also, what I've noticed is that once $\psi$ reaches to zero, the platform starts oscillating (after 1.5 second in the figure).
For solving ODEs and computing the derivatives for the velocities, I use Euler methods.
It is simulation in Matlab.