% LQG design of 2 inverted pendulums on wagons connected with spring
% bob 100318

% System data
m1=1; m2=1; k=100; cd=0.1; kI=10;
g=9.81; l1=1; l2=1;
mtot=m1+m2;m=m1*m2/mtot;
w0=sqrt(k*(m1+m2)/m1/m2);z0=cd/(m*2*w0);
w1= sqrt(g/l1); w2= sqrt(g/l2);
alpha1=m2/(m1+m2);alpha2=m1/(m1+m2);
beta1=cd/(w0*m1);beta2=cd/(w0*m2);
gamma1=kI/(w0*m1);gamma2=kI/(w0*m2);
A=[0 0 1 0;0 0 0 1;-alpha1 alpha1 -beta1 beta1;alpha2 -alpha2 beta2 -beta2]*w0;
A=[A zeros(4,4); 0 0 -w0 0 0 w1 0 0 ; 0 0 0 0 w1 0 0 0 ; 0 0 0 -w0 0 0 0 w2; 0 0 0 0 0 0 w2 0];
B1=[0;0;gamma1;0;0;0;0;0];B2=[0;0;0;gamma2;0;0;0;0];
C1=[1 0 0 0 0 0 0 0];C2=[0 1 0 0 0 0 0 0];C3=[0 0 0 0 1 0 0 0];C4=[0 0 0 0 0 0 1 0];
B=[B1 B2];C=[C1;C2;C3;C4];D=0*C*B;
sys=ss(A,B,C,D);

timev=0:0.001:10;
wv={1e-2 1e3};

% initial values
phi0 = 0.05;
x0 = 0.05;

for ind=1:2
  if ind==1
    Bd = [0;0;0;0;1;0;0;0]*phi0;
  else
    Bd = [1;0;0;0;0;0;0;0]*x0;
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Tune LQG design below

%   State feedback
Q=eye(8);
R=eye(2);
[K,S,E]=lqr(A,B,Q,R);

%   Observer
G=eye(8);
H=zeros(4,8);
Qn=eye(8);
Rn=eye(4);
syse = ss(A,[B G],C,[D H]);
[kest,L,P] = kalman(syse,Qn,Rn);


%%%%%%%%%%%%%%%%%%%%%%%% Evaluation %%%%%%%%%%%%%%%%%

Ac=A-B*K-L*C;
Bc=L; 
Cc=K;
Dc=0*D';
sysc = ss(Ac,Bc,Cc,Dc);

% initial responses 

Atot = [A -B*K; L*C A-B*K-L*C];
Btot = [Bd;0*Bd];
Be = [0*L;L]; 
Cy = [C 0*C];
Cu = [0*K -K];
cly = ss(Atot,Btot,eye(16),0*eye(16)*Btot);
clye = ss(Atot,Be,eye(16),0*eye(16)*Be);
clu = ss(Atot,Btot,Cu,0*Cu*Btot);
clue = ss(Atot,Be,Cu,0*Cu*Be);

noise = 1/128*(rand(length(timev),4)-0.5); % ~8 bit resolution
[y,t]=impulse(cly,timev);
[ye,t]=lsim(clye,noise,timev);
[u,t]=impulse(clu,timev);
[ue,t]=lsim(clue,noise,timev);

figure(1)
subplot(411)
plot(t,y(:,1),'r');hold on;grid on;axis([0 max(timev) -0.1 0.1])
plot(t,y(:,1)+ye(:,1),'k');hold on;grid on
subplot(412)
plot(t,y(:,2),'r');hold on;grid on;axis([0 max(timev) -0.1 0.1])
plot(t,y(:,2)+ye(:,2),'k');hold on;grid on
subplot(413)
plot(t,y(:,5),'r');hold on;grid on;axis([0 max(timev) -0.1 0.1])
plot(t,y(:,5)+ye(:,5),'k');hold on;grid on
subplot(414)
plot(t,y(:,7),'r');hold on;grid on;axis([0 max(timev) -0.1 0.1])
plot(t,y(:,7)+ye(:,7),'k');hold on;grid on

figure(2);
plot(t,u);hold on;grid on
plot(t,u+ue);hold on;grid on
title('control signals')

figure(3)
Loopgain1 = sysc*sys;
T = feedback(Loopgain1,ss(0,zeros(1,2),zeros(2,1),eye(2)));
[svt,wt]=sigma(T,wv);
loglog(wt,svt,'Linewidth',2)
hold on; grid on
title('T=(I+CP)\^(-1)CP')
maxT=max(max(svt))

figure(4)
S = feedback(ss(0,zeros(1,2),zeros(2,1),eye(2)),Loopgain1);
[svs,wt]=sigma(S,wv);
loglog(wt,svs,'Linewidth',2)
hold on; grid on
title('Input Sensitivity S=(I+CP)\^(-1)')
maxS=max(max(svs))

figure(5)
[svc,wc]=sigma(sysc,wv);
loglog(wc,svc,'Linewidth',2)
hold on; grid on
title('Controller (singular values)')

uxe = norm(clue(:,[1 2]))
uphie = norm(clue(:,[3 4]))

figure(6)
[svcp,wcp]=sigma(sysc*sys,wv);
loglog(wcp,svcp,'Linewidth',2)
title('Loop Gain (singular values)')
grid on; hold on


figure(7)
[svclue,wclue]=sigma(clue,wv);
loglog(wclue,svclue,'Linewidth',2)
title('Meas noise to controller (singular values)')
grid on; hold on
end