function rmsd()
X = [0.000   0.000   0.000;...
     1.520   0.000   0.000;...
     2.162   0.851  -1.083;...
     3.255   1.791  -0.598;...
     2.787   2.887   0.345;...
     1.360   2.737   0.847;...
     0.316   2.533  -0.239;...
    -0.642   1.378   0.000]';
Xs = set_mean(X);
Y = [0.000   0.000   0.000;...
     1.520   0.000   0.000;...
     2.162   0.091   1.375;...
     1.298   0.735   2.446;...
     0.975   2.202   2.216;...
     1.360   2.737   0.847;...
     0.316   2.533  -0.239;...
    -0.642   1.378   0.000]';
Ys = set_mean(Y);
Z = [0.000   0.000   0.000;...
     1.520   0.000   0.000;...
     2.162   0.055   1.376;...
     1.779   1.262   2.217;...
     0.430   1.159   2.911;...
    -0.612   2.161   2.441;...
    -1.442   1.714   1.249;...
    -0.642   1.378   0.000]';
%Zs = set_mean(Z);
Rxy = Xs*Ys';
%Ryz = Ys*Zs';
%Rzx = Zs*Xs';
N=4;
Fxy = quat_corr(Rxy);
[H, Q] = sym_hessenberg(Fxy)
% begin Rayleigh quotient iteration
% (1) find upper bound of largest eigenvalue (Gershgorin)
lam = max([H(1,1)+abs(H(2,1)),...
          H(2,2)+abs(H(2,1))+abs(H(3,2)),...
          H(3,3)            +abs(H(3,2))+abs(H(4,3)),...
          H(4,4)                        +abs(H(4,3))]);
  lam = 21;
% tridiagonal form
a = diag(H);
b = [diag(H,1);0];
c = [0;diag(H,-1)];
TH = [c a b];
% estimate first guess:
xx(1:N,1) = (1:N)';
x0 = LUTriPiv(TH,lam,xx)
x0 = x0'/norm(x0);
V = zeros(N,100);
%whos
V(:,1) = x0;
lambda(1) = Rayleigh(TH,x0);
for k = 2:100
    x0 = LUTriPiv(TH,lambda(k-1),V(:,k-1));
    x0 = x0'/norm(x0);
    V(:,k) = x0;
    lambda(k) = Rayleigh(TH,x0);
end
plot(1:100,lambda)

lambda

[VV,DD] = eig(H);
diag(DD)
function [H, Q] = sym_hessenberg(A)
% converts NxN A=A' to upper Hessenberg form using
% Givens transformations
%    / a11 a12 a13 a14 \         /h11 h12  0   0  \
%A = | a12 a22 a23 a24 | --> H = |h12 h22 h23  0  |
%    | a13 a23 a33 a34 |         | 0  h23 h33 h34 |
%    \ a14 a24 a34 a44 /         \ 0   0  h34 h44 /
H = A;
N=length(A);
for i = 1:N-2
    for j = N:-1:i+2
        % use [A(j-1,i),A(j,i)] to perform a Givens rotation;
        % rotate elements A(j-1:j,i+1:N) by G from left
        % rotate elements A(i+1:N,j-1:j) by G' from right
        a = H(j-1,i); b = H(j,i);
        a2 = a*a; b2 = b*b;
        r = sqrt(a2+b2); c = a/r; s = b/r;
        H(j-1,i) = r; H(i,j-1) = r;
        H(j  ,i) = 0; H(i,j  ) = 0;
        H(j-1:j,i+1:N) = [c s;-s c]*H(j-1:j,i+1:N);
        H(i+1:j-2,j-1:j) = H(j-1:j,i+1:j-2)';
        H(j-1:j,j-1:j) = H(j-1:j,j-1:j)*[c -s;s c];
        H(j+1:N,j-1:j) = H(j-1:j,j+1:N)';
        Q(i,j) = c; Q(j,i) = s;
    end
end
% end function sym_hessenberg
%------------------------------------
function x = recover_evec(y,Q)
N = length(y);
for i = N-2:-1:1
    for j = i+2:N
        c = Q(i,j); s = Q(j,i);
        x(j-1:j) = [c -s;s c]*y(j-1:j);
    end
end
%end function recover_evec
%------------------------------------
function Xs = set_mean(X)
[M,N] = size(X);
Xm = zeros(1:M,1); 
for i = 1:N
    Xm = Xm + X(:,i);
end
    Xm = (1/N)*Xm;
for i = 1:N
    Xs(:,i) = X(:,i) - Xm;
end
%end function set_mean
%------------------------------------
function F = quat_corr(R)
F(1,1) = R(1,1) + R(2,2) + R(3,3);
F(1,2) = R(2,3) - R(3,2);
F(1,3) = R(3,1) - R(1,3);
F(1,4) = R(1,2) - R(2,1);
F(2,1) = F(1,2);
F(2,2) = R(1,1) - R(2,2) - R(3,3);
F(2,3) = R(1,2) + R(2,1);
F(2,4) = R(3,1) + R(1,3);
F(3,1) = F(1,3);
F(3,2) = F(2,3);
F(3,3) =-R(1,1) + R(2,2) - R(3,3);
F(3,4) = R(2,3) + R(3,2);
F(4,1) = F(1,4);
F(4,2) = F(2,4);
F(4,3) = F(3,4);
F(4,4) =-R(1,1) - R(2,2) + R(3,3);
%end function quat_corr
%------------------------------------
function x = LUTriPiv(H,lam,R)
%solves (H-lam*I)x = b for x; H is tridiagonal
% uses partial pivoting
[N,M] = size(H);
r = R(1:N);
a = H(:,2) - lam;
b = H(:,3);
c = H(:,1);
d = zeros(N,1);
%piv(1:N) = 1:N;
if (abs(a(1)) >= abs(c(2)))
    u(1) = a(1);   e(1) = b(1);   f(1) = 0;
    l(2) = c(2)/a(1);
    next(1) = a(2) - b(1)*l(2);   next(2) = b(2);
else
    u(1) = c(2);   e(1) = a(2);   f(1) = b(2);
    l(2) = a(1)/c(2);
    next(1) = b(1) - a(2)*l(2);   next(2) = -l(2)*b(2);
%    piv(2) = 1;   piv(1) = 2;
    r(2) = 1;   r(1) = 2;
end
for i = 2:N-2
    if (abs(next(1)) >= abs(c(i+1)))
       u(i) = next(1);   e(i) = b(i);   f(i) = 0;
       l(i+1) = c(i+1)/a(i);
       next(1) = a(i+1) - b(i)*l(i+1);   next(2) = b(i+1);
    else
       u(i) = c(i+1);   e(i) = a(i+1);   f(i) = b(i+1);
       l(i+1) = a(i)/c(i+1);
       next(1) = b(i) - a(i+1)*l(i+1);   next(2) = -l(i+1)*b(i+1);
       rtmp = r(i);   r(i) = r(i+1);   r(i+1) = rtmp;
      %ptmp = piv(i);   piv(i) = piv(i+1);   piv(i+1) = ptmp;
    end
end
if (abs(next(1)) >= abs(c(N)))
    u(N-1) = next(1);
    e(N-1) = next(2);
    l(N) = c(N)/next(1);
    next(1) = a(N)-next(2)*l(N);
else
    u(N-1) = c(N);
    e(N-1) = a(N);
    l(N) = next(1)/c(N);
    next(1) = next(2)-a(N)*l(N);
    rtmp = r(N-1);   r(N-1) = r(N);   r(N) = rtmp;
  % ptmp = piv(N-1);   piv(N-1) = piv(N);   piv(N) = ptmp;
end
u(N) = next(1);
y = zeros(N,1);
% Solve L y = r
y(1) = r(1);
%whos
for i = 2:N
    y(i) = r(i) - l(i)*y(i-1);
end
%solve U x = y
x(N) = y(N)/u(N);
x(N-1) = (y(N-1) - e(N-1)*x(N))/u(N-1);
for i = N-2:-1:1
    x(i) = (y(i) - e(i)*x(i+1) - f(i)*x(i+2))/u(i);
end
%end function LUtriPiv
%------------------------------------
function lambda = Rayleigh(H,x0);
%Rayleigh quotient; tridiagonal H, unit input
[N,M] = size(H);
z(1) = H(1,2)*x0(1) + H(1,3)*x0(2);
for i = 2:N-1
    z(i) = H(i,1)*x0(i-1) + H(i,2)*x0(i) + H(i,3)*x0(i+1);
end
z(N) = H(N,1)*x0(N-1) + H(N,2)*x0(N);
lambda = x0(1)*z(1);
for i = 2:N
    lambda = lambda + x0(i)*z(i);
end
%end function Rayleigh
%------------------------------------