ddq(1,1) =1/M*(cos(psi)*cos(theta)*Force_G_E1-sin(psi)*cos(theta)*Force_G_E2+sin(theta)*Force_G_E3); 
ddq(2,1) =-(-Force_G_E1*sin(psi)*cos(phi)-Force_G_E1*cos(psi)*sin(theta)*sin(phi)-Force_G_E2*cos(psi)*cos(phi)+Force_G_E2*sin(psi)*sin(theta)*sin(phi)+cos(theta)*sin(phi)*Force_G_E3)/M; 
ddq(3,1) =1/M*(Force_G_E1*sin(psi)*sin(phi)-Force_G_E1*cos(psi)*sin(theta)*cos(phi)+Force_G_E2*cos(psi)*sin(phi)+Force_G_E2*sin(psi)*sin(theta)*cos(phi)+cos(theta)*cos(phi)*Force_G_E3-M*g); 
ddq(4,1) =1/cos(theta)*(-Ix*dpsi*Iy*dtheta+Iy^2*cos(psi)^2*dpsi*dtheta-dpsi*Ix^2*cos(psi)^2*dtheta+sin(theta)*Ix^2*dphi*dtheta-Ix*dtheta*dpsi*Iz-Ix*Torque_G_E2*sin(psi)+dpsi*Ix^2*dtheta+cos(psi)*cos(theta)*sin(psi)*Iy*Iz*dphi^2*sin(theta)-Iy*cos(psi)^2*dtheta*Iz*dphi*sin(theta)+Ix*cos(psi)^2*dtheta*Iz*dphi*sin(theta)+sin(theta)*Ix^2*dphi^2*cos(psi)*cos(theta)*sin(psi)-cos(psi)*cos(theta)*sin(psi)*dpsi*Iy^2*dphi-cos(psi)*cos(theta)*sin(psi)*Iy^2*dphi^2*sin(theta)-Ix*dtheta*Iz*dphi*sin(theta)+Iy^2*cos(psi)^2*dtheta*dphi*sin(theta)-Iy*cos(psi)^2*dtheta*dpsi*Iz+Ix*dtheta*Iy*dphi*sin(theta)-dtheta*Ix^2*dphi*cos(psi)^2*sin(theta)+dpsi*Ix^2*dphi*cos(psi)*cos(theta)*sin(psi)+Ix*cos(psi)^2*dtheta*dpsi*Iz+cos(psi)*cos(theta)*sin(psi)*Iy*Iz*dphi*dpsi-cos(psi)*cos(theta)*sin(psi)*Ix*Iz*dphi*dpsi-cos(psi)*cos(theta)*sin(psi)*Ix*Iz*dphi^2*sin(theta)+cos(psi)*Iy*Torque_G_E1)/Iy/Ix; 
ddq(5,1) =-(-Iy*Iz*dphi^2*cos(theta)*sin(theta)-Iy*Iz*dphi*cos(theta)*dpsi-Ix*dpsi*Iy*dphi*cos(theta)-dpsi*Iy^2*dphi*cos(psi)^2*cos(theta)+Iz*Iy*dphi^2*cos(theta)*cos(psi)^2*sin(theta)+Iz*dpsi*Iy*dphi*cos(psi)^2*cos(theta)-Iy*Torque_G_E1*sin(psi)+Iy^2*dphi^2*cos(theta)*sin(theta)+dpsi*Iy^2*dphi*cos(theta)+cos(psi)*sin(psi)*sin(theta)*Ix^2*dphi*dtheta-cos(psi)*sin(psi)*Ix*dtheta*Iz*dphi*sin(theta)-Iz*dpsi*Ix*dtheta*cos(psi)*sin(psi)+cos(psi)*sin(psi)*Iy*dtheta*Iz*dphi*sin(theta)-cos(psi)*sin(psi)*dtheta*Iy^2*dphi*sin(theta)+Ix^2*dphi^2*cos(psi)^2*cos(theta)*sin(theta)-Ix*Torque_G_E2*cos(psi)+Iz*dpsi*Iy*dtheta*cos(psi)*sin(psi)-dpsi*Iy^2*dtheta*cos(psi)*sin(psi)+dpsi*Ix^2*dtheta*cos(psi)*sin(psi)-Iz*Ix*dphi^2*cos(psi)^2*cos(theta)*sin(theta)-Iz*dpsi*Ix*dphi*cos(psi)^2*cos(theta)+dpsi*Ix^2*dphi*cos(psi)^2*cos(theta)-Iy^2*dphi^2*cos(theta)*cos(psi)^2*sin(theta))/Iy/Ix; 
ddq(6,1) =-1/cos(theta)*(Iz*Ix^2*dphi*dtheta+Iz*Ix*Iy*dphi*dtheta-Iz^2*Ix*dphi*dtheta+Iz*Ix^2*dphi^2*cos(psi)*cos(theta)*sin(psi)-Iz*Ix^2*dphi*cos(psi)^2*dtheta+Iz^2*Ix*cos(psi)^2*dphi*dtheta+Iz*Iy^2*cos(psi)^2*dphi*dtheta-Iz^2*Iy*cos(psi)^2*dphi*dtheta-Iz*cos(theta)^3*Ix^2*dphi^2*cos(psi)*sin(psi)-Iz*cos(theta)^2*Ix^2*dphi*dtheta+cos(psi)*sin(theta)*Iz*Iy*Torque_G_E1+cos(psi)*cos(theta)*sin(psi)*Iz^2*Iy*dphi^2-cos(psi)*cos(theta)^3*sin(psi)*Iz^2*Iy*dphi^2+Iz^2*sin(theta)*Ix*cos(psi)^2*dtheta*dpsi+Iz*sin(theta)*dpsi*Ix^2*dphi*cos(psi)*cos(theta)*sin(psi)-Iz*sin(theta)*Ix*Torque_G_E2*sin(psi)-Iz^2*sin(theta)*Iy*cos(psi)^2*dtheta*dpsi+Iy^2*cos(theta)*Ix*dtheta^2*cos(psi)*sin(psi)-cos(psi)*cos(theta)*sin(psi)*Iz*Iy^2*dphi^2+cos(psi)*cos(theta)^3*sin(psi)*Iz*Iy^2*dphi^2-Iz*sin(theta)*dpsi*Ix^2*cos(psi)^2*dtheta+Iz*sin(theta)*Iy^2*cos(psi)^2*dpsi*dtheta-Iz*sin(theta)*Ix*dpsi*Iy*dtheta-Iz^2*sin(theta)*Ix*dtheta*dpsi-cos(psi)*cos(theta)*sin(psi)*Iz^2*Ix*dphi^2+cos(psi)*cos(theta)^3*sin(psi)*Iz^2*Ix*dphi^2-cos(psi)*cos(theta)*sin(psi)*sin(theta)*Iz*dpsi*Iy^2*dphi+Iz*sin(theta)*dpsi*Ix^2*dtheta-Iy*cos(theta)*Ix*Torque_G_E3-cos(psi)*cos(theta)*sin(psi)*sin(theta)*Iz^2*Ix*dphi*dpsi+cos(psi)*cos(theta)*sin(psi)*sin(theta)*Iz^2*Iy*dphi*dpsi+Iz^2*cos(theta)^2*Iy*cos(psi)^2*dphi*dtheta+Iz^2*cos(theta)^2*Ix*dphi*dtheta-Iz^2*cos(theta)^2*Ix*cos(psi)^2*dphi*dtheta-2*Ix^2*cos(psi)^2*cos(theta)^2*Iy*dphi*dtheta+Ix^2*cos(psi)^2*cos(theta)^2*Iz*dphi*dtheta+Iy*cos(theta)^3*Ix^2*dphi^2*cos(psi)*sin(psi)+Iy*cos(theta)^2*Ix^2*dphi*dtheta-Iy*cos(theta)*Ix^2*dtheta^2*cos(psi)*sin(psi)-Iy^2*cos(theta)^3*Ix*dphi^2*cos(psi)*sin(psi)-Iy^2*cos(theta)^2*Ix*dphi*dtheta+2*Iy^2*cos(theta)^2*Ix*dphi*cos(psi)^2*dtheta-Iy^2*cos(theta)^2*cos(psi)^2*Iz*dphi*dtheta)/Iz/Iy/Ix; 
