%
%   simultaneous_iteration.m    ver 1.3  Sep 7, 2012
%   by Tom Irvine   Email: tomirvine@aol.com
%  
%   Solution of the Generalized Eigenvalue Problem via
%   Simultaneous Iteration with Gram-Schmidt Orthogonalization
%
clear K;
clear M;
clear U1;
clear Uhat1;
clear U2;
clear yhat;
clear x;
clear L;
clear L2;
clear A;
clear B;
clear nnn;
clear sum;
clear dof;
clear Ev;
clear fn;
clear sum;
%
%  Sample stiffness matrix K and mass matrix M
%
%%% K=[ 2 -1 0 ; -1 3 -2 ; 0 -2 2 ]
%%% M=[ 1 -0 0 ; 0 1 0   ; 0 0 2 ]
%
disp(' ');
disp(' Enter the mass matrix name ');
M=input(' ');
disp(' ');
disp(' Enter the stiffness matrix name ');
K=input(' ');
%
disp(' ');
disp(' Enter number of vectors ');
nv=input(' ');
%
disp(' ');
disp(' Enter number of iterations ');
niter=input(' ');
%
sz=size(M);
dof=sz(1);
%
U1=4*(0.5-rand(dof,nv)); 
%
nnn=size(U1);  % number of columns
%
n1=dof;
n2=nv;
n=nv;
%
Kinv=pinv(K);
KinvM=Kinv*M;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
for ik=1:niter   
%
% Normalize trial eigenvectors with respect to mass matrix
%
    nn=U1'*M*U1;
%
    for i=1:nv
        U1(:,i)= U1(:,i)/sqrt(nn(i,i));
    end
%
    U2=KinvM*U1;
%
% Determine first orthogonalized eigenvector yhat(:,1)
%
    x=U2;
    y=zeros(n1,n2);
    yhat=zeros(n1,n2);   
    y(:,1)=x(:,1);
    yhat(:,1)=y(:,1)/sqrt(y(:,1)'*M*y(:,1)); 
%
% Gram-Schmidt orthogonalization with respect to mass matrix
%
for i=2:nv
    for j=i:nv
        x(:,j)=x(:,j)-(yhat(:,i-1)'*M*x(:,j))*yhat(:,i-1);
    end
    nn=x(:,i)'*M*x(:,i);
    yhat(:,i)=x(:,i)/sqrt(nn);
end    
%
% Normalize trial eigenvectors with respect to mass matrix
% 
    nn=yhat'*M*yhat;
%
    for i=1:n
        yhat(:,i)= yhat(:,i)/sqrt(nn(i,i));
    end
%
% Solve for eigenvalue matrix L
%
     L=pinv(U2)*yhat;     
     U1=yhat;
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%% disp(' Eigenvalues ')
%%% L
%
fn=zeros(nv,1);
%
[Ev,yhat]=sort_eigenvalues_eigenvectors(L,yhat);
%
for i=1:nv
    fn(i)=sqrt(Ev(i))/(2*pi);
end
%
disp(' ');
disp(' n  eigenvalue  fn(Hz) ');
for i=1:nv
    out1=sprintf('%d  %8.4g  %8.4g ',i,Ev(i),fn(i));
    disp(out1);
end
%
disp(' ');
disp(' Display eigenvectors?  1=yes 2=no ');
ide=input(' ');
if(ide==1)
    disp(' ');
    disp(' Eigenvectors ')
    yhat
end