more on
echo on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Aircraft model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

A=[0,1,0;0,-.875,-20;0,0,-50];
B=[0;0;50];
C=[1,0,0;0,1,0];
D=[0;0];

% for set-point control
F=[1;0;0];
N=0;

%%% state-space model
sys_z_ss=ss(A,B,C,D,...
    'InputName',{'u'},...
    'OutputName',{'roll-angle','roll-rate'},...
    'StateName',{'roll-angle','roll-rate','roll-acceleration'})

sys_y_ss=sys_z_ss(1);

disp('press <return> to continue');pause

%%% transfer-function
sys_z_tf=tf(sys_z_ss)
sys_z_zpk=zpk(sys_z_ss)

sys_y_tf=tf(sys_y_ss);
sys_y_zpk=zpk(sys_y_ss);

disp('press <return> to continue');pause

% e^{A t}
syms t
expm(A*t)

disp('press <return> to continue');pause

%%% controllability & observability
CC=ctrb(sys_z_ss)
rank(CC)
OOz=obsv(sys_z_ss)
rank(OOz)
OOy=obsv(sys_y_ss)
rank(OOy)

disp('press <return> to continue');pause

% stability
eig(sys_z_ss)

disp('press <return> to continue');pause

%%% Bode plot

figure(1)
bode(sys_z_ss)
title('Process Bode Diagram')
grid on
disp('press <return> to continue');pause

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LQR control (z = {roll-angle,roll-rate})
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Q=diag([1,1]);
R=1;

rho=1;
gam=.1;
G=[1,0,0;0,gam*1,0];
H=[0;0];
K=lqr(A,B,G'*Q*G,H'*Q*H+rho*R,G'*Q*H)
G0=ss(A,B,K,0,'InputName','u','OutputName','uu')

%% open-loop Bode plot
figure(2)
bode(G0)
grid on
title('Open-loop Bode Diagram')
disp('press <return> to continue');pause

%% open-loop Nyquist plot
figure(3);clf
nyquist(G0)
axis([-5,2,-3,3])
axis equal
title(sprintf('Open-loop Nyquist Diagram (rho = %g, gamma = %g)',rho,gam))
hold on
% plot LQR circle
plot(cos(0:.3:2*pi)-1,sin(0:.3:2*pi),':')
plot(-cos(0:.3:pi/2.5),-sin(0:.3:pi/2.5),'--')
plot([0,-1.2*cos(pi/3)],[0,-1.2*sin(pi/3)],'-')
text(-1,.2,'-1','VerticalAlignment','baseline','HorizontalAlignment','center')
text(-2,.2,'-2','VerticalAlignment','baseline','HorizontalAlignment','center')
text(-.6,-.5,'\pi/3','VerticalAlignment','baseline','HorizontalAlignment','center')
hold off
disp('press <return> to continue');pause

print -depsc lqr-z-omega.eps

%%% changing \rho 

gam=.01;
G=[1,0,0;0,gam*1,0];
H=[0;0];

rho1=.01;
K1=lqr(A,B,G'*Q*G,H'*Q*H+rho1*R,G'*Q*H)
G01=ss(A,B,K1,0,'InputName','u','OutputName','uu');
rho2=1;
K2=lqr(A,B,G'*Q*G,H'*Q*H+rho2*R,G'*Q*H)
G02=ss(A,B,K2,0,'InputName','u','OutputName','uu');
rho3=100;
K3=lqr(A,B,G'*Q*G,H'*Q*H+rho3*R,G'*Q*H)
G03=ss(A,B,K3,0,'InputName','u','OutputName','uu');

%% open-loop Bode plot
figure(4)
bode(G01,'-',G02,'-.',G03,'.-',-sys_y_tf,'-o')
grid on
title('Open-loop Bode Diagrams')
% fix axis for better viewing
a=get(gcf,'children');
axes(a(1));axis([1e-2,1e3,-180,-80])
axes(a(2));axis([1e-2,1e3,-50,90])
legend(sprintf('rho = %g',rho1),sprintf('rho = %g',rho2),sprintf('rho = %g',rho3),'u to roll angle',sprintf('gamma = %g',gam),3)
title('Open-loop Bode Diagrams')

print -depsc lqr-rho-bode.eps

%% Closed-loop step
figure(5)
%step(sys_y_tf*(K1*F+N)/(1+G01),'-',sys_y_tf*(K2*F+N)/(1+G02),'-.',sys_y_tf*(K3*F+N)/(1+G03),'.-',6)
step(ss(A-B*K1,B*(K1*F+N),G(1,:)-H(1,:)*K1,H(1,:)*(K1*F+N)),'-',...
    ss(A-B*K2,B*(K2*F+N),G(1,:)-H(1,:)*K2,H(1,:)*(K2*F+N)),'-.',...
    ss(A-B*K3,B*(K3*F+N),G(1,:)-H(1,:)*K3,H(1,:)*(K3*F+N)),'.-',6)
grid on
legend(sprintf('rho = %g',rho1),sprintf('rho = %g',rho2),sprintf('rho = %g',rho3),sprintf('gamma = %g',gam),4)

print -depsc lqr-rho-step.eps

%%% changing z

H=[0;0];
rho=.01

gam1=.01
G1=[1,0,0;0,gam1,0];
K1=lqr(A,B,G1'*Q*G1,H'*Q*H+rho*R,G1'*Q*H)
G01=ss(A,B,K1,0,'InputName','u','OutputName','uu');
gam2=.1
G2=[1,0,0;0,gam2,0];
K2=lqr(A,B,G2'*Q*G2,H'*Q*H+rho*R,G2'*Q*H)
G02=ss(A,B,K2,0,'InputName','u','OutputName','uu');
gam3=.3
G3=[1,0,0;0,gam3,0];
K3=lqr(A,B,G3'*Q*G3,H'*Q*H+rho*R,G3'*Q*H)
G03=ss(A,B,K3,0,'InputName','u','OutputName','uu');

%% open-loop Bode plot
figure(6)
bode(G01,'-',G02,'-.',G03,'.-',-sys_y_tf,'-o')
grid on
title('Open-loop Bode Diagrams')
% fix axis for better viewing
a=get(gcf,'children');
axes(a(1));axis([1e-2,1e3,-180,-80])
axes(a(2));axis([1e-2,1e3,-50,90])
legend(sprintf('gamma = %g',gam1),sprintf('gamma = %g',gam2),sprintf('gamma = %g',gam3),'u to roll angle',sprintf('(rho = %g)',rho),3)

print -depsc lqr-z-bode.eps

%% Closed-loop step
figure(7)
%step(sys_y_tf*(K1*F+N)/(1+G01),'-',sys_y_tf*(K2*F+N)/(1+G02),'-.',sys_y_tf*(K3*F+N)/(1+G03),'.-',6)
step(ss(A-B*K1,B*(K1*F+N),G1(1,:)-H(1,:)*K1,H(1,:)*(K1*F+N)),'-',...
    ss(A-B*K2,B*(K2*F+N),G2(1,:)-H(1,:)*K2,H(1,:)*(K2*F+N)),'-.',...
    ss(A-B*K3,B*(K3*F+N),G3(1,:)-H(1,:)*K3,H(1,:)*(K3*F+N)),'.-',6)
grid on
legend(sprintf('gamma = %g',gam1),sprintf('gamma = %g',gam2),sprintf('gamma = %g',gam3),sprintf('(rho = %g)',rho),4)

print -depsc lqr-z-step.eps



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LQR control (z = {roll-angle,\dot tau})
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Q=diag([1,1]);
R=1;

gam=1
G=[1,0,0;0,0,gam*(-50)];
H=[0;gam*50];
rho=.01;
K=lqr(A,B,G'*Q*G,H'*Q*H+rho*R,G'*Q*H)
G0=ss(A,B,K,0,'InputName','u','OutputName','uu')

%% open-loop Bode plot
figure(8)
bode(G0)
grid on
title('Open-loop Bode Diagram')
disp('press <return> to continue');pause

%% open-loop Nyquist plot
figure(9);clf
nyquist(G0)
axis([-5,2,-3,3])
axis equal
title('Open-loop Nyquist Diagram')
hold on
% plot LQR circle
plot(cos(0:.3:2*pi)-1,sin(0:.3:2*pi),':')
plot(-cos(0:.3:pi/2.5),-sin(0:.3:pi/2.5),'--')
plot([0,-1.2*cos(pi/3)],[0,-1.2*sin(pi/3)],'-')
text(-1,.2,'-1','VerticalAlignment','baseline','HorizontalAlignment','center')
text(-2,.2,'-2','VerticalAlignment','baseline','HorizontalAlignment','center')
text(-.6,-.5,'\pi/3','VerticalAlignment','baseline','HorizontalAlignment','center')
hold off
disp('press <return> to continue');pause


%% changing z

rho=1

gam1=1
G1=[1,0,0;0,0,gam1*(-50)];
H1=[0;gam1*50];
K1=lqr(A,B,G1'*Q*G1,H1'*Q*H1+rho*R,G1'*Q*H1)
G01=ss(A,B,K1,0,'InputName','u','OutputName','uu');
gam2=.5
G2=[1,0,0;0,0,gam2*(-50)];
H2=[0;gam2*50];
K2=lqr(A,B,G2'*Q*G2,H2'*Q*H2+rho*R,G2'*Q*H2)
G02=ss(A,B,K2,0,'InputName','u','OutputName','uu');
gam3=.2
G3=[1,0,0;0,0,gam3*(-50)];
H3=[0;gam3*50];
K3=lqr(A,B,G3'*Q*G3,H3'*Q*H3+rho*R,G3'*Q*H3)
G03=ss(A,B,K3,0,'InputName','u','OutputName','uu');

print -depsc lqr-z-taudot.eps

%% open-loop Bode plot
figure(10)
bode(G01,'-',G02,'-.',G03,'.-',-sys_y_tf,'-o')
grid on
title('Open-loop Bode Diagrams')
% fix axis for better viewing
a=get(gcf,'children');
%axes(a(1));axis([1e-2,1e4,-180,-80])
axes(a(2));axis([1e-2,1e4,-50,90])
legend(sprintf('gamma = %g',gam1),sprintf('gamma = %g',gam2),sprintf('gamma = %g',gam3),'u to roll angle',sprintf('(rho = %g)',rho),3)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LQG state-estimation   ( y = {roll-angle} )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% LQR regulator

Q=diag([1,1]);
H=[0;0];
rho=.01
gam=.1
G=[1,0,0;0,gam,0];
K=lqr(A,B,G'*Q*G,H'*Q*H+rho*R,G'*Q*H)
G0=ss(A,B,K,0,'InputName','u','OutputName','uu')

%% LQG estimator

BB=B;
QN=1;
sig=1e-6;

[est,L]=kalman(sys_y_ss,QN,sig);

eig(A-L*C(1,:))

%% changing sigma
sig1=1e-2;
[est,L1]=kalman(sys_y_ss,QN,sig1);
C1=-reg(sys_y_ss,K,L1);
sig2=1e-5;
[est,L2]=kalman(sys_y_ss,QN,sig2);
C2=-reg(sys_y_ss,K,L2);
sig3=1e-8;
[est,L3]=kalman(sys_y_ss,QN,sig3);
C3=-reg(sys_y_ss,K,L3);

%% open-loop Bode plot
figure(11)
bode(C1*sys_y_ss,'-o',C2*sys_y_ss,'.-',C3*sys_y_ss,'-.',G0,'-')
grid on
title('Open-loop Bode Diagrams')
% fix axis for better viewing
a=get(gcf,'children');
axes(a(1));axis([1e-2,1e3,-200,-80])
axes(a(2));axis([1e-2,1e3,-90,90])
legend(sprintf('sigma = %g',sig1),sprintf('sigma = %g',sig2),sprintf('sigma = %g',sig3),'LQR loop-gain',sprintf('(rho = %g, gamma=%g)',rho,gam),3)

print -depsc lqg-sig-bode.eps

%% Closed-loop step
figure(12)
step(ss([A,B*K;-L1*C(1,:),A-L1*C(1,:)-B*K],[B*N;L1*C(1,:)*F],[G(1,:),H(1,:)*K],H(1,:)*N),'-o',...
    ss([A,B*K;-L2*C(1,:),A-L2*C(1,:)-B*K],[B*N;L2*C(1,:)*F],[G(1,:),H(1,:)*K],H(1,:)*N),'.-',...
    ss([A,B*K;-L3*C(1,:),A-L3*C(1,:)-B*K],[B*N;L3*C(1,:)*F],[G(1,:),H(1,:)*K],H(1,:)*N),'-.',...
    ss(A-B*K,B*(K*F+N),G(1,:)-H(1,:)*K,H(1,:)*(K*F+N)),'-',...
    2)
grid on
legend(sprintf('sigma = %g',sig1),sprintf('sigma = %g',sig2),sprintf('sigma = %g',sig3),'LQR loop-gain',sprintf('(rho = %g, gamma=%g)',rho,gam),4)

print -depsc lqg-sig-step.eps