
t = [0:500]'/500*2*pi*5;

phi   = cos(t)*pi/6;
theta = sin(t)*pi/6;
psi   = sin(2*t)*pi/6;
psi   = 0*t;

% Let's walk through all 4 beams just to be sure:
%
c3s2 = 1./sqrt(2.)/2.;   %cos(pi/4)*sin(pi/6),   pi/6 = 30 degrees.
s3s2 = 1./sqrt(2.)/2.;   %sin(pi/4)*sin(pi/6)
c2   = sqrt(3.)/2.;      %cos(pi/6)

beamHat = [ c3s2 -c3s2 c3s2 -c3s2;
           -s3s2  s3s2 s3s2 -s3s2;
	     c2    c2   c2    c2  ];
for(i=1:length(t))  
  %
  % Putting the above E.A.'s in the standard 3-2-1 should produce a nadir vector:
  %
  cpsi   = cos( psi(i) );
  spsi   = sin( psi(i) );
  ctheta = cos( theta(i) );
  stheta = sin( theta(i) );
  cphi   = cos( phi(i) );
  sphi   = sin( phi(i) );
  %
  % Form the direction cosine matrix that recoordinatizes a vector from the 
  % body frame (B) into the local-level-local-vertical (LV) navigation 
  % frame, which is (North, East, Down).  See Fossen, p.10, Eqn 2.11.
  % Here we will assume the LV frame is intertially fixed and thus
  % also a Newtonian (N) frame.
  %
  % The transformation T_B_N, is 3-2-1 Euler.  The matrix below, T_N_B, is
  % the transpose.
  %
  %
  T_N_B(1,1) =  cpsi.*ctheta; 
  T_N_B(2,1) =  spsi.*ctheta; 
  T_N_B(3,1) = -stheta; 

  T_N_B(1,2) = -spsi.*cphi + cpsi.*stheta.*sphi; 
  T_N_B(2,2) =  cpsi.*cphi + sphi.*stheta.*spsi; 
  T_N_B(3,2) =  ctheta.*sphi; 

  T_N_B(1,3) =  spsi.*sphi + cpsi.*cphi.*stheta; 
  T_N_B(2,3) = -cpsi.*sphi + stheta.*spsi.*cphi;
  T_N_B(3,3) =  ctheta.*cphi; 

  beamIndx = [2 3 1 4];
  %fprintf('Beam %d:\n',beamIndx(i));
  %vec_N = T_N_B * beamHat(:,beamIndx(i))

  cmd_alt = 75;
  
  beamNum = 1;
  vec_N  = T_N_B * beamHat(:,beamNum);
  angle(i,beamNum) = asin(norm(cross(vec_N, [ 0 0 1 ]')));  % in N frame now
  mag(i,beamNum) = cmd_alt/cos(angle(i,beamNum));

  beamNum = 2;
  vec_N  = T_N_B * beamHat(:,beamNum);
  angle(i,beamNum) = asin(norm(cross(vec_N, [ 0 0 1 ]')));  % in N frame now
  mag(i,beamNum) = cmd_alt/cos(angle(i,beamNum));

  beamNum = 3;
  vec_N  = T_N_B * beamHat(:,beamNum);
  angle(i,beamNum) = asin(norm(cross(vec_N, [ 0 0 1 ]')));  % in N frame now
  mag(i,beamNum) = cmd_alt/cos(angle(i,beamNum));

  beamNum = 4;
  vec_N  = T_N_B * beamHat(:,beamNum);
  angle(i,beamNum) = asin(norm(cross(vec_N, [ 0 0 1 ]')));  % in N frame now
  mag(i,beamNum) = cmd_alt/cos(angle(i,beamNum));

  %  ***** Now run it backwards using the math from Navigation/Dvl.cc. *****
  %
  % That is, compute the altitude which should remain constant at cos(pi/6).

  offset = pi/18;   % 10 degrees
  ctheta = cos( theta(i) + offset );
  stheta = sin( theta(i) + offset );

   alt(i,1) = ( -stheta*c3s2  -  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*mag(i,1);
   alt(i,2) = (  stheta*c3s2  +  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*mag(i,2);
   alt(i,3) = ( -stheta*c3s2  +  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*mag(i,3);
   alt(i,4) = (  stheta*c3s2  -  ctheta*sphi*s3s2  +  ctheta*cphi*c2 )*mag(i,4);
   
   altitude(i,1) = (alt(i,1)+alt(i,2)+alt(i,3)+alt(i,4))/4;

end





figure;
plot(t, [ phi theta psi mag ]);
hold on
plot(t, altitude,'m','linewidth',3)
grid on
xlabel('Time')
ylabel('Pitch, Magnitude')
set(gca,'fontsize',20)




