%--------------------------------------------------------------------------
% Mass-spring model
%--------------------------------------------------------------------------
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); 

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

%--------------------------------------------------------------------------
% State space modeling
%--------------------------------------------------------------------------

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

syss=ss(A,B,C,D);

Mshape=es;  % modeshape matrix

%
%  Bode plot approach
%
freq=0.01:0.01:3;
[mago,phso]=bode(syss,(freq)*2*pi);

% mago=reshape(mago(:,:,:),sn,length(freq)); 
% phso=reshape(phso(:,:,:),sn,length(freq));

% plot the six (n*(n+1)/2) FRFs
nplot =0;
magnitude = zeros(1, length(freq));
for i=1:ndof;
for j= 1:ndof;
    nplot = nplot+1;
figure(nplot);
clf
magnitude(1,1:length(freq)) = mago(i,j,1:length(freq));
plot(freq,20*log10(magnitude));
xlabel('freq(Hz)');ylabel('mag(dB)'); 
legend(['FRF  H(',  num2str(i),',' ,num2str(j), ')' ])
title('Bode Plot');
end;
end;

%--------------------------------------------------------------------------
% FRF computation
%--------------------------------------------------------------------------

H33=freqresp(syss,(freq)*2*pi);
 
for i=1:ndof;
for j= 1:ndof;
    nplot=nplot+1;
figure(nplot);
clf
magnitude(1,1:length(freq)) = H33(i,j,1:length(freq));
magnitude = abs(magnitude);
plot(freq,20*log10(magnitude));
xlabel('freq(Hz)');ylabel('mag(dB)'); 
legend(['FRF  H(',  num2str(i),',' ,num2str(j), ')' ])
title('Freqresp Plot');
end;
end;