%
%   simultaneous_iteration.m    ver 1.1  July 14, 2004
%   by Tom Irvine   Email: tomirvine@aol.com
%  
%   Solution of the Generalized Eigenvalue Problem
%   Simultaneous Iteration with Gram-Schmidt Orthogonalization
%
clear K;
clear M;
clear U1;
clear Uhat1;
clear U2;
clear Uhat2;
clear yhat;
clear x;
clear L2;
clear A;
clear B;
clear nnn;
%
%  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 ]
%
%  Sample trial vectors
%
U1(:,1)=[1 2 3]';
U1(:,2)=[1 2 -1]';
%
nnn=size(U1);  % number of columns
n1=nnn(1,1);
n2=nnn(1,2);
n=n2;
%
for ik=1:10   % set for 10 trials
%
% Normalize trial eigenvectors with respect to mass matrix
%
    nn=U1'*M*U1;
%
    for i=1:2
        U1(:,i)= U1(:,i)/sqrt(nn(i,i));
    end
%
    U2=K\(M*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:n
        sum=[0;0;0];
        for j=1:(i-1)
            alpha=(yhat(:,j)'*M*x(:,i));   
            sum=sum+alpha*yhat(:,j);
        end
        y(:,i)=x(:,i)-sum;
        yhat(:,i)=y(:,i)/norm(y(:,i));
    end    
    Uhat2=yhat;
%
% Normalize trial eigenvectors with respect to mass matrix
% 
    nn=Uhat2'*M*Uhat2;
%
    for i=1:2
        Uhat2(:,i)= Uhat2(:,i)/sqrt(nn(i,i));
    end
%
% Solve for eigenvalue matrix L
%
    for i=1:n
        for j=1:n
            A(i,j)=Uhat2(i,j);
            B(i,j)=U2(i,j);
        end
    end
%    L=(inv(B)*A)';
     L=B\A;     
     U1=Uhat2;
end
disp(' Eigenvectors ')
Uhat2
disp(' Eigenvalues ')
L