% 
% First run:-
% 
% viewdat -d29 -m -h navKal.T02 > KalPos.txt
% viewdat -d29 -m -h navLsq.T02 > LsqPos.txt
%
%

set(0,'defaulttextfontname','*');

Kal  = load('KalPos.txt');
Lsq  = load('LsqPos.txt');

% Time align
[C, A, B] = intersect(Kal(:,2), Lsq(:,2));

Kal = Kal(A,:);
Lsq = Lsq(B,:);

MeanLat = mean(Kal(:,11));
MeanLon = mean(Kal(:,12));
MeanHgt = mean(Kal(:,13));

ENU_Kal = LLH2ENU(Kal(:,11) * pi / 180, ...
                  Kal(:,12) * pi / 180, ...
                  Kal(:,13), ...
                  MeanLat * pi / 180, ...
                  MeanLon * pi / 180, ...
                  MeanHgt );

ENU_Lsq = LLH2ENU(Lsq(:,11) * pi / 180, ...
                  Lsq(:,12) * pi / 180, ...
                  Lsq(:,13), ...
                  MeanLat * pi / 180, ...
                  MeanLon * pi / 180, ...
                  MeanHgt );

figure('Visible','off');
plot(ENU_Lsq(:,1), ENU_Lsq(:,2),'bx-');
hold on;
plot(ENU_Kal(:,1), ENU_Kal(:,2),'rx-');
A = axis;
AMax = max(abs(A));
A(1) = -AMax;
A(2) =  AMax;
A(3) = -AMax;
A(4) =  AMax;
axis(A);
axis square;
xlabel('East [m]');
ylabel('North [m]');
legend('LSQ','Kalman');
grid on;
print('-dpng','EastNorth.png','-S960,720');

% East
figure('Visible','off');
plot(Lsq(:,2), ENU_Lsq(:,1),'bx-');
hold on;
plot(Kal(:,2), ENU_Kal(:,1),'rx-');

xlabel('Time [GPS Sec]');
ylabel('East offset from average [m]');
legend('LSQ','Kalman');
grid on;
print('-dpng','East.png','-S960,720');

% North
figure('Visible','off');
plot(Lsq(:,2), ENU_Lsq(:,2),'bx-');
hold on;
plot(Kal(:,2), ENU_Kal(:,2),'rx-');

xlabel('Time [GPS Sec]');
ylabel('North offset from average [m]');
legend('LSQ','Kalman');
grid on;
print('-dpng','North.png','-S960,720');

% Height
figure('Visible','off');
plot(Lsq(:,2), Lsq(:,13),'bx-');
hold on;
plot(Kal(:,2), Kal(:,13),'rx-');

xlabel('Time [GPS Sec]');
ylabel('Height [m]');
legend('LSQ','Kalman');
grid on;
print('-dpng','Height.png','-S960,720');

% delta lat
figure('Visible','off');
plot(Lsq(:,2), Lsq(:,14),'bx-');
hold on;
plot(Kal(:,2), Kal(:,14),'rx-');

xlabel('Time [GPS Sec]');
ylabel('Latitude Velocity [m/s]');
legend('LSQ','Kalman');
grid on;
print('-dpng','dlat.png','-S960,720');

% delta lon
figure('Visible','off');
plot(Lsq(:,2), Lsq(:,15),'bx-');
hold on;
plot(Kal(:,2), Kal(:,15),'rx-');

xlabel('Time [GPS Sec]');
ylabel('Longitude Velocity [m/s]');
legend('LSQ','Kalman');
grid on;
print('-dpng','dlon.png','-S960,720');

% delta hgt
figure('Visible','off');
plot(Lsq(:,2), Lsq(:,16),'bx-');
hold on;
plot(Kal(:,2), Kal(:,16),'rx-');

xlabel('Time [GPS Sec]');
ylabel('Height Velocity [m/s]');
legend('LSQ','Kalman');
grid on;
print('-dpng','dhgt.png','-S960,720');

% 3D velocity
figure('Visible','off');
Kal_V = sqrt(   Kal(:,14).*Kal(:,14) ...
              + Kal(:,15).*Kal(:,15) ...
              + Kal(:,16).*Kal(:,16) );

Lsq_V = sqrt(   Lsq(:,14).*Lsq(:,14) ...
              + Lsq(:,15).*Lsq(:,15) ...
              + Lsq(:,16).*Lsq(:,16) );

plot(Lsq(:,2), Lsq_V,'bx-');
hold on;
plot(Kal(:,2), Kal_V,'rx-');

xlabel('Time [GPS Sec]');
ylabel('3D Velocity [m/s]');
legend('LSQ','Kalman');
grid on;
print('-dpng','d3D.png','-S960,720');

if(0)
  %
  % Kal acceleration
  %

  % lat
  figure('Visible','off');
  plot(diff( Kal(:,14 ).*5 ),'rx-');
  grid on;
  print('-dpng','alat.png','-S960,720');

  % lon
  figure('Visible','off');
  plot(diff( Kal(:,15 ).*5 ),'rx-');
  grid on;
  print('-dpng','alon.png','-S960,720');

  %
  % LSQ acceleration
  %

  % lat
  figure('Visible','off');
  plot(diff( Lsq(:,14 ).*5 ),'rx-');
  grid on;
  print('-dpng','alat-lsq.png','-S960,720');

  % lon
  figure('Visible','off');
  plot(diff( Lsq(:,15 ).*5 ),'rx-');
  grid on;
  print('-dpng','alon-lsq.png','-S960,720');
end


