%
% PURPOSE: Compute the motion of the radar wrto inertially fixed NED
%          coordinates, using the logged nav440 data.
%
% CHANGE LOG: 1) R. McEwen 29 Oct 2020 - Initial.
%

  if(~exist('nav440')) 
    %nav440 = csvread('ahrs.csv',1);
    nav440 = csvread('nav440_imu_20200928.csv',1);
  end

  if(~exist('coast_eastings')) 
    load('coastdat_MontereyBayUTM.mat')
  end

% 1. pcap_time,
% 2. lcm_time,
% 3. latitude,
% 4. longitude,
% 5. yaw_true,
% 6. pitch,
% 7. roll,
% 8. x_accel,
% 9. y_accel,
% 10. z_accel,
% 11. n_velocity,
% 12. e_velocity,
% 13. d_velocity,
% 14. altitude,
% 15. x_rate,       (omega_x)
% 16. y_rate,       (omega_y)
% 17. z_rate,       (omega_z)
% 18. x_rate_temp
 
  deci = 100;


  if(~exist('position'))
    position = get(0,'screensize');
    npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
    npposition(3:4) = round([position(3)*.95 position(4)*.90]);
    position = npposition;
  end

  co = [0 0 1;
    0 0.5 0;
    1 0 0;
    0 0.75 0.75;
    0.75 0 0.75;
    0.75 0.75 0;
    0.25 0.25 0.25];
  set(groot,'defaultAxesColorOrder',co)

  if( exist( 'fig1' ) ~= 1 )
    fig1 = figure;
    figure(fig1);
  else
    figure(fig1);
    clf
  end
  set( fig1, 'Name', '28 Sept Paragon Data', 'DeleteFcn', 'clear fig1' );
  set(gcf,'position',position)
  set(gcf,'colormap',jet)
  if(~exist('fontsize')) fontsize = 20;end
  
  [eastings northings]=geo2utm(nav440(:,4),nav440(:,3),10);  

% Account for nav440 mounted backwards:
  yaw    = ( nav440(:,5) + 180 )*pi/180;
  pitch  =  -nav440(:,6)*pi/180;
  roll   =  -nav440(:,7)*pi/180;

% Angular vel of the ship wrto Newtonian, coordinatized in ship:  
  omega = [ -nav440(:,15) -nav440(:,16) nav440(:,17) ]*pi/180;

% Velocity of the Paragon in NED is:
  v_P = nav440(:,11:13)';

% Time:
  pcap_time = nav440(:,1);
  lcm_time  = nav440(:,2);
  
  
% Location of the radar wrto the nav440, in ship coordinates.  
  rr = [93 15 -70]'*.0254;
  
% Compute the velocity of the radar in NED coordinates.  First, the component
% due to the ship's angular motion, in ship coordinates:
  vv_B = cross(omega', rr*ones(1,length(omega)));

% Preallocate memory:
  vv_N      = NaN(3,length(omega));
  v_P_B     = NaN(3,length(omega));
  v_radar_B = NaN(3,length(omega));
  v_radar   = NaN(3,length(omega));
  
% Recoordinatize in NED and add the translational term v_P.

  for( i=1:length(omega) )
    cphi   = cos( roll(i) );
    sphi   = sin( roll(i) );
    ctheta = cos( pitch(i) );
    stheta = sin( pitch(i) );
    cpsi   = cos( yaw(i) );
    spsi   = sin( yaw(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; 

    
    vv_N(:,i) = T_N_B * vv_B(:,i);
    
    %
    % Add translational velocity.
    v_radar(:,i) = v_P(:,i) + vv_N(:,i);

    %
    % Now let's go the other way compute v_radar in ship coordinates.  So, we
    % just need convert and store v_radar in ship (B) coordinates:
    
    v_radar_B(:,i) = T_N_B'*v_radar(:,i);
    
    %
    % Let's also save velocity at the Ahrs:
    v_P_B(:,i) = T_N_B'*v_P(:,i);
    %
    % Check:
    %v_P_B_2(:,i)= vv_B(:,i) + v_P_B(:,i);
    
  end

  % Now flip these back to tall and thin:
  v_P       = v_P';
  v_P_B     = v_P_B';
  v_radar   = v_radar';
  v_radar_B = v_radar_B';

  %v_P_B_2   = v_P_B_2';
  
  mag = 20;
  quiver( eastings(1:deci:end),  northings(1:deci:end),...
          mag*sin(yaw(1:deci:end) ),mag*cos(yaw(1:deci:end) ),0 );

  set(gca,'fontsize',fontsize);
  grid on
  hold on

  plot( coast_eastings, coast_northings,'k' )
  
  quiver(eastings(1:deci:end), northings(1:deci:end),...
         nav440(1:deci:end,12), nav440(1:deci:end,11),'g');

  legend('Heading, no units','Monterey Coast','Velocity, m/s','location','northeastoutside')
  xlabel('Eastings, meters')
  ylabel('Northings, meters')
  title('Nav440 Data from the Paragon 28 Sept 2020')

  axis equal
  set(gca,'ylim',[4071887.46    4074507.98]);
  set(gca,'xlim',[605112.83     608990.87]);
  

  if( exist( 'fig2' ) ~= 1 )
    fig2 = figure;
    figure(fig2);
  else
    figure(fig2);
    clf
  end
  set( fig2, 'Name', '28 Sept Paragon Data', 'DeleteFcn', 'clear fig2' );
  set(gcf,'position',position)
  set(gcf,'colormap',jet)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)  
  
  time = pcap_time-pcap_time(1,1);
  
  subplot(3,1,1)
  plot(time,[v_P(:,1) v_radar(:,1)])
  grid on 
  hold on
  legend('Ahrs','Radar','location','northeastoutside');
  ylabel('North, m/s')
  title('Velocity of Nav440 Ahrs and Radar in NED Coordinates');
  
  subplot(3,1,2)
  plot(time,[v_P(:,2) v_radar(:,2)])
  grid on 
  hold on
  legend('Ahrs','Radar','location','northeastoutside');
  ylabel('East, m/s')
  
  subplot(3,1,3)
  plot(time,detrend([v_P(:,3) v_radar(:,3)]))
  grid on 
  hold on
  legend('Ahrs','Radar','location','northeastoutside');
  ylabel('Down, m/s')
  xlabel('Pcap Seconds Since Start')
  
  linkaxes(get(gcf,'children'),'x')
  

  if( exist( 'fig3' ) ~= 1 )
    fig3 = figure;
    figure(fig3);
  else
    figure(fig3);
    clf
  end
  set( fig3, 'Name', '28 Sept Paragon Data', 'DeleteFcn', 'clear fig3' );
  set(gcf,'position',position)
  set(gcf,'colormap',jet)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)  
  
  
  subplot(3,1,1)
  plot(time, pitch*180/pi, 'k--','linewidth',1)
  hold on
  plot(time,[v_P_B(:,1)],'b');
  plot(time,[v_radar_B(:,1)],'g');
  grid on 
  legend('Pitch','Ahrs','Radar','location','northeastoutside');
  ylabel('Along Track, m/s and Deg.')
  title('Velocity of Nav440 Ahrs and Radar in Ship Coordinates');
  
  subplot(3,1,2)
  plot(time, roll*180/pi, 'k--','linewidth',1)
  hold on
  plot(time,[v_P_B(:,2)],'b');
  plot(time,[v_radar_B(:,2)],'g');
  grid on 
  legend('Roll','Ahrs','Radar','location','northeastoutside');
  ylabel('Cross Track, m/s and Deg.')
  
  subplot(3,1,3)
  plot(time, pitch*180/pi, 'k--','linewidth',1)
  hold on
  plot(time,[detrend(v_P_B(:,3))],'b')
  plot(time,[detrend(v_radar_B(:,3))],'g')
  grid on 
  legend('Pitch','Ahrs','Radar','location','northeastoutside');
  ylabel('Down, m/s and Deg.')
  xlabel('Pcap Seconds Since Start')
  
  linkaxes(get(gcf,'children'),'x')


  if( exist( 'fig4' ) ~= 1 )
    fig4 = figure;
    figure(fig4);
  else
    figure(fig4);
    clf
  end
  set( fig4, 'Name', '28 Sept Paragon Data', 'DeleteFcn', 'clear fig4' );
  set(gcf,'position',position)
  set(gcf,'colormap',jet)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)  
  hold on;
  grid on;
  %
  % Sampling rate is 10 Hz.
  zz = cumtrapz(v_radar(:,3))*.1;
  %
  % Cutoff periods of 50, 100 and 500 seconds. If fc is cutoff then use fc/(fs/2).
  [b,a]=butter(4,1/50/5,'high');
  zz50  = filtfilt(b,a,zz);

  [b,a]=butter(4,1/100/5,'high');
  zz100 = filtfilt(b,a,zz);

  [b,a]=butter(4,1/500/5,'high');
  zz500 = filtfilt(b,a,zz);
  
  plot(time,[zz50 zz100 zz500]);
  
  xlabel('Pcap Seconds Since Start')
  ylabel('Vertical Speed of Radar, m/s')
  title('Filtered Vertical Speed of Radar')
  legend('Cutoff Period 50 Sec.',...
      'Cutoff Period 100 Sec.',...
      'Cutoff Period 500 Sec.');
%
% Save the relevant quantities:
  save Nav440Processed28Sept2020.mat nav440 eastings northings roll pitch yaw ...
       omega v_P v_P_B v_radar v_radar_B rr zz50
