It seems the current Kalman filter tuning of Q and R is insufficient and may be the source of control delays. This yields unstable or wobbly flight. Perform some tuning of these, perhaps assigning larger values to the Q's in relation to R's so that sensor input has quicker response to the estimated states.
But make sure the angles do not drift because of this. If they do, it is because the gyroscope/angular rate has too much influence on the angle through the prediction model.
It seems the current Kalman filter tuning of Q and R is insufficient and may be the source of control delays. This yields unstable or wobbly flight. Perform some tuning of these, perhaps assigning larger values to the Q's in relation to R's so that sensor input has quicker response to the estimated states.
But make sure the angles do not drift because of this. If they do, it is because the gyroscope/angular rate has too much influence on the angle through the prediction model.