%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Solution to Problem 2 of
%  Achieving a Common Viewpoint: Yaw, Pitch, and Roll
%  Dianne P. O'Leary and David A. Schug
%  Computing in Science and Engineering
%
%  In this problem, we use a nonlinear least squares 
%  program lsqnonlin to solve a series of problems
%  specified by the data in the global variables myA
%  and myB (3x7).
%  We compute the orthogonal matrix Q that 
%  minimizes   || myB - Q myA ||_F  
%  (where "F" denotes the Frobenius norm).
%  The matrix Q is parameterized by the three Euler angles
%  that give yaw, pitch, and roll.
%
%  This method of solution is not to be recommended; 
%  use the technique of Problem 4 instead.
%
%  problem2.m   06/2004
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global myA myB

anglsav = [];
qerror = [];
aerror = [];
zz = optimset('MaxFunEvals',10000,'MaxIter',10000);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate the problems one by one and solve them.
% For each, the true yaw is pi/4 and the true roll is pi/9.
% The pitch varies in (-pi/2,pi/2).
% The function makedata is used to create each problem,
% and the function f evaluates || myB - Q myA ||_F^2.
% We save the error in Q, the rmsd, and the Euler angles
% for plotting.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for pitch = linspace(-pi/2,pi/2,121),
  angltrue = [pi/4,pitch,pi/9];
  [myA,myB,Qtrue] = makedata(angltrue);

  angl = [0;0;0];
  angl = lsqnonlin(@f,angl,[],[],zz);
  [fval,Qc,Bc] = f(angl);
  qerror = [qerror,norm(Qtrue-Qc,'fro')];
  aerror = [aerror,sqrt(fval/7)];

  anglsav = [anglsav,angl];
end

[m,n]=size(anglsav);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Make the plots, leaving room for the results of Problem 4.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

subplot(2,2,1)
plot(1:n,anglsav(1,:),'b+',1:n,anglsav(2,:),'go',1:n,anglsav(3,:),'rx')
%legend('Yaw','Pitch','Roll')
xlabel('sample number')
ylabel('angle(radians)')
title('Problem 2')
v = axis;
axis([1 125 v(3) v(4)])

subplot(2,2,3)
semilogy(1:n,qerror,'b+',1:n,aerror,'go')
%legend('Error in Q','RMSD')
xlabel('sample number')
ylabel('error')
title('Problem 2')
v = axis;
axis([1 125 v(3) v(4)])