%This script is modified version of EMA 523 class script to solve
%problem 5, HW5.
%step input in \psi_c and r_c and impulse in \psi_c and r_c
%are given. For each, response in v,p,r,\phi,\psi,del_a and del_r is
%plotted.
% EMA 523 Univ. Wisconsin, Madison, spring 2014
% modified by Nasser M. Abbasi

close all;
clear all;

v_cruise = 774;%cruise speed in fps
max_time = 40;
T        = 0:0.01:max_time; %time interval for plotting

%set up matrices%%%%%%%%%%%%%%%%%%%
A=[-0.0558 0.0 -774.0 32.20;
    -0.003865 -0.4342 0.4136 0.0;
    0.001086 -0.006112 -0.1458 0.0;
    0.0 1.0 0.0 0.0]

B=[0.0 5.642;
    -0.1431 0.1144;
    0.003741 -0.4859;
    0.0 0.0]

C=[1 0 0 0;
   0 1 0 0;
   0 0 1 0;
   0 0 0 1]
 
D=[0 0;
   0 0;
   0 0;
   0 0]

%---------- set up string arrays and cells
states='v p r \phi'
inputs='aileron rudder';
outputs='v p r \phi';

states_st={' v ';' p ';' r ';' \phi '};
inputs_st={' aileron ';' rudder '};
outputs_st=states_st;
 
%--------- display all matrices
printsys(A,B,C,D,inputs, outputs,states);
 
%------- SYS1
[A1,B1,C1,D1]=zp2ss(0,0,1)  %between zeta_c and zeta_c
sys1=ss(A1,B1,C1,D1,'inputname','\psi_c','outputname','\psi_c');

%-------- SYS2
[A2,B2,C2,D2]=zp2ss(0,0,2.5)  %between error_zeta and phi_c
sys2=ss(A2,B2,C2,D2,'inputname','e_\psi','outputname','\phi_c');

%-------- SYS 3, Roll-rate controller (Jp=SYS3)
K_p=1.5
[A3,B3,C3,D3]=zp2ss(0,0,K_p)
sys3=ss(A3,B3,C3,D3,'inputname','e_\phi','outputname','p_c')

%--------- SYS4
[A4,B4,C4,D4]=zp2ss(0,0,1)
sys4=ss(A4,B4,C4,D4,'inputname','r_c','outputname','r_c')

%--------- SYS 5, Aileron controller (Ja=SYS5)
tau_a  = 0.15
K_a    = -1.0
den_ja = [1 1./tau_a]
num_ja = [K_a/tau_a]
[A5,B5,C5,D5] = tf2ss(num_ja,den_ja)
sys5 = ss(A5,B5,C5,D5,'inputname','e_p','outputname','\delta_a')

%---------- SYS 6, Rudder controller (Jr=SYS6)
tau_r   = 0.3
K_r     = -1.6
den_jr  = [1 1./tau_r]
num_jr  = [K_r/tau_r]
[A6,B6,C6,D6] = tf2ss(num_jr,den_jr)
sys6 = ss(A6,B6,C6,D6,'inputname','e_r','outputname','\delta_r')

%---------- SYS 7
sys7 = ss(A,B,C,D,'inputname',{'\delta_a' '\delta_r'}, 'outputname',{'v' 'p' 'r' '\phi'})

%---------- SYS 8, Washout 
tau_wo = 4.0
[A8,B8,C8,D8] = zp2ss(0,-1/tau_wo,1)
sys8=ss(A8,B8,C8,D8,'inputname','r','outputname','r_f')

%---------- SYS 9, Integrator 
den_int = [1 0]
num_int = [1]
[A9,B9,C9,D9] = tf2ss(num_int,den_int)
sys9 = ss(A9,B9,C9,D9,'inputname','r','outputname','\psi')

sysa=append(sys1,sys2,sys3,sys4,sys5,sys6,sys7,sys8,sys9)

Q=[2 1 -12
   3 2 -10
   5 3 -8
   6 4 -11
   7 5 0
   8 6 0
   9 9 0
   10 9 0];

inputs=[1 4];
outputs=[5 6 7 8 9 10 12];

sys=connect(sysa,Q,inputs,outputs)

a=sys.A
b=sys.B
c=sys.C
d=sys.D

%set up string arrays and cells%%%%%%%%%
states_1='v p r \phi \psi';
inputs_1='\psi_c r_c';
outputs_1='\delta_a \delta_r v p r \phi \psi';

states_st_1={' \delta_a ';' \delta_r ';' \beta ';' p ';' r ';' \phi ';' \psi '};
inputs_st_1={' \psi_c ';' r_c '};
outputs_st_1=states_st_1;
%set up string arrays and cells%%%%%%%%%

% IMPULSE IN PSI_C
figure;
plot(T,impulse(a,b,c(1,:),d(1,:),1,T)*180/pi,'r','LineWidth',2);%*0.262);
title(strcat('1 radian impulse response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(1))], ']'));
axis([0 15 -1 60]);
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'impulse_psi_to_del_a', 'epsc');

figure;
plot(T,impulse(a,b,c(2,:),d(2,:),1,T)*180/pi,'r','LineWidth',2);%*0.262);
title(strcat('1 radian degree impulse response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(2))], ']'));
%axis([0 max_time -0.05 0.05])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'impulse_psi_to_del_r', 'epsc');


figure;
plot(T,impulse(a,b,c(3,:),d(3,:),1,T)/v_cruise*180/pi,'r','LineWidth',2);%*0.262/v_cruise);
title(strcat('1 radian degree impulse response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(3))], ']'));
%axis([0 max_time -0.03 0.05])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'impulse_psi_to_beta', 'epsc');


figure;
plot(T,impulse(a,b,c(4,:),d(4,:),1,T)*180/pi,'r','LineWidth',2);%*0.262*2);
title(strcat('1 radian degree impulse response. Input:  [',...
    [char(inputs_st_1(1)), '] Output:  [', char(outputs_st_1(4))], ']'));
%axis([0 max_time -0.3 0.5])
xlabel('t (s)')
ylabel('degree per second')
grid;
saveas(gcf, 'impulse_psi_to_p', 'epsc');

figure;
plot(T,impulse(a,b,c(5,:),d(5,:),1,T)*2*180/pi,'r','LineWidth',2);%*0.262*2);
title(strcat('1 radian degree impulse response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [2', char(outputs_st_1(5))], ']'));
%axis([0 max_time -0.05 0.05])
xlabel('t (s)');
ylabel('degree per second');
grid;
saveas(gcf, 'impulse_psi_to_r', 'epsc');

figure;
plot(T,impulse(a,b,c(6,:),d(6,:),1,T)*180/pi,'r','LineWidth',2);%*0.262);
title(strcat('1 radian degree impulse response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(6))], ']'));
%axis([0 max_time -0.3 0.6])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'impulse_psi_to_phi', 'epsc');

figure;
plot(T,impulse(a,b,c(7,:),d(7,:),1,T)*180/pi,'r','LineWidth',2);%*0.262);
title(strcat('1 radian degree impulse response. Input: [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(7))], ']'));
%axis([0 max_time -0.02 0.1])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'impulse_psi_to_psi', 'epsc');

%------------------------------
% IMPULSE IN r_C
figure; 
plot(T,impulse(a,b,c(1,:),d(1,:),2,T)*0.0175*180/pi,'r','LineWidth',2);%*0.262);
title(strcat('1 degree/s impulse response. Input: [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(1))], ']'));
%axis([0 max_time -0.3 0.5])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'impulse_rc_to_del_a', 'epsc');


figure;
plot(T,impulse(a,b,c(2,:),d(2,:),2,T)*0.0175*180/pi,'r','LineWidth',2);%*0.262);
title(strcat('1 degree/s impulse response. Input: [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(2))], ']'));
axis([0 15 -4 1])
xlabel('t (s)')
ylabel('degree')
grid;
saveas(gcf, 'impulse_rc_to_del_r', 'epsc');

figure;
plot(T,impulse(a,b,c(3,:),d(3,:),2,T)*0.0175/v_cruise*180/pi,'r','LineWidth',2);%*0.262/v_cruise);
title(strcat('1 degree/s impulse response. Input: [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(3))], ']'));
axis([0 15 -1 1])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'impulse_rc_to_beta', 'epsc');


figure;
plot(T,impulse(a,b,c(4,:),d(4,:),2,T)*0.0175*180/pi,'r','LineWidth',2);%*0.262*2);
title(strcat('1 degree/s impulse response. Input:  [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(4))], ']'));
%axis([0 max_time -0.3 0.5])
xlabel('t (s)')
ylabel('degree per second')
grid;
saveas(gcf, 'impulse_rc_to_p', 'epsc');


figure;
plot(T,impulse(a,b,c(5,:),d(5,:),2,T)*2*0.0175*180/pi,'r','LineWidth',2);%*0.262*2);
title(strcat('1 degree/s impulse response. Input:  [',...
    [char(inputs_st_1(2)), '] Output: [2', char(outputs_st_1(5))], ']'));
%axis([0 max_time -0.05 0.05])
xlabel('t (s)');
ylabel('degree per second');
grid;
saveas(gcf, 'impulse_rc_to_r', 'epsc');


figure;
plot(T,impulse(a,b,c(6,:),d(6,:),2,T)*0.0175*180/pi,'r','LineWidth',2);%*0.262);
title(strcat('1 degree/s impulse response. Input:  [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(6))], ']'));
%axis([0 max_time -0.3 0.5])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'impulse_rc_to_phi', 'epsc');


figure;
plot(T,impulse(a,b,c(7,:),d(7,:),2,T)*0.0175*180/pi,'r','LineWidth',2);%*0.262);
title(strcat('1 degree/s impulse response. Input:  [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(7))], ']'));
%axis([0 max_time -0.05 0.05])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'impulse_rc_to_psi', 'epsc');



%-------------------------15 degree STEP IN PSI_C

figure 
plot(T,step(a,b,c(1,:),d(1,:),1,T)*0.262*180/pi,'r','LineWidth',2);
title(strcat('15 degrees step response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(1))], ']'));
axis([0 max_time -55 13]);
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_psi_to_del_a', 'epsc');

figure;
plot(T,step(a,b,c(2,:),d(2,:),1,T)*0.262*180/pi,'r','LineWidth',2);
title(strcat('15 degrees step response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(2))], ']'));
%axis([0 max_time -0.022 0.1])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_psi_to_del_r', 'epsc');


figure;
plot(T,step(a,b,c(3,:),d(3,:),1,T)*0.262/v_cruise *180/pi,'r','LineWidth',2);
title(strcat('15 degrees step response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(3))], ']'));
%axis([0 max_time -0.022 0.1])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_psi_to_beta', 'epsc');


figure;
plot(T,step(a,b,c(4,:),d(4,:),1,T)*0.262*180/pi,'r','LineWidth',2);
title(strcat('15 degrees step response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(4))], ']'));
%axis([0 max_time -0.5 0.5])
xlabel('t (s)');
ylabel('degree per second');
grid;
saveas(gcf, 'step_psi_to_p', 'epsc');


figure;
plot(T,step(a,b,c(5,:),d(5,:),1,T)*2*0.262*180/pi,'r','LineWidth',2);
title(strcat('15 degrees step response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [2', char(outputs_st_1(5))], ']'));
%axis([0 max_time -0.022 0.1])
xlabel('t (s)');
ylabel('degree per second');
grid;
saveas(gcf, 'step_psi_to_r', 'epsc');


figure;
plot(T,step(a,b,c(6,:),d(6,:),1,T)*0.262*180/pi,'r','LineWidth',2);
title(strcat('15 degrees step response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(6))], ']'));
%axis([0 max_time -0.1 0.6])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_psi_to_phi', 'epsc');


figure;
plot(T,step(a,b,c(7,:),d(7,:),1,T)*0.262*180/pi,'r','LineWidth',2);
title(strcat('15 degrees step response. Input:  [',...
    [char(inputs_st_1(1)), '] Output: [', char(outputs_st_1(7))], ']'));
axis([0 max_time -1 18])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_psi_to_psi', 'epsc');


%------------------------------------
%STEP INPUT 1 deg/sec in r_c

figure
plot(T,step(a,b,c(1,:),d(1,:),2,T)*0.0175*180/pi,'r','LineWidth',2);
title(strcat('1 degree/s step response. Input:  [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(1))], ']'));
%axis([0 max_time -0.5 0.5])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_rc_to_dela', 'epsc');


figure;
plot(T,step(a,b,c(2,:),d(2,:),2,T)*0.0175*180/pi,'r','LineWidth',2);
title(strcat('1 degree/s step response. Input:  [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(2))], ']'));
%axis([0 max_time -0.022 0.1])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_rc_to_delr', 'epsc');


figure;
plot(T,step(a,b,c(3,:),d(3,:),2,T)*0.0175/v_cruise*180/pi,'r','LineWidth',2);
title(strcat('1 degree/s step response. Input: [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(3))], ']'));
%axis([0 max_time -0.022 0.1])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_rc_to_beta', 'epsc');


figure;
plot(T,step(a,b,c(4,:),d(4,:),2,T)*0.0175*180/pi,'r','LineWidth',2);
title(strcat('1 degree/s step response. Input: [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(4))], ']'));
%axis([0 max_time -0.5 0.5])
xlabel('t (s)');
ylabel('degree per second');
grid;
saveas(gcf, 'step_rc_to_p', 'epsc');


figure;
plot(T,step(a,b,c(5,:),d(5,:),2,T)*2*0.0175*180/pi,'r','LineWidth',2);
title(strcat('1 degree/s step response. Input:  [',...
    [char(inputs_st_1(2)), '] Output: [2', char(outputs_st_1(5))], ']'));
%axis([0 max_time -0.022 0.1])
xlabel('t (s)');
ylabel('degree per second');
grid;
saveas(gcf, 'step_rc_to_r', 'epsc');


figure;
plot(T,step(a,b,c(6,:),d(6,:),2,T)*0.0175*180/pi,'r','LineWidth',2);
title(strcat('1 degree/s step response. Input:  [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(6))], ']'));
%axis([0 max_time -0.5 0.5])
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_rc_to_phi', 'epsc');


figure;
plot(T,step(a,b,c(7,:),d(7,:),2,T)*0.0175*180/pi,'r','LineWidth',2);
title(strcat('1 degree/s step response. Input: [',...
    [char(inputs_st_1(2)), '] Output: [', char(outputs_st_1(7))], ']'));
%axis([0 max_time -0.022 0.15]);
xlabel('t (s)');
ylabel('degree');
grid;
saveas(gcf, 'step_rc_to_psi', 'epsc');


