% projectLosv.m
% 
% Project the MPOS velocity on the Line of Sight Vector to a
% Satellite.
%
% Vivek Nadkarni
% November 23, 2004
% Trimble

new; 

loadData;

sv = 5; 

% Select az and el from base data file. 
seg = find( baseData(:,dkey.SVID) == sv );
az = baseData(seg,dkey.AZ)*pi/180;
el = baseData(seg,dkey.ELV)*pi/180;
baseT = baseData(seg,dkey.TIME);

% Compute direction cosine vectors to SV. Each row corresponds to
% a direction cosine vector. 
dcosNED = [cos(el).*cos(az), cos(el).*sin(az), -sin(el)]; 


%%%%%%%%
%% MPOS
%%%%%%%%
mposVel = load([directoryname 'MPOS\mposNEDvel.txt']);

% atsVel = load([directoryname 'ATS\ATStraj_NEDvel.txt']);
% warning('Kludge: replacing mpos vel with ats vel');
% mposVel = atsVel; 

mposVelT = mposVel(:,1);
% mposVelN = mposVel(:,2);
% mposVelE = mposVel(:,3);
% mposVelD = mposVel(:,4);

[comT, baseIdx, mposIdx] = intersect(baseT, mposVelT);

computedLosv = sum(dcosNED(baseIdx,:).*mposVel(mposIdx,2:4),2);


if (exist([directoryname 'gielog_nco.txt'], 'file'))
  gieNco = load([directoryname 'gielog_nco.txt']);
  seg = find(gieNco(:,2) == sv);
  gieT = gieNco(seg,1)/1000;
  gieLosv = gieNco(seg,9);
elseif (exist([directoryname 'gielog_Ldiag.txt'], 'file'))
  gieNco = load([directoryname 'gielog_Ldiag.txt']);
  seg = find(gieNco(:,2) == sv);
  gieT = gieNco(seg,1)/1000;
  gieLosv = gieNco(seg,16);
end
    

% mposVelN = interp1(comT, mposVel(mposIdx,2), gieT);
% mposVelE = interp1(comT, mposVel(mposIdx,3), gieT);
% mposVelD = interp1(comT, mposVel(mposIdx,4), gieT);

losvAtGieT = interp1(comT, computedLosv, gieT);

% [newComT, gieIdx, losvIdx] = intersect(comT, gieT);

newfigure;
plot(gieT, gieLosv, 'r'); 
hold on; 
plot(comT,computedLosv);
grid minor; 
legend('Recorded LOSV', 'Computed LOSV', -1);
title('Recorded LOSV vs Computed LOSV');
xlabel('gps seconds');
ylabel('meters/second');

newfigure;
plot(gieT, gieLosv, 'r.'); 
hold on; 
plot(comT,computedLosv, '.');
grid minor; 
legend('Recorded LOSV', 'Computed LOSV', -1);
title('Recorded LOSV vs Computed LOSV');
xlabel('gps seconds');
ylabel('meters/second');
% subplot(3,1,3);
% title('Recorded minus Computed LOSV');


errLosv = gieLosv - losvAtGieT; 

seg = find(~isnan(errLosv));
muL = mean(errLosv(seg));
sigL = std(errLosv(seg));

newfigure;
plot(gieT, errLosv);
titlestr = sprintf('Recorded minus Computed LOSV: \\mu=%f \\sigma=%f m/s', ...
  muL, sigL); 
xlabel('gps seconds');
ylabel('meters/second');
title(titlestr);


movavgB = [0.2 0.2 0.2 0.2 0.2];
movavgA = [1];
% smoothGieLosv = filter(movavgB,movavgA, gieLosv);
smoothGieLosv = conv(movavgB, gieLosv);
smoothGieLosv = smoothGieLosv(3:end-2);

% smoothComputedLosv = filter(movavgB,movavgA, losvAtGieT);
smoothComputedLosv = conv(movavgB, losvAtGieT);
smoothComputedLosv = smoothComputedLosv(3:end-2);

highpassGieLosv = gieLosv - smoothGieLosv;
highpassComputedLosv = losvAtGieT - smoothComputedLosv;

newfigure;
plot(gieT, highpassGieLosv, 'r'); 
hold on; 
plot(gieT,highpassComputedLosv);
grid minor; 
legend('HP Recorded LOSV', 'HP Computed LOSV', -1);
titlestr = sprintf( ...
  'Highpass Rec. LOSV (\\mu=%.3e,\\sigma=%f) vs Highpass Comp. LOSV (\\mu=%.3e,\\sigma=%f)', ...
  mean(highpassGieLosv), ...
  std(highpassGieLosv), ...
  mean(highpassComputedLosv), ...
  std(highpassComputedLosv));
title(titlestr);
xlabel('gps seconds');
ylabel('meters/second');


%%%%%%%%%%%

seg = find(abs(losvAtGieT) > 0.05); 
newfigure;
plot(gieT(seg), highpassGieLosv(seg), 'r.'); 
hold on; 
plot(gieT(seg),highpassComputedLosv(seg),'.');
grid minor; 
legend('HP Recorded LOSV', 'HP Computed LOSV', -1);
titlestr = sprintf( ...
  'Motion: Highpass Rec. LOSV (\\mu=%.3e,\\sigma=%f) vs Highpass Comp. LOSV (\\mu=%.3e,\\sigma=%f)', ...
  mean(highpassGieLosv(seg)), ...
  std(highpassGieLosv(seg)), ...
  mean(highpassComputedLosv(seg)), ...
  std(highpassComputedLosv(seg)));
title(titlestr);
xlabel('gps seconds');
ylabel('meters/second');




%%%%%%%%

% seg = find(~isnan(errLosv));
seg = find(abs(losvAtGieT) > 0.05); 
muL = mean(errLosv(seg));
sigL = std(errLosv(seg));

newfigure;
plot(gieT(seg), errLosv(seg), '.');
titlestr = sprintf('Motion: Recorded minus Computed LOSV: \\mu=%f \\sigma=%f m/s', ...
  muL, sigL); 
xlabel('gps seconds');
ylabel('meters/second');
title(titlestr);
