Kp_y = 1;

Kv_y = 0;
Kp_z = 30;
Kv_z = 0;
Kp_phi = 1;
Kv_phi = 0;

phi_c = -(1/params.gravity)*(Kv_y * (-state.vel(1)) + Kp_y * (des_state.pos(1) -

state.pos(1)));

phi_c_dot = 0;
% phi_c_ddot = 0;

u1 = params.mass * (params.gravity - Kv_z * state.vel(2) + Kp_z * (des_state.pos(2)

- state.pos(2)));
% u2 = params.Ixx * (phi_c_ddot + Kv_phi * (phi_c_dot - state.rot) + Kp_phi *
(phi_c - state.omega));

u2 = Kv_phi * (phi_c_dot - state.rot) + Kp_phi * (phi_c - state.omega);

-----------------------------------------------------------------------------------
-------------------------

Kp_y = 10;
Kv_y = 0;
Kp_z = 2000;
Kv_z = 0;
Kp_phi = 2000;
Kv_phi = 0;

phi_c = -(1/params.gravity)*(Kv_y * (-state.vel(1)) + Kp_y * (des_state.pos(1) -

state.pos(1)));

phi_c_dot = 0;
phi_c_ddot = 0;

u1 = params.mass * (params.gravity - Kv_z * state.vel(2) + Kp_z * (des_state.pos(2)

- state.pos(2)));
u2 = params.Ixx * (phi_c_ddot + Kv_phi * (phi_c_dot - state.rot) + Kp_phi * (phi_c
- state.omega));

-------------------------------------------

Parece que este s� funciona:

% u1 = 0;
% u2 = 0;

% FILL IN YOUR CODE HERE

% Kp_phi = 1000;
% Kv_phi = 0;
% Kp_z = 2000;
% Kv_z = 0;
% Kp_y = 10;
% Kv_y = 0;
Kp_phi = 1000;
Kv_phi = 0;
Kp_z = 2000;
Kv_z = 0;
Kp_y = 10;
Kv_y = 0;

phi_c = -(1/params.gravity)*(des_state.acc(1) + Kv_y *(des_state.vel(1) -

state.vel(1)) + Kp_y *(des_state.pos(1) - state.pos(1)));
phi_c_dot = 0;
phi_c_ddot = 0;

u1 = params.mass*(params.gravity + des_state.acc(2) + Kv_z*(des_state.vel(2) -

state.vel(2)) + Kp_z*(des_state.pos(2) - state.pos(2)));
u2 = params.Ixx * (phi_c_ddot + Kv_phi*(phi_c_dot - state.omega) + Kp_phi*(phi_c -
state.rot));

end