I_s = [cos(alpha1(1))^2 sin(alpha1(1))^2 -sin(2*alpha1(1));...
sin(alpha1(1))^2 cos(alpha1(1))^2 sin(2*alpha1(1));...
sin(2*alpha1(1))/2 -sin(2*alpha1(1))/2 cos(2*alpha1(1)) ]*[Ixx_b(1,:);Izz_b(1,:);Ixz_b(1,:)];
Y_b = qbar(1)*S*c_y_b(1)/m(1);
Y_p = qbar(1)*S*b*c_y_p(1)/(2*m(1)*U1(1));
Y_r = qbar(1)*S*b*c_y_r(1)/(2*m(1)*U1(1));
Y_da = qbar(1)*S*c_y_da(1)/m(1);
Y_dr = qbar(1)*S*c_y_dr(1)/m(1);
L_b = qbar(1)*S*b*c_l_b(1)/Ixx(1);
L_p = qbar(1)*S*b*b*c_l_p(1)/(2*U1(1)*Ixx(1));
L_r = qbar(1)*S*b*b*c_l_r(1)/(2*U1(1)*Ixx(1));
L_da = qbar(1)*S*b*c_l_da(1)/Ixx(1);
L_dr = qbar(1)*S*b*c_l_dr(1)/Ixx(1);
N_b = qbar(1)*S*b*c_n_b(1)/Izz(1);
N_Tb = qbar(1)*S*b*c_n_Tb(1)/Izz(1);
N_p = qbar(1)*S*b*b*c_n_p(1)/(2*U1(1)*Izz(1));
N_r = qbar(1)*S*b*b*c_n_r(1)/(2*U1(1)*Izz(1));
N_da = qbar(1)*S*b*c_n_da(1)/Izz(1);
N_dr = qbar(1)*S*b*c_n_dr(1)/Izz(1);
A_lat = U1(1)*(1 - A1_bar*B1_bar);
B_lat = -Y_b*(1 - A1_bar*B1_bar) -U1(1)*(L_p + N_r + A1_bar*N_p +B1_bar*L_r);
C_lat = U1(1)*(L_p*N_r - L_r*N_p) + Y_b*(N_r + L_p + A1_bar*N_p + B1_bar*L_r) ...
-Y_p*(L_b + N_b*A1_bar + N_Tb*A1_bar) +U1(1)*(L_b*B1_bar + N_b + N_Tb) ...
-Y_r*(L_b*B1_bar + N_b + N_Tb);
D_lat = -Y_b*(L_p*N_r - L_r*N_p) + Y_p*(L_b*N_r - N_b*L_r -N_Tb*L_r) ...
-g*cos(theta1(1))*(L_b + N_b*A1_bar + N_Tb*A1_bar) + U1(1)*(L_b*N_p - N_b*L_p - N_Tb*L_p) ...
-Y_r*(L_b*N_p - N_b*L_p -N_Tb*L_p);
E_lat = g*cos(theta1(1))*(L_b*N_r - N_b*L_r -N_Tb*L_r);
Den_lat = [A_lat B_lat C_lat D_lat E_lat];
A_b = Y_d*(1 - A1_bar*B1_bar);
B_b = -Y_d*(N_r + L_p + A1_bar*N_p + B1_bar*L_r) + Y_p*(L_d + N_d*A1_bar) ...
+Y_r*(L_d*B1_bar + N_d) -U1(1)*(L_d*B1_bar + N_d);
C_b = Y_d*(L_p*N_r - N_p*L_r) +Y_p*(N_d*L_r - L_d*N_r) + g*cos(theta1(1))*(L_d + N_d*A1_bar) ...
+ Y_r*(L_d*N_p - N_d*L_p) -U1(1)*(L_d*N_p -N_d*L_p);
D_b = g*cos(theta1(1))*(N_d*L_r - L_d*N_r);
Num_b = [A_b B_b C_b D_b];
sys_beta = tf(Num_b,Den_lat);
A_phi = U1(1)*(L_d + N_d*A1_bar);
B_phi = U1(1)*(N_d*L_r -L_d*N_r) -Y_b*(L_d + N_d*A1_bar) + Y_d*(L_b + N_b*A1_bar + N_Tb*A1_bar);
C_phi = -Y_b*(N_d*L_r - L_d*N_r) +Y_d*(L_r*N_b + L_r*N_Tb -N_r*L_b) ...
+ (U1(1) - Y_r)*(N_b*L_d + N_Tb*L_d -L_b*N_d);
Num_phi = [A_phi B_phi C_phi];
sys_phi = tf(Num_phi,Den_lat);
A_psi = U1(1)*(N_d + L_d*B1_bar);
B_psi = U1(1)*(L_d*N_p -N_d*L_p) -Y_b*(N_d + L_d*B1_bar) + Y_d*(L_b*B1_bar + N_b + N_Tb);
C_psi = -Y_b*(L_d*N_p - N_d*L_p) + Y_p*(N_b*L_d +N_Tb*L_d -L_b*N_d) +Y_d*(L_b*N_p -N_b*L_p -N_Tb*L_p);
D_psi = g*cos(theta1(1))*(N_b*L_d + N_Tb*L_d - L_b*N_d);
Num_psi = [A_psi B_psi C_psi D_psi];
sys_psi = tf(Num_psi, Den_lat);
B_lat_dr = -(N_r + Y_b/U1(1));
C_lat_dr = N_b+ (Y_b*N_r - N_b*Y_r)/U1(1);
B_dr = N_b*Y_d - N_d*Y_b;
Den_dr = U1(1)*[A_lat_dr B_lat_dr C_lat_dr];
sys_dr = tf(Num_dr, Den_dr);
sideslip = lsim(sys_beta,u,t);
plot(t, sideslip,'LineWidth',1.5);
xlabel('time (s)','FontName','Palatino Linotype', 'FontSize', 14);
ylabel('sideslip, \beta, radians','FontName','Palatino Linotype', 'FontSize', 14)
title('Sideslip response to a rudder doublet','FontName','Palatino Linotype', 'FontSize', 14)
figure; plot(t,p,'LineWidth',1.5)
xlabel('time (s)','FontName','Palatino Linotype', 'FontSize', 14);
ylabel('roll rate, p (radians/s)','FontName','Palatino Linotype', 'FontSize', 14)
title('roll rate response to a rudder doublet','FontName','Palatino Linotype', 'FontSize', 14)
figure;plot(t,r,'LineWidth',1.5)
xlabel('time (s)','FontName','Palatino Linotype', 'FontSize', 14);
ylabel('yaw rate, r (radians/s)','FontName','Palatino Linotype', 'FontSize', 14)
title('yaw rate response to a rudder doublet','FontName','Palatino Linotype', 'FontSize', 14)
G_rudder = tf(Num_rudder,Den_rudder);
G_WO = tf(Num_WO, Den_WO)
G_forward = G_rudder*sys_psi;
figure;rlocus(G_forward*k_r*G_WO);
k = rlocfind(G_forward*k_r)
cltf = feedback(k*G_forward,k_r);
G_forward_dr = G_rudder*sys_dr
figure;rlocus(G_forward_dr*k_r)
k1 = rlocfind(G_forward_dr*k_r)
cltf1 = feedback(k1*G_forward_dr,k_r);