i wouldn't use float variables in this , which might slow things down .
Have you tried tuning the PID loop ( starting with D off ).
Why are you not using a bought controller ( quad) for this , they're cheap have all the connectors and work , with great configuration software. They tend to use 32bit processors too - which might , as you imply, suggest the Arduino is too slow for this task , and the source of jitter.
?