%
% SAMPLE MATLABcode for homework on FRFs computation

%--------------------------------------------------------------------------

% Mass-spring model

%--------------------------------------------------------------------------
clear all;
M=diag([1 1 1]);
k1 =1.0; k2 =  10; k3=100;  k4=100;

K=[
k1+k2  -k2  0;
-k2 k2+k3 -k3;
0 -k3  k3+k4];

% perform eigenvalue problem
[es,ev]=eig(K,M);

% temp=2*diag([0.01 0.01 0.02 0.02 0.01 0.02])*sqrt(ev); % damping

% Dmatrix=inv(es')*temp*inv(es);
ndof = size(K,1);
Dmatrix = zeros(ndof); 


%--------------------------------------------------------------------------

% State space modeling

%--------------------------------------------------------------------------
% Consider the full MIMO system
% BB=eye(ndof);  % actuator location
BB=[1 0 0]'; 
sensorL=eye(ndof); % sensor location
sn=size(sensorL,1); % sensor number

A=[zeros(ndof)  eye(ndof); -inv(M)*K -inv(M)*Dmatrix];
B=[zeros(ndof,1); inv(M)*BB];
%C=sensorL*[zeros(ndof) eye(ndof)]*A;  % acceleration output
C=sensorL*[eye(ndof) zeros(ndof)]; % displacement output
%C= [1 1 1 0 0 0]; % displacement output
%D=sensorL*[zeros(ndof) zeros(ndof)]*B;
D=[0 0 0 0 0 0]*B;
syss=ss(A,B,C,D);

freq=0:0.01:10;
%--------------------------------------------------------------------------
% Analytical FRF computation
%--------------------------------------------------------------------------

H33=freqresp(syss,(freq)*2*pi);

nplot=0; 
for i=1:ndof;
for j= 1:1;
    nplot=nplot+1;
figure(nplot);

magnitude(1,1:length(freq)) = H33(i,j,1:length(freq));
magnitude = abs(magnitude);
plot(freq,20*log10(magnitude));  %db  vs freq
% semilogy(freq, magnitude);  %logy vs freq

xlabel('freq(Hz)');ylabel('mag(dB)'); 
legend(['FRF  H(',  num2str(i),',' ,num2str(j), ')' ])
title('Freqresp Plot');
end;
end;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   Simulate the output under random excitations
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% IF THE FRFS PEAKS are not sharp enough, then increase 
%     the number of N (fft sample number)
%     and halve teh step size h until youare satisfied!!!
%

N = 1024; %number of samples per ensemble
os = 1;  %number of times to oversample the forcing function and the responses, NOT used!
nens=1; % number of ensemble. INCREASING ENSEMBLES WILL IN GENERAL IMPROVE THE RESULTS! 
h = 1/20;  
  

%generate forcing function
%F = burstrandom(nens*N*os, os*N,1);


% obtain the output response
dh = h/os;  t_max = nens*h*N - dh;  
t=0:dh:t_max;
F= 1000*rand(1,size(t,2)); 

y = lsim(full(A), full(B),  full(C), full(D), F',t);	% response of system
y=y';

nplot= nplot+1;
figure(nplot);
plot(t,F);  
nplot = nplot+1;
figure(nplot);
plot(t,y(1,:));
nplot = nplot+1;
figure(nplot);
plot(t,y(2,:));
nplot = nplot+1;
figure(nplot);
plot(t,y(3,:));


%y=aaf(y,os); % anti-aliasing of output and input
%F=aaf(F,os); % down sample!

freq = (0:(N/2-1))/(N*h);
H =zeros(3, N); G =zeros(3, N);

% perform FFT
for i = 1:nens   %number of ensemble
finput = F(1,((i-1)*N+1): (i*N) );
xoutput = y(:,((i-1)*N+1): (i*N));
Fin  = fft(finput.*hanning(N)');   % use hanning time filter
%Fin  = fft(finput);
 for j=1:3;
Xout(j,:) = fft(xoutput(j,:).*hanning(N)');  % use hanning time filter
%Xout(j,:) = fft(xoutput(j,:));
 end;

 for k=1:N;
	G(:,k) = Xout(:,k)*conj(Fin(:,k)) *pinv(Fin(:,k) *Fin(:,k)');
end;
H = H + G;
end;
H =H/nens;
for i=1:3;
figure(i+nplot);
plot(freq, 20*log10(abs(H(i, 1:N/2))), 'k');
%semilogy(freq,  (abs(H(i, 1:N/2))), 'k');
end;