% Model2 (Vibration contro - 3 storey building model)
% LQR control
%
% Base parameters
m0=5; % base mass kg
c0=100; % base damping coefficient N/m/s
k0=16000; % base stiffness N/m

% Floor parameters
m1=1.72; % first floor mass kg
c1=0.078; % base damping coefficient N/m/s
k1=2600; % base stiffness N/m

% Floor parameters
m2=1.48; % second floor mass kg
c2=0.078; % base damping coefficient N/m/s
k2=2600; % base stiffness N/m

% Floor parameters
m3=2.34; % third floor mass kg
c3=0.078; % base damping coefficient N/m/s
k3=2600; % base stiffness N/m

%------------------------------------------------
% Open loop without actuator dynamics
% State space model
% State: [x0 x1 x2 x3 x0' x1' x2' x3']
% Input disturbance: Earthquake acceleration

A1=[0   0   0   0    1    0    0    0
    0   0   0   0    0    1    0    0
    0   0   0   0    0    0    1    0
    0   0   0   0    0    0    0    1
   -(k0+k1)/m0 k1/m0 0 0 -(c0+c1)/m0 c1/m0 0 0
   k1/m1 -(k1+k2)/m1 k2/m1 0 c1/m1 -(c1+c2)/m1 c2/m1 0
   0  k2/m2 -(k2+k3)/m2 k3/m2 0 c2/m2 -(c2+c3)/m2 c3/m2
   0 0 k3/m3 -k3/m3 0 0 c3/m3 -c3/m3];

B1=[0 0 0 0 -1/m0 1/m1 0 0]'; % control input matrix

F1=[0 0 0 0 1 0 0 0]'; % disturbance matrix (Earthqauake force)

% Measurements are: (1) acceleration x1'' and relative displacement x1-x0
C1=[k1/m1 -(k1+k2)/m1 k2/m1 0 c1/m1 -(c1+c2)/m1 c2/m1 0
    -1 1 0 0 0 0 0 0 ];
D1=[1/m1 ; 0];

sys1=ss(A1,F1,C1,D1); % from vu -> y (earthquake acceleration -> [x1'';x1-x0])
%-----------------------------------------------------------------------------
% LQR design
%
% Minimisa J=int_{0}^{\inf} [|x1''|^2+rho1*|x1-x0|^2+rho2*u^2] dt
% Express in standard form with Q,R,N

rho1=.1;
rho2=.5;
alpha1=(A1(6,:))';
beta1=B1(6,1);
gamma1=(C1(2,:))';

Q=alpha1*alpha1'+rho1*gamma1*gamma1';
R=rho2+beta1^2;
N=beta1.*alpha1;

[K,S,E] = lqr(A1,B1,Q,R,N); % get LQR controller

A_cl=A1-B1*K; % closed-loop matrix
C1_cl=C1(1,:);D1_cl=D1(1); % acceleration x_1''
C2_cl=C1(2,:);D2_cl=D1(2); % relative displacement(x1-x0)
C3_cl=-K;D3_cl=0; % Actuator force
C_cl=[C1_cl;C2_cl;C3_cl];
D_cl=[D1_cl;D2_cl;D3_cl];

sys_cl=ss(A_cl,F1,C_cl,D_cl); % closed-loop system
%-----------------------------------------------------------
% Impulse responses OL and CL
t=linspace(0,2,200);

[y_ol,t1,x_ol]=impulse(sys1,t);
[y_cl,t1,x_cl]=impulse(sys_cl,t);

figure(1)
plot(t',y_ol(:,1),t',y_cl(:,1)); % Acceleration x_1'' (Open and closed-loop)
grid;
xlabel('time sec')
ylabel('Amplitude acceleration x1_dd (m/s^2)')
title('Impulse response: nu->x1dd')
%
figure(2)
plot(t',y_ol(:,2),t',y_cl(:,2)); % Relative displacement x1-x0 (Open and closed-loop)
grid;
xlabel('time sec')
ylabel('Amplitude relative displacement x1-x0(m)')
title('Impulse response: nu->x1-x0')
%
figure(3)
plot(t',y_cl(:,3)); % Actuator force (closed-loop)
grid;
xlabel('time sec')
ylabel('Actuator Force U(N)')
title('Impulse response: nu->U')
%
figure(4)
plot(t',x_ol(:,1),t',x_cl(:,1)); % x_0 (Open and closed-loop)
grid;
xlabel('time sec')
ylabel('Amplitude displacement x0(m)')
title('Impulse response: nu->x0')
%
figure(5)
plot(t',x_ol(:,2),t',x_cl(:,2)); % Displacement x1 (Open and closed-loop)
grid;
xlabel('time sec')
ylabel('Amplitude displacement x1(m)')
title('Impulse response: nu->x1')
%
figure(6)
plot(t',x_ol(:,3),t',x_cl(:,3)); % Displacement x2 (Open and closed-loop)
grid;
xlabel('time sec')
ylabel('Amplitude displacement x2(m)')
title('Impulse response: nu->x2')
%
figure(7)
plot(t',x_ol(:,4),t',x_cl(:,4)); % Displacement x3 (Open and closed-loop)
grid;
xlabel('time sec')
ylabel('Amplitude displacement x3(m)')
title('Impulse response: nu->x3')
%
%------------------------------------------------------------------------
% Frequency responses

w=logspace(1,3,200);
[mag_ol,ph_ol]=bode(sys1,w);
[mag_cl,ph_cl]=bode(sys_cl,w);

mag_ol_1=mag_ol(1,1,:);
mag_ol_1=mag_ol_1(1,:); % row vector
mag_ol_1=20*log10(mag_ol_1); % acceleration x_1'' dB
mag_ol_2=mag_ol(2,1,:);
mag_ol_2=mag_ol_2(1,:); % row vector
mag_ol_2=20*log10(mag_ol_2); % acceleration x_1'' dB

mag_cl_1=mag_cl(1,1,:);
mag_cl_1=mag_cl_1(1,:); % row vector
mag_cl_1=20*log10(mag_cl_1); % acceleration x_1'' dB
mag_cl_2=mag_cl(2,1,:);
mag_cl_2=mag_cl_2(1,:); % row vector
mag_cl_2=20*log10(mag_cl_2); % acceleration x_1'' dB
mag_cl_3=mag_cl(3,1,:);
mag_cl_3=mag_cl_3(1,:); % row vector
mag_cl_3=20*log10(mag_cl_3); % Actuator force

figure(8)
semilogx(w,mag_ol_1,w,mag_cl_1); % Bode plot
grid;
xlabel('ang freq rads/s')
ylabel('Acceleration m/s^2')
title('Freq response: nu->x1dd (OL and CL)')

figure(9)
semilogx(w,mag_ol_2,w,mag_cl_2); % Bode plot
grid;
xlabel('ang freq rads/s')
ylabel('Displacement (m)')
title('Freq response: nu->x1-x0 (OL and CL)')

figure(10)
semilogx(w,mag_cl_3); % Bode plot
grid;
xlabel('ang freq rads/s')
ylabel('Actuator force (N)')
title('Freq response: nu->U (CL)')
%
%------------------------------- end of lqr_msc_lab.m ------------------------------------
