Quadcopter Aggressive Maneuvers along Singular Configurations: An Energy-Quaternion Based Approach

Algorithm 1

Constant thrust.

(1) Initialisation;

(2) ;

(3) ;           ⊳ Thrust

(4) ;        ⊳ time step

(5) ;     ⊳ -Array

(6) ;     ⊳ -Array

(7) ;

(8) ;

(9) ;

(10) ;

(11) ;

(12) ;

(13) ; ⊳ radius of curvature

(14) ;

(15) ;

(16) while   do

(17) ;

(18) ;

(19) ;

(20) ;

(21) ;

(22) ;

(23) ;

(24) ;

(25) ;

(26) ;

(27) end