While tuning PID, we found that our system only maintain an update rate about 30Hz, Which is a little bit not enough.
The first rule of performance tuning: find out where the time is spent. Actually this rule is also rules 2,3,4,5,6,7,8,9, etc.
Do not guess! Human intuition on computer performance is very unreliable. Measure, measure, measure!
For the Arduino simply have a few long variables, e.g. loopStart, loopMid, LoopWhatever, .,,.
Assign loopStart = micros();
and so forth. At the end of the loop print the values (not in the loop!).
By this method you will find the actual bottleneck - not somebody's guess.
I did performance tuning for a living, I ALWAYS start with rule #1.