function xaugdot = QuadrotorSim(t,xaug,yref,Omegatrim, K, quad, wind) %QUADROTORSIM % xdot = QuadrotorSim(t, xaug, yref, Omegatrim, K, quad, wind) % Returns the state derivatives for a quadrotor with % a state feedback controller K with a P+I structure (LQI) % % Inputs % t = time (not used) % xaug = augmented state vector [xe ye ze u v w phi theta psi p q r eix eiy eiz eipsi] % yref = output reference [x,y,z psi] % Omegatrim = control trim values % K = state feedback controller % quad = vehicle coefficients structure (see Coefficients.m) % wind = wind disturbance vector % % Control equations: % int y_e dt = [eix eiy eiz eipsi] : error integrals % y_e = yref-y; : output error % K = [K1, K2]; : partitioned controlle [P, I] % u = -K1 x + K2 int y_e dt : LQI Controller % y_e = yref-y : output error % % (c) James Whidborne % Cranfield University, 19 October 2020 %t=t % uncomment to track time x = xaug(1:12); % state of quadrotor psi = x(9); % yaw angle % rotate x & y error and integrals to the body axes (not used) %Rpsi=[ cos(psi), sin(psi); % (not used) % -sin(psi), cos(psi)]; Rotatex=eye(16); %Rotatex(1:2,1:2)=Rpsi; Rotatex(13:14,13:14)=Rpsi; % (not used) %% calculate control y_e = yref-[x(1:3);psi]; % output error u = -K*Rotatex*xaug; % control u Omega = quad.CA*u + Omegatrim; % add trim and allocate to rotor speeds %% apply to quadrotor model xdot = QuadrotorDynamics( x, Omega, quad, wind ); % state derivatives xaugdot = [xdot; y_e]; % augment with output error error end