% Code develloped by Sebastien Corner

function FinalProject_SebastienCorner
clc
% z output form Noisy output, sattelit
%y output from the state space model
close all
clear all
LOADData=load('dtplant.mat');

ssOL=LOADData.dt_sys;
fs =LOADData.fs ;
Ts =1/fs;
N = 40/Ts+1;
t = Ts*[0:N-1];
T = N*Ts; % total period of data
AVnbr=20;
Np=5/Ts;
tp = Ts*[0:Np];
H=tf(ssOL);
v=[9.9999,zeros(1,N-1)];


[z_av,y]=QuestionA_C(H,v);% Input the discret transfert function of the SS Open loop object. Number of average 20
% Ts=0.05 s which is the sampling time provided from the SS OPEN loop
% object.

QuestionD(z_av,y,v);% Plot of the outputs z_av which is the noisy output from the satellite averaging 20 times
% y is the output from the open loop system model
%v is the pulse input

QuestionE(ssOL);% We look at the controllability and observability of the open loop system

[ssControler,G]=QuestionF_G();% Pole placement of the closed-loop z-domain of the system
% For that we alter the open loop from s domain to s domain using d2c()
% function and use the damp function to have the required information.
%We set out close loop to the s domain and transform them to the z domain
%Then we used place() function to find the gain matrix G

[MY,MU]=QuestionH(G);%  the full-state feedback controller performance
% MY is the the output of the full state feedback and Mu the input

QuestionI(MY,MU);% plot of the results of your full-state feedback controller performance from Task 2h. The first subplot  
%contain all three sensor responses, and the second subplot  contain both control input signals

K=QuestionJ(ssControler);%  Design closed-loop z-domain observer
%Determine the full state observer feedback control gains

[SSCOL]=QuestionK(K,G);%Construct a discrete-time state-space LTI object (dt_ofc) to 
%implement your complete output feedback controller

QuestionL(SSCOL)%o plot the results of t 
%output feedback controller performance.



function [z_av,y]=QuestionA_C(H,v)
% Response z from the satelitte
u1=[v;zeros(1,N)];
u2=[zeros(1,N);v];

[z_av1]=Average(u1,AVnbr);
[z_av2]=Average(u2,AVnbr);
z_av=[z_av1;z_av2];


% Response y from the state space model provided
for cpt=1:3
y1(cpt,1:N)=filter(H(cpt,1).num{1},H(cpt,1).den{1},v);
y2(cpt,1:N)=filter(H(cpt,2).num{1},H(cpt,2).den{1},v);
end
y=[y1;y2];
end

function [z_av]=Average(u,AVnbr)
Mz1=[];Mz2=[];Mz3=[];
for m=1:AVnbr
    z=satellite_uplink(u);
    z1=z(1,:);z2=z(2,:);z3=z(3,:);
    Mz1=[Mz1;z1];Mz2=[Mz2;z2];Mz3=[Mz3;z3];
end % we crated a matrice for each z output , each line of this matrice is the output of zi output.
z1av=mean(Mz1);z2av=mean(Mz2);z3av=mean(Mz3);% Average calculation for each output zi
z_av=[z1av;z2av;z3av];
end

function QuestionD(z_av,y,v)
    %Plots of the outputs for input1 
z_av1=z_av(1:3,:);
y1=y(1:3,:);

figure(1);
    for cpt=1:3
    subplot(3,2,2*(cpt-1)+1)
    plot(t,z_av1(cpt,:),'b')
    hold on
    plot(t,y1(cpt,:),'r')
    plot(t,v,'g')
    ylim([-0.3 .42])
    xlim([0,40])
    grid on
    hold off
        if cpt==2
        ylabel('Magnitude')
        xlabel('Settling time 27s')
        end
        if cpt==1
        title('Output signal for input u1')
        xlabel('Settling time >40s')
        end
    STDm=mean(y1(cpt,N-50:N));
    line([0 T],[STDm STDm],'Color','black','LineStyle','--')
    line([0 T],[STDm-5/100*abs(y1(cpt,1)-STDm) STDm-5/100*abs(y1(cpt,1)-STDm)],'Color','black','LineStyle','--')
    line([0 T],[STDm+5/100*abs(y1(cpt,1)-STDm) STDm+5/100*abs(y1(cpt,1)-STDm)],'Color','black','LineStyle','--')
    end
    h=legend('AV','ssOutput','input','location','SouthEast');
    set(h,'orientation','horizontal')
    xlabel('Time domain signal  Settling time >40s')

    
    
    %Plots of the outputs for input2
z_av2=z_av(4:6,:);
y2=y(4:6,:);   

    for cpt=1:3
    subplot(3,2,2*(cpt))
    plot(t,z_av2(cpt,:),'b')
    hold on
    plot(t,y2(cpt,:),'r')
    plot(t,v,'g')
    ylim([-0.3 .42])
    xlim([0,40])
    grid on
        if cpt==2
        ylabel('Magnitude')
        xlabel('Settling time 27s')
        end
        if cpt==1
        title('Output signal for input u2')
        xlabel('Settling time 34s')
        end
      STDm=mean(y2(cpt,N-50:N));
    line([0 T],[STDm STDm],'Color','black','LineStyle','--')
    line([0 T],[STDm-5/100*abs(y2(cpt,1)-STDm) STDm-5/100*abs(y2(cpt,1)-STDm)],'Color','black','LineStyle','--')
    line([0 T],[STDm+5/100*abs(y2(cpt,1)-STDm) STDm+5/100*abs(y2(cpt,1)-STDm)],'Color','black','LineStyle','--')
    hold off
    end
    h=legend('AV','ssOutput','input','location','SouthEast');
    set(h,'orientation','horizontal')
    xlabel('Time domain signal /Settling time 22s')
    Mz_av=[z_av1;z_av2];
end

function QuestionE(ssOL)
disp('Controlability Matrix Q1')
Q1=ctrb(ssOL.a,ssOL.b)
rank_CTR=rank(Q1)
disp('Controlability Matrix is full rank the system is contralable')
disp('Observability Matrix O1')
O1=obsv(ssOL.a,ssOL.c)
rank_OBS=rank(O1)
disp('Observability Matrix is full rank the system is observable')
end

function [ssControler,G]=QuestionF_G()
 A=ssOL.a;
B=ssOL.b;
C=ssOL.c;
D=ssOL.d;

wCL1 =2.68; wCL2=7.33; wCL3=  2.6845;  %Do not change the flexible mode natural frequencies
zCL1 = 0.5; zCL2 = 0.5;zCL3 = 1; % Increase the damping ratios of both flexible modes to 0.5
%Move the rigid body poles into the LHP such that they have the same magnitude as the first (i.e. lowest frequency) flexible mode
CTCLpoles = [-zCL1*wCL1 + 1j*wCL1*sqrt(1-zCL1^2), -zCL1*wCL1 - 1j*wCL1*sqrt(1-zCL1^2),...
           -zCL2*wCL2+ 1j*wCL2*sqrt(1-zCL2^2),  -zCL2*wCL2-1j*wCL2*sqrt(1-zCL2^2),...
            -zCL3*wCL3+ 1j*wCL3*sqrt(1-zCL3^2),  -zCL3*wCL3-1j*wCL3*sqrt(1-zCL3^2) ]'; 
DTCLpoles = exp(CTCLpoles * Ts); 
disp('full-state feedback control gain matrix ')
[G] = place(A,B,DTCLpoles)
ssControler = ss(A-B*G, zeros(6,2), C, D,Ts);


disp('OPEN LOOP State space Z domain')
damp(ssOL)
disp('OPEN LOOP state space S domain')
ssOL_S=d2c(ssOL);
damp(ssOL_S)

disp('Close LOOP state space Z domain')
damp(ssControler)


disp('Matrice of controler Gains')


end

function [MY,MU]=QuestionH(G)
A=ssOL.a;
B=ssOL.b;
C=ssOL.c;
D=ssOL.d;
X=zeros(6,1);
MX=[];
MY=[];
MU=[];

for cpt=0:Np-1
    if cpt*Ts<=1.5
        U=[-2;3];%intput signal from t=[0 1.5] s
    else 
        U=-G*X;%intput signal from t=[1.5 5] s
    end
    Y=C*X+D*U;% outout of the full-state feedback controller performance at a time t
    MU=[MU U];
    MX=[MX X];
    MY=[MY Y];
    X=A*X+B*U ; % state of the full-state feedback controller performance at a time t
end
Y=C*X+D*U; % we need to calculate the last term
  U=G*X;
MU=[MU U];% row of the input
MX=[MX X];% row of the state
MY=[MY Y];% row of the output`

end        

function QuestionI(MY,MU)
figure(2)
subplot(2,1,1)

    plot(tp,MY)
    hold on

    xlim([0,5])
    grid on
    ylabel('Magnitude')  
    title('Output signal of the full-state feedback controller')
    legend('Y1','Y2','Y3','location','SouthWest')
    
subplot(2,1,2)

    plot(tp,MU)
    hold on
    xlim([0,5])
    grid on
    ylabel('Magnitude')  
    title('Input signal u')
    legend('U1','U2','location','SouthWest')
    xlabel('Time domain signal')             
end

function K=QuestionJ(ssControler)
    
 disp('Close LOOP state space S domain')
ssControler_S=d2c(ssControler);
damp(ssControler_S)
Eig_CT_S=eig(ssControler_S);

 A=ssOL.a;
B=ssOL.b;
C=ssOL.c;
D=ssOL.d;

wOB1 =2.68*4; wOB2=7.33*1.5; wOB3=2.6845*4; 
zOB1 = 0.6; zOB2 = 1;zOB3 = 0.6; 

CTOBpoles = [-zOB1*wOB1 + 1j*wOB1*sqrt(1-zOB1^2), -zOB1*wOB1 - 1j*wOB1*sqrt(1-zOB1^2),...
           -zOB2*wOB2+ 1j*wOB2*sqrt(1-zOB2^2),  -zOB2*wOB2-1j*wOB2*sqrt(1-zOB2^2),...
            -zOB3*wOB3+ 1j*wOB3*sqrt(1-zOB3^2),  -zOB3*wOB3-1j*wOB3*sqrt(1-zOB3^2) ]'; 
DTOBpoles = exp(CTOBpoles * Ts); 
disp('final observer feedback control gain matrix')
[K] = place(A',C',DTOBpoles)'

end

function [SSCOL]=QuestionK(K,G)
A=ssOL.a;
B=ssOL.b;
C=ssOL.c;
D=ssOL.d;

Acl=A-K*C; Bcl=[B-K*D K];
Ccl=-G; Dcl=zeros(2,5);
disp('output feedback controller object')
SSCOL=ss(Acl,Bcl,Ccl,Dcl)


end

function QuestionL(SSCOL)
    
[ycol,ucol,xhat] =satellite_uplink(SSCOL,[1.5 5]);
C=ssOL.c;
D=ssOL.d;
yhat=C*xhat+D*ucol;
err=ycol-yhat;

 figure(3)
for cpt=1:3 
subplot(4,1,cpt)
plot(tp,ycol(cpt,:),'r')
hold on
plot(tp,yhat(cpt,:),'k')
plot(tp,err(cpt,:),'b')
ylim([-2.2 2.2])
xlim([0,5])
grid on
hold off
  if cpt==2
  ylabel('Magnitude')
  legend('Y2','Yhat2','error2')
  end
  if cpt==1
  title('output feedback controller performance.')
  legend('Y1','Yhat1','error1')
  end
  if cpt==3
  legend('Y3','Yhat3','error3')
  end
line([0 T],[-2  -2],'Color','black','LineStyle','--')
line([0 T],[2 2],'Color','black','LineStyle','--')

end
subplot(4,1,4)
plot(tp,ucol(1,:),'b',tp,ucol(2,:),'r')
ylim([-11 11])
xlim([0,5])
line([0 T],[-10  -10],'Color','black','LineStyle','--')
line([0 T],[10 10],'Color','black','LineStyle','--')
legend('U1','U2')
xlabel('Time domain signal')
grid on
end

end