%
%
% The goal is to take compare the Roll/Pitch/Heading (R/P/H) produced by
% a reference system, e.g. POSPac to the R/P/H device under test. 
%
% Initially we simply used correctRollPitchHeading.m to remove the
% estimated mechanical mis-alignment between the two sensors and then
% subtracted the R/P/H values. Apparently this isn't correct, this
% script performs a more rigorous R/P/H comparison using Mathematics
% supplied by Applanix "Computation of Orientation Difference" provided
% by Jan Zywiel.
%
% Input:
%  Reference system roll, pitch, heading [all in degrees]
%  Device under test (DUT) R/P/H [all in degrees]
%
% Returned:
%  RollDelta PitchDelta HeadingDelta [in degrees]
%

function [deltaRoll deltaPitch deltaHeading] = compareRollPitchHeading(rollRef,pitchRef,headRef, ...
                                                                       rollDUT,pitchDUT,headDUT)

% Convert to Radians

rollRef  = rollRef  * pi/180;
pitchRef = pitchRef * pi/180;
headRef  = headRef  * pi/180;

rollDUT  = rollDUT  * pi/180;
pitchDUT = pitchDUT * pi/180;
headDUT  = headDUT  * pi/180;


% TODO - this could be made a lot faster....
for i=1:length(rollRef)

  roll    = rollRef(i);
  pitch   = pitchRef(i);
  heading = headRef(i);

  Cbg_ref(1,1) = cos(pitch)*cos(heading);
  Cbg_ref(1,2) = sin(roll)*sin(pitch)*cos(heading) - cos(roll)*sin(heading);
  Cbg_ref(1,3) = cos(roll)*sin(pitch)*cos(heading) + sin(roll)*sin(heading);

  Cbg_ref(2,1) = cos(pitch)*sin(heading);
  Cbg_ref(2,2) = sin(roll)*sin(pitch)*sin(heading) + cos(roll)*cos(heading);
  Cbg_ref(2,3) = cos(roll)*sin(pitch)*sin(heading) - sin(roll)*cos(heading);

  Cbg_ref(3,1) = -sin(pitch);
  Cbg_ref(3,2) =  sin(roll)*cos(pitch);
  Cbg_ref(3,3) =  cos(roll)*cos(pitch);

  roll    = rollDUT(i);
  pitch   = pitchDUT(i);
  heading = headDUT(i);

  Cbg_DUT(1,1) = cos(pitch)*cos(heading);
  Cbg_DUT(1,2) = sin(roll)*sin(pitch)*cos(heading) - cos(roll)*sin(heading);
  Cbg_DUT(1,3) = cos(roll)*sin(pitch)*cos(heading) + sin(roll)*sin(heading);

  Cbg_DUT(2,1) = cos(pitch)*sin(heading);
  Cbg_DUT(2,2) = sin(roll)*sin(pitch)*sin(heading) + cos(roll)*cos(heading);
  Cbg_DUT(2,3) = cos(roll)*sin(pitch)*sin(heading) - sin(roll)*cos(heading);

  Cbg_DUT(3,1) = -sin(pitch);
  Cbg_DUT(3,2) =  sin(roll)*cos(pitch);
  Cbg_DUT(3,3) =  cos(roll)*cos(pitch);
  
  
  Cbb = Cbg_ref'*Cbg_DUT;

  deltaRoll(i)    = atan2(  Cbb(3,2) , Cbb(3,3)) * 180/pi;
  deltaPitch(i)   = atan2( -Cbb(3,1) , sqrt(Cbb(1,1)^2 + Cbb(2,1)^2)) * 180/pi;
  deltaHeading(i) = atan2(  Cbb(2,1) , Cbb(1,1)) * 180/pi;

end


