This paper deals with the under-actuated characteristic of a quad-rotor unmanned aerial vehicle (UAV). By designing the double loop configuration, the autonomous trajectory tracking is realized. The model uncertainty, external disturbance and the senor noise are also taken into consideration. Then the controller is put forward in the inner loop. An optimal stability augmentation control (SAC) method is used to stabilize the horizon position and keep it away from oscillation. By calculating the nonlinear decouple map, control quantity is converted to the speeds of the four rotors. At last some simulation results and the prototype implementation prove that the control method is effective.