clear all
clc
% close all
% clf
%#############################################################
%input data
%gm.asc          :Mass matrix (kg)
%gk.asc          :Stiffness matrix (N/m)
%gc.asc          :Damping matrix (Ns/m)
%
%variables to be specified in this source file
%ntargetfloor    :floor number for identification
%f0              :sampling frequency
%N               :number of sample data
%nsample         :number of samples
%ro              :correlation of noises at the upper and lower floors
%level1          :noise level for the lower floor
%level2          :noise level for the upper floor
%sxorig          :spectrum of input ground motion
%r               :order of approximate curve 
%fl1             :lower limit of frequency range to fit approximate curve (fk1)
%fu1             :upper limit of frequency range to fit approximate curve (fk1)
%fl2             :lower limit of frequency range to fit approximate curve (fk2)
%fu2             :upper limit of frequency range to fit approximate curve (fk2)
%
%inner variables
%WZ              :noise at the lower floor (frequency domain)
%WN              :noise at the upper floor (frequency domain)
%SWZ             :power spectrum of noise at the lower floor
%SWN             :power spectrum of noise at the upper floor
%SWZWN           :cross spectrum of noises at the lower and upper floor
%H               :transfer function (response accerelation to input acceleration)
%Ugpp            :input ground motion (frequency domain)
%Z               :response at the lower floor (frequency domain)
%UN              :response at the upper floor (frequency domain)
%SZ              :power spectrum of response at the lower floor
%SN              :power spectrum of response at the upper floor
%SZN             :cross spectrum of reponses at the upper and lower floors
%fk0             :identification function without noise
%fk1             :identification function of property AA
%fk2             :identification function of property BB
%#############################################################

%#############################################################
%data setting
%#############################################################
%-------------------------
ntargetfloor = 1;   %floor number for identification (1:lowest floor)
%-------------------------

%-------------------------
% model data
%-------------------------
gm = importdata('gm.txt');
gc = importdata('gc.txt');
gk = importdata('gk.txt');
nf = size(gm,1);
mj = diag(gm);
Mj = sum(mj(ntargetfloor:nf));
Kj = diag(gk);
kj = [-diff(Kj) Kj(nf)]';
Cj = diag(gc);
cj = [-diff(Cj) Cj(nf)]';

%--------------------------
%speification of record
f0 = 250;       %sampling frequency
N = 2^11;       %number of data
nsample = 300;  %number of samples
%--------------------------

T = N/f0;   %time duration
f = f0 / N * (0:N/2)';  %frequency data

%--------------------------
%specification of noise
ro = 0.0;             %correlation of noise
level1 = sqrt(0.5);   % noise level to the lower floor (level1 = a)
level2 = sqrt(0.5);   % noise level to the upper floor (level2 = b)
sxorig = 1.0;         %Spectrum of noise
%--------------------------

%--------------------------
%specification of approximate curve fk1(f) = a * f^r + b (fl<f<fu)
r = 2;    %order of approximate curve 
fl1 = 3.0;   %lower limit of frequency range to fit the approximate curve (Hz)
fu1 = 5.0;   %upper limit of frequency range to fit the approximate curve (Hz)
fl2 = 2.5;   %lower limit of frequency range to fit the approximate curve (Hz)
fu2 = 3.5;   %upper limit of frequency range to fit the approximate curve (Hz)
%--------------------------


%#############################################################
%noises generated in frequency domain
%#############################################################
WZ = zeros(N,nsample);
WN = zeros(N,nsample);
Sx = sxorig;
theta = acos(ro);
if abs(theta) <= 1.d-15
    theta = 0.d0;
end
for i = 1:2:nsample
    a = (2 * pi) .* rand(N,1);
    WZ(:,i) = sqrt((T/2.0/pi)*Sx*level1^2) .*( cos(a) + 1.0i * sin(a));
    WN(:,i) = sqrt((T/2.0/pi)*Sx*level2^2) .*( cos(a+theta) + 1.0i * sin(a+theta));
end
for i = 2:2:nsample
    a = (2 * pi) .* rand(N,1);
    WZ(:,i) = sqrt((T/2.0/pi)*Sx*level1^2) .*( cos(a) + 1.0i * sin(a));
    WN(:,i) = sqrt((T/2.0/pi)*Sx*level2^2) .*( cos(a-theta) + 1.0i * sin(a-theta));
end


%--------------------------
%power spectrum and cross spectrum of noises
%--------------------------
pWZ = zeros(N,nsample);
pWN = zeros(N,nsample);
cWZWN = zeros(N,nsample);
for i = 1:nsample
    for j =1:N
        pWZ(j,i) = (2.0*pi/T) * WZ(j,i) .* conj(WZ(j,i));
        pWN(j,i) = (2.0*pi/T) * WN(j,i) .* conj(WN(j,i));
        cWZWN(j,i) = (2.0*pi/T) * conj(WZ(j,i)) .* WN(j,i);
    end
end

%--------------------------
%ensemble mean
%--------------------------
SWZ = zeros(N,1);
SWN = zeros(N,1);
SWZWN = zeros(N,1);
m = 0;
for i = 1:nsample
    m = m + 1;
    for j=1:N
        SWZ(j) = SWZ(j) + pWZ(j,i) ;
        SWN(j) = SWN(j) + pWN(j,i) ;
        SWZWN(j) = SWZWN(j) + cWZWN(j,i) ;
    end
end
SWZ = SWZ ./ m;
SWN = SWN ./ m;
SWZWN = SWZWN ./ m;

%--------------------------
%coherence of noises
%--------------------------
co = (abs(SWZWN).*abs(SWZWN)) ./ (SWZ.*SWN);
coh = sqrt(co);

%----------------------------
figure(10);
plot(f,abs(SWZ(1:N/2+1)) );
xlabel('frequency(Hz)')
title('SWZ(f) (noise)')
axis([0,10,0,1.2]);

figure(11);
plot(f,abs(SWN(1:N/2+1)) );
xlabel('frequency(Hz)')
title('SWN(f) (noise)')
axis([0,10,0,1.2]);

figure(12);
plot(f, abs(SWZWN(1:N/2+1)) );
xlabel('frequency(Hz)')
title('SWZWN(f) (noise)')
axis([0,10,0,1.2]);

figure(13);
plot(f, coh(1:N/2+1) );
xlabel('frequency(Hz)')
title('coh(f) (noise)')
axis([0,10,0,1.2]);
%----------------------------




%#############################################################
%response data generated in frequency domain
%#############################################################

omega = 2.0 * pi * f0 / N * (0:N-1);
one = ones(nf,1);
%-------------------------
%transfer function
%-------------------------
H = zeros(N,nf);
for i = 1:N
    awork = - omega(i)^2 * gm + 1.0i * omega(i) * gc + gk;
    H(i,:) = omega(i)^2 * awork^-1 * gm * one  +  one;
end


%------------------------------
%input ground motion in frequency domain
%------------------------------
Sx = sxorig;
Ugpp = zeros(N,nsample);
for i = 1:nsample
    a = (2 * pi) .* rand(N,1);
    Ugpp(:,i) = sqrt((T/2.0/pi)*Sx) .*( cos(a) + 1.0i * sin(a));
end
aH = abs(H);
aUgpp = abs(Ugpp);

%-------------------------
%response containing noise in frequency domain
%-------------------------
Z = zeros(N,nsample);
UN = zeros(N,nsample);
pZ = zeros(N,nsample);
pN = zeros(N,nsample);
cZN = zeros(N,nsample);
for i = 1:nsample
    %------------------
    %responses at the upper and lower floor
    %------------------
    %upper floor
    UN(:,i) = H(:,ntargetfloor) .* Ugpp(:,i);
    %lower floor
    if ntargetfloor == 1
        Z(:,i) = Ugpp(:,i);
    else
        Z(:,i) = H(:,ntargetfloor-1) .* Ugpp(:,i);
    end
    
    %------------------
    %addition of noise
    %------------------
%     Z(:,i) = Z(:,i) + WZ(:,i) * (level1 );
%     UN(:,i) = UN(:,i) + WN(:,i) * (level2 );
    Z(:,i) = Z(:,i) + WZ(:,i) ;
    UN(:,i) = UN(:,i) + WN(:,i) ;
    
    %------------------
    %power spectrum and cross spectrum
    %------------------
    for j =1:N
        pZ(j,i) = (2*pi/T) * Z(j,i) .* conj(Z(j,i));
        pN(j,i) = (2*pi/T) * UN(j,i) .* conj(UN(j,i));
        cZN(j,i) = (2*pi/T) * conj(Z(j,i)) .* UN(j,i);
    end
end

%-------------------------
%ensemble mean
%-------------------------
SZ = zeros(N,1);
SN = zeros(N,1);
SZN = zeros(N,1);
m = 0;
for i = 1:nsample
    m = m + 1;
    for j=1:N
        SZ(j) = SZ(j) + pZ(j,i) ;
        SN(j) = SN(j) + pN(j,i) ;
        SZN(j) = SZN(j) + cZN(j,i) ;
    end
end
SZ = SZ ./ m;
SN = SN ./ m;
SZN = SZN ./ m;

%-------------------------
%coherence
%-------------------------
co = (abs(SZN).*abs(SZN)) ./ (SZ.*SN);
coh2 = sqrt(co);

%----------------------------
figure(20);
plot(f,SZ(1:N/2+1) );
xlabel('frequency(Hz)')
title('S_Z (response)')
axis([0,10,0,10]);

figure(21);
plot(f,SN(1:N/2+1) );
xlabel('frequency(Hz)')
title('S_N (response)')
axis([0,10,0,10]);

figure(22);
plot(f, sqrt(SZN(1:N/2+1) .* conj(SZN(1:N/2+1))) );
xlabel('frequency(Hz)')
title('S_N_Z (response)')
axis([0,10,0,10]);
%----------------------------
%----------------------------
figure(23);
plot(f, coh2(1:N/2+1) );
xlabel('frequency(Hz)')
title('coh(f) (response)')
axis([0,10,0,1.2]);
%----------------------------

%#############################################################
%identification function (without noise)
%#############################################################
%upper floor
UN0 = H(:,ntargetfloor);
%lower floor
if ntargetfloor == 1
    Z0 = ones(size(UN0));
else
    Z0 = H(:,ntargetfloor-1);
end
fk0 = zeros(N,1);
for j = 1:N/2
    fk0(j)= - Mj * (2.0*pi*f(j))^2  * UN0(j)/(Z0(j)-UN0(j));
end

%#############################################################
%identification function (property AA)
%#############################################################
fk1 = zeros(N,1);
for j = 1:N/2
    fk1(j)= - Mj * (2.0*pi*f(j))^2 * SZN(j)/(SZ(j)-SZN(j));
end

%----------------------------
%approximate curve fk1(f) = a*f^n + b
%----------------------------
nl = round(fl1*N/f0);
nu = round(fu1*N/f0);
x2r = f(nl:nu).^(2*r);
xr = f(nl:nu).^r;
xry = f(nl:nu).^r.*real(fk1(nl:nu));
A = zeros(2,2);
A(1,1) = sum(x2r);
A(1,2) = sum(xr);
A(2,1) = sum(xr);
A(2,2) = nu-nl+1;
B = zeros(2,1);
B(1) = sum(xry);
B(2) = sum(real(fk1(nl:nu)));
C = A^-1*B;

%----------------------------
figure(1);
clf;
xlabel('frequency(Hz)')
ylabel('real part of identification function (Re[-\omega^2*M*fk_1(\omega)])')
title('Property AA')
axis([0,6,0,1500]);
hold on
plot(f(2:N/2+1),real(fk0(2:N/2+1)),'Color','k' );
plot(f(2:N/2+1),real(fk1(2:N/2+1)),'Color','r');
plot(f(2:N/2+1),C(2)+C(1)*f(2:N/2+1).^r,'LineStyle','--','Color','r','LineWidth',2);
plot(0,kj(ntargetfloor),'LineStyle','none','Marker','o','MarkerSize',14,'MarkerFaceColor','k','MarkerEdgeColo','k');
legend('without noise','with noise','extrapolation','Exact stiffness');
hold off
%----------------------------
figure(2);
clf;
xlabel('frequency(Hz)')
ylabel('imaginary part of identification function (Im[-\omega^2*M*fk_1(\omega)])')
axis([0,6,0,1500]);
hold on
plot(f(2:N/2+1),imag(fk0(2:N/2+1))  ,'Color','k');
plot(f(2:N/2+1),imag(fk1(2:N/2+1))  ,'Color','r');
plot(f(2:N/2+1),cj(ntargetfloor)*2*pi*f(2:N/2+1),'LineStyle','--','LineWidth',3,'Color','k');
legend('without noise','with noise','Exact gradient');
hold off
%----------------------------



%#############################################################
%identification function (property BB)
%#############################################################
fk2 = zeros(N,1);
for j = 1:N/2
    fk2(j)= - Mj * (2.0*pi*f(j))^2 * SN(j) / (conj(SZN(j)) - SN(j));
end

%----------------------------
%approximate curve fk1(f) = a*f^n + b
%----------------------------
nl = round(fl2*N/f0);
nu = round(fu2*N/f0);
x2r = f(nl:nu).^(2*r);
xr = f(nl:nu).^r;
xry = f(nl:nu).^r.*real(fk2(nl:nu));
A = zeros(2,2);
A(1,1) = sum(x2r);
A(1,2) = sum(xr);
A(2,1) = sum(xr);
A(2,2) = nu-nl+1;
B = zeros(2,1);
B(1) = sum(xry);
B(2) = sum(real(fk2(nl:nu)));
C = A^-1*B;

%----------------------------
figure(3);
clf;
xlabel('frequency(Hz)')
ylabel('real part of identification function (Re[-\omega^2*M*fk_2(\omega)])')
title('Property BB')
axis([0,6,0,1500]);
hold on
plot(f(2:N/2+1),real(fk0(2:N/2+1)) ,'Color','k');
plot(f(2:N/2+1),real(fk2(2:N/2+1)) ,'Color','r');
plot(f(2:N/2+1),C(2)+C(1)*f(2:N/2+1).^r,'LineStyle','--','Color','r','LineWidth',2);
plot(0,kj(ntargetfloor),'LineStyle','none','Marker','o','MarkerSize',14,'MarkerFaceColor','k','MarkerEdgeColo','k');
legend('without noise','with noise','extrapolation','Exact stiffness');
hold off
%----------------------------
figure(4);
clf;
xlabel('frequency(Hz)')
ylabel('imaginary part of identification function (Im[-\omega^2*M*fk_2(\omega)])')
axis([0,6,0,1500]);
hold on
plot(f(2:N/2+1),imag(fk0(2:N/2+1))  ,'Color','k');
plot(f(2:N/2+1),imag(fk2(2:N/2+1))  ,'Color','r');
plot(f(2:N/2+1),cj(ntargetfloor)*2*pi*f(2:N/2+1),'LineStyle','--','LineWidth',3,'Color','k');
legend('without noise','with noise','Exact gradient');
hold off
%----------------------------


rfk0 = real(fk0);
ifk0 = imag(fk0);
rfk1 = real(fk1);
ifk1 = imag(fk1);
rfk2 = real(fk2);
ifk2 = -imag(fk2);
