% alternate similar implementation of hartley2.m
%
% gives same answer as hartley2.m and hartley2b.m

function K=aaa(p)

[M,N]=size(p);
if N~=8
  error('hartley2: parameter list must be an M by 8 matrix')
end%if
if M<=3
  error('hartley2: parameter list must be long enough to give good estimate')
end%if

Q=[];   % initialize
P=[];
for m=1:M
  p11=p(m,1); p12=p(m,2); p13=p(m,3);
  p21=p(m,4); p22=p(m,5); p23=p(m,6);
  p31=p(m,7); p32=p(m,8); p33=1;

  %%% q=pinverse(p(m,:));  wrong: should be conjtranspose inverse
  thisP=[p(m,1) p(m,2) p(m,3); p(m,4) p(m,5) p(m,6); p(m,7) p(m,8) 1];
  thisQ=inv(thisP');
  thisQ=thisQ/thisQ(3,3);
  q=[thisQ(1,1) thisQ(1,2) thisQ(1,3) thisQ(2,1) thisQ(2,2) thisQ(2,3)...
                                             thisQ(3,1) thisQ(3,2)];
  q11=q(1); q12=q(2); q13=q(3);
  q21=q(4); q22=q(5); q23=q(6);
  q31=q(7); q32=q(8); q33=1;

  Qm=[q11 q21 q31 0 0 0;...
      q12 q22 q32 0 0 0;...
      q13 q23 q33 0 0 0;...
...
      0 q11 0 q21 q31 0;...
      0 q12 0 q22 q32 0;...
      0 q13 0 q23 q33 0;...
...
      0 0 q11 0 q21 q31;...
      0 0 q12 0 q22 q32;...
      0 0 q13 0 q23 q33...
     ];
  Q=[Q;Qm];

  Pm=[p11 p12 p13 0   0   0;...
      0   p11 0   p12 p13 0;...
      0   0   p11 0   p12 p13;...
...
      p21 p22 p23 0   0   0;...
      0   p21 0   p22 p23 0;...
      0   0   p21 0   p22 p23;...
...
      p31 p32 p33 0   0   0;...
      0   p31 0   p32 p33 0;...
      0   0   p31 0   p32 p33...
     ];
  P=[P;Pm];

end%for

X=Q-P; % the ``X'' of Hartley, page 8

[v d]=eig(X'*X);
d=diag(d);
[trash location]=min(d);  % choose least eigenvalue
answer=v(:,location);

C=[answer(1) answer(2) answer(3);...
   answer(2) answer(4) answer(5);...
   answer(3) answer(5) answer(6);...
  ];
%%K=chol(C); wrong because C=KK' not K'K
K=inv(chol(inv(C)));
K=K/K(3,3);