% compareVel.m
% Compare velocities from ATS and RTK and MPOS. 
% 
% Vivek Nadkarni
% Trimble
% November 3, 2004

new; 
loadCfg; 

str2 = directoryname;
while (length(str2) > 1)
  [testName,str2] = strtok(str2,'\');
end

plotMposVsAtsNED = 1; 
plotRtkVsMposNED = 0;
plotRtkVsAtsNED = 0;
testRtk = 0; 

%%%%%%%%
%% ATS 
%%%%%%%%

atsVel = load([directoryname 'ATS\ATStraj_NEDvel.txt']);
atsVelT = atsVel(:,1);
atsVelN = atsVel(:,2);
atsVelE = atsVel(:,3);
atsVelD = atsVel(:,4);


%%%%%%%%%%%%%%%%%
%% RTK Reference
%%%%%%%%%%%%%%%%%

if (testRtk)
  ref = load([directoryname 'ref_rec11.txt']);
  rtkT = ref(:,1);
  dLat = ref(:,8)*180/pi;
  dLon = ref(:,9)*180/pi;
  dEht = ref(:,10);

  geqaLat = 37 + 23/60 + 19.20459/3600; % degrees
  geqaLon = - (122 + 2/60 + 13.68926/3600); % degrees
  geqaEht  = -5.0875; % meters


  [dn, de, du] = deltablh_decimal(...
    geqaLat, ...
    geqaLon, ...
    geqaEht, ...
    geqaLat + dLat, ...
    geqaLon + dLon, ...
    geqaEht + dEht);

  rtkVelT = rtkT;
  rtkVelN = dn;
  rtkVelE = de;
  rtkVelD = -du;
end
  
%%%%%%%%
%% MPOS
%%%%%%%%

mposVel = load([directoryname 'MPOS\mposNEDvel.txt']);
mposVelT = mposVel(:,1);
mposVelN = mposVel(:,2);
mposVelE = mposVel(:,3);
mposVelD = mposVel(:,4);


%%%%%%%%%%%%%%%%%%%%%%
if (plotMposVsAtsNED)
%%%%%%%%%%%%%%%%%%%%%%

  newfigure;
  subplot(3,1,1);
  plot(atsVelT, atsVelN, '.');
  hold on;
  plot(mposVelT, mposVelN, 'm.');
  title('vel N');
  legend('ATS', 'MPOS');

  subplot(3,1,2);
  plot(atsVelT, atsVelE, '.');
  hold on;
  plot(mposVelT, mposVelE,'m.');
  ylabel('meters/second');
  title('vel E');

  subplot(3,1,3);
  plot(atsVelT, atsVelD, '.');
  hold on;
  plot(mposVelT, mposVelD, 'm.');
  title('vel D');
  xlabel('gps seconds');
  suptitlebar('MPOS vs ATS');


  atsSpeed = sqrt(atsVelN.^2 + atsVelE.^2 + atsVelD.^2);
  mposSpeed = sqrt(mposVelN.^2 + mposVelE.^2 + mposVelD.^2);
  
  newfigure;
  plot(atsVelT, atsSpeed, '.');
  hold on; 
  plot(mposVelT, mposSpeed, 'r.');
  

  [comT, atsIdx, mposIdx] = intersect(atsVelT, mposVelT);

  errN = mposVelN(mposIdx) - atsVelN(atsIdx); 
  errE = mposVelE(mposIdx) - atsVelE(atsIdx); 
  errD = mposVelD(mposIdx) - atsVelD(atsIdx);
  
  seg = find(atsSpeed>0.01); 
  
  segN = find(~isnan(errN(seg)));
  segE = find(~isnan(errE(seg)));
  segD = find(~isnan(errD(seg)));
  
  muN = mean(errN(seg(segN)));
  muE = mean(errE(seg(segE)));
  muD = mean(errD(seg(segD)));
  sigN = std(errN(seg(segN)));
  sigE = std(errE(seg(segE)));
  sigD = std(errD(seg(segD)));
  
  newfigure;
  subplot(3,1,1);
  plot(comT, errN , '.');
  grid minor;
  ylim([-0.2 0.2]);
  titlestr = sprintf('vel N (mpos - ats):\\mu=%f \\sigma=%f m/s when in motion', muN, sigN);
  title(titlestr);

  subplot(3,1,2);
  plot(comT, errE , '.');
  grid minor;
  ylabel('meters/second');
  ylim([-0.2 0.2]);
  titlestr = sprintf('vel E (mpos - ats):\\mu=%f \\sigma=%f m/s when in motion', muE, sigE);
  title(titlestr);
  
  subplot(3,1,3);
  plot(comT, errD, '.');
  grid minor;
  ylim([-0.2 0.2]);
  xlabel('gps seconds');
  titlestr = sprintf('vel D (mpos - ats):\\mu=%f \\sigma=%f m/s when in motion', muD, sigD);
  title(titlestr);

  
  mposVelErrTotal = sqrt( errN.^2 + errE.^2 + errD.^2 );

  newfigure;
  plot(comT, mposVelErrTotal);
  title('Total mPOS velocity error');
  xlabel('seconds');
  ylabel('meters/second');
  
  
  %  suptitlebar('MPOS vs ATS');

  % New figure
  newfigure;
  subplot(6,1,1);
  plot(atsVelT, atsVelN, '.');
  hold on;
  plot(mposVelT, mposVelN, 'm.');
  title('vel N');
  % legend('ATS', 'MPOS');

  subplot(6,1,2);
  plot(atsVelT, atsVelE, '.');
  hold on;
  plot(mposVelT, mposVelE,'m.');
  title('vel E');

  subplot(6,1,3);
  plot(atsVelT, atsVelD, '.');
  hold on;
  plot(mposVelT, mposVelD, 'm.');
  title('vel D');
  % suptitlebar('MPOS vs ATS');
  xlimits = xlim; 

  [comT, atsIdx, mposIdx] = intersect(atsVelT, mposVelT);

  subplot(6,1,4);
  plot(comT, mposVelN(mposIdx) - atsVelN(atsIdx) , '.');
  grid minor;
  xlim(xlimits);
  ylim([-0.2 0.2]);
  title('vel N (mpos - ats)');

  subplot(6,1,5);
  plot(comT, mposVelE(mposIdx) - atsVelE(atsIdx), '.');
  grid minor;
  xlim(xlimits);
  ylim([-0.2 0.2]);
  title('vel E (mpos - ats)');

  subplot(6,1,6);
  plot(comT, mposVelD(mposIdx) - atsVelD(atsIdx), '.');
  grid minor;
  xlim(xlimits);
  ylim([-0.2 0.2]);
  title('vel D (mpos - ats)');

  %  suptitlebar('MPOS vs ATS');
  
end

%%%%%%%%%%%%%%%%%%%%%%
if (plotRtkVsMposNED)
%%%%%%%%%%%%%%%%%%%%%%

  newfigure;
  subplot(3,1,1);
  plot(rtkVelT, rtkVelN, '.');
  hold on;
  plot(mposVelT, mposVelN, 'm.');
  title('vel N');
  legend('MPOS', 'rtk');

  subplot(3,1,2);
  plot(rtkVelT, rtkVelE, '.');
  hold on;
  plot(mposVelT, mposVelE,'m.');
  title('vel E');

  subplot(3,1,3);
  plot(rtkVelT, rtkVelD, '.');
  hold on;
  plot(mposVelT, mposVelD, 'm.');
  title('vel D');
  suptitlebar('RTK vs MPOS');


  [comT, rtkIdx, mposIdx] = intersect(rtkVelT, mposVelT);

  newfigure;
  subplot(3,1,1);
  plot(comT, mposVelN(mposIdx) - rtkVelN(rtkIdx) , '.');
  grid minor;
  title('vel N');

  subplot(3,1,2);
  plot(comT, mposVelE(mposIdx) - rtkVelE(rtkIdx), '.');
  grid minor;
  title('vel E');

  subplot(3,1,3);
  plot(comT, mposVelD(mposIdx) - rtkVelD(rtkIdx), '.');
  grid minor;
  title('vel D');
  suptitlebar('RTK vs MPOS');

end


%%%%%%%%%%%%%%%%%%%%%%
if (plotRtkVsAtsNED)
%%%%%%%%%%%%%%%%%%%%%%

  newfigure;
  subplot(3,1,1);
  plot(atsVelT, atsVelN, '.');
  hold on;
  plot(rtkVelT, rtkVelN, 'm.');
  title('vel N');
  legend('ats', 'rtk');

  subplot(3,1,2);
  plot(atsVelT, atsVelE,'.');
  hold on;
  plot(rtkVelT, rtkVelE, 'm.');
  title('vel E');

  subplot(3,1,3);
  plot(atsVelT, atsVelD, '.');
  hold on;
  plot(rtkVelT, rtkVelD, 'm.');
  title('vel D');
  suptitlebar('RTK vs ATS');


  [comT, rtkIdx, atsIdx] = intersect(rtkVelT, atsVelT);

  newfigure;
  subplot(3,1,1);
  plot(comT, rtkVelN(rtkIdx) - atsVelN(atsIdx), '.');
  grid minor;
  title('vel N');

  subplot(3,1,2);
  plot(comT,rtkVelE(rtkIdx) - atsVelE(atsIdx), '.');
  grid minor;
  title('vel E');

  subplot(3,1,3);
  plot(comT,rtkVelD(rtkIdx) - atsVelD(atsIdx) , '.');
  grid minor;
  title('vel D');
  suptitlebar('RTK vs ATS');

end


