clear all
clc
clf
%#############################################################
%  [input data files]
%record(test).txt :input, response(first floor), response(second floor)
%                       , response(third floor), response(forth floor)
%exactk(test).txt :exact stiffness of each story
%m(test).txt      :mass of each floor
%
%  [vairables to be specified]
%f0         :sampling frequency (Hz)
%idenfloor  :floor number for identification
%nb         :order of ARX model
%
%  [inner variables]
%nf         :number of floos
%N          :number of sample data
%mi         :mass of each floor (kg)
%Msum       :sum of mass (kg)
%record     :recorded data (gal)
%omega      :circlar frequency
%u          :input data
%y          :output data
%U          :Fourier transform of input data u
%Y          :Fourier transform of output data y
%Gnon       :non-parametric transfer function (=Y/U)
%fnon       :identification function using 'Gnon'
%G0         :parametric transfer function (ARX without constraint)
%fARX       :identification function using 'G0' (ARX without constraint)
%G1         :parametric transfer function (ARX with constraint)
%fARXc      :identification function using 'G1' (ARX with constraint)
%fARXcreG1  :identification function using 'Real[G1]' (ARX with constraint)
%#############################################################

%#############################################################
%Specified data
%#############################################################
%----------------------------
%sampling frequency
%----------------------------
f0 = 500; 
%----------------------------
%specify floor number of identification(1-4, 1:top floor)
%----------------------------
idenfloor = 4;
%----------------------------
%order of ARX model (na = nb)
%----------------------------
nb = 150;

%#############################################################
%Data setting
%#############################################################
%----------------------------
%input data
%----------------------------
record = importdata('record(test).txt');
N = 2^(ceil(log2(size(record,1))));
record = [record;zeros(N-size(record,1),size(record,2))];

omega =  2 * pi * f0 / N * (1:N)';

%----------------------------
%number of floors
%----------------------------
nf = size(record,2)-1;

%----------------------------
%mass 
%----------------------------
mi = importdata('m(test).txt');
Msum = sum(mi(idenfloor:nf));

%----------------------------
%Data for identification
%----------------------------
% ki = importdata('exactk.txt');
ki = importdata('exactk(test).txt');

%----------------------------
%Data for identification
%----------------------------
u = record(1:N,idenfloor);%response at lower floor
y = record(1:N,idenfloor+1);%response at upper floor


%###################################################################
%non-parametric System Identification (FFT)
%###################################################################
U = fft(u,N);
Y = fft(y,N);
Gnon = Y ./ U;
reGnon = real(Gnon);
imGnon = imag(Gnon);
fnon(:,1) = - Msum / 1000* (omega(:,1)).^2 .* (1./ (1./Gnon(:,1) - 1)) ;
refnon = real(fnon);%correspoinding to stiffness (N/mm)
imfnon = imag(fnon);


%###################################################################
%parametric System Identification (ARX)
%###################################################################
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%ARX without constraint
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%----------------------------
%Generate Matrix fai
%----------------------------
uu = zeros(N,nb);
for j = 1:nb
    uu(1+j:N,j) = u(1:N-j,1);
end
yy = zeros(N,nb);
for j = 1:nb
    yy(1+j:N,j) = y(1:N-j,1);
end
fai = [-yy,uu];

%----------------------------
%Estimation of ARX parameters
%----------------------------
R = fai' * fai;
ff = fai' * y;
theta = R \ ff;
a0 = theta(1:nb);
b0 = theta(nb+1:2*nb);

%----------------------------
%Transfer function
%----------------------------
a0r = [flipud(a0);1];
b0r = [flipud(b0);0];
q = 1 ./ exp(omega.*1i/f0);   
A0 = polyval(a0r,q);
B0 = polyval(b0r,q);

G0 = B0 ./ A0 ;
reG0 = real(G0);
imG0 = imag(G0);
abG0 = abs(G0);

%----------------------------
%Identification function
%----------------------------
fARX(:,1) = - Msum/1000* (omega(:,1)).^2 .* (1 ./ (1./G0(:,1) - 1)) ;
refARX = real(fARX);%correspoinding to stiffness (N/mm)
imfARX = imag(fARX);
fARXreG(:,1) = - Msum/1000* (omega(:,1)).^2 .* (1 ./ (1./reG0(:,1) - 1)) ;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%ARX with constraint
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%----------------------------
%Estimation of ARX parameters with constraint
%----------------------------
v(1:nb) = 1;
v(nb+1:2*nb) = -1;
v = v';
v0 = [v;0];
Rc = [R -v];
Rc = [Rc ; v0'];
ffc = [ff;-1]; 
thetac = Rc \ ffc;
a1 = thetac(1:nb);
b1 = thetac(nb+1:2*nb);
   
%----------------------------
%Transfer function 
%----------------------------
a1r = [flipud(a1);1];
b1r = [flipud(b1);0];
A1 = polyval(a1r,q);
B1 = polyval(b1r,q);

G1 = B1 ./ A1 ;
reG1 = real(G1);
imG1 = imag(G1);
abG1 = abs(G1);
      
%----------------------------
%Identification function
%----------------------------
fARXc(:,1) = - Msum/1000* (omega(:,1)).^2 .* ( 1 ./ ( 1./G1(:,1) - 1 ) ) ;
refARXc = real(fARXc);%correspoinding to stiffness (N/mm)
imfARXc = imag(fARXc);
fARXcreG1(:,1) = - Msum/1000* (omega(:,1)).^2 .* ( 1 ./ ( 1./reG1(:,1) - 1 ) ) ;
  

%----------------------------
%Plot
%----------------------------
hold off;
figure(10);
axis([0 20 0 50]);
hold on;
plot(omega,refnon,'LineStyle','-','Color','black','LineWidth',1);
plot(omega,refARX,'LineStyle','-','Color','blue','LineWidth',3);
plot(omega,fARXcreG1,'LineStyle','-','Color','red','LineWidth',3);
plot(0,ki(idenfloor),'LineStyle','none','Marker','o','MarkerSize',14,'MarkerFaceColor','k','MarkerEdgeColor','k');
legend('non-parametric','ARX without constraint','ARX with constraint','Exact stiffness');
xlabel('circular frequency \omega (rad/s)');
ylabel('real part of identification function Re[f(\omega)]');



