%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Solution to Problem 4 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 the singular value decomposition
%  to solve a series of problems
%  specified by the data in the global variables A
%  and B (3x7).
%  We compute the orthogonal matrix Q that
%  minimizes   || B - Q A ||_F
%  (where "F" denotes the Frobenius norm).
%  The matrix Q is parameterized by the three Euler angles
%  that give yaw, pitch, and roll.
%
%  problem4.m   06/2004
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

anglsav = [];
qerror = [];
aerror = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 || B - Q A ||_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];
  [A,B,Qtrue] = makedata(angltrue);

  [rmsd,angl,t,Q,newB] = viewpoint(A,B,0);

  anglsav = [anglsav,angl];
  qerror = [qerror,norm(Qtrue-Q,'fro')];
  aerror = [aerror,rmsd];
end

[m,n]=size(anglsav);

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

subplot(2,2,2)
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 4')
v = axis;
axis([1 125 v(3) v(4)])

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