% 
% First run:-
% 
% viewdat -d29 -m -h 1234567890201006151707.T02 > OrigPos.txt
% viewdat -d29 -m -h navKal.T02 > KalPos.txt
%
%

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

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

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

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

[C, A, B] = intersect(Orig(:,2), Lsq(:,2));

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


[C, A, B] = intersect(Orig(:,2), Kal(:,2));

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

% Find all the RTK positions.
i = find(Orig(:,9) == 4);

ENU_Kal = LLH2ENU(Kal(i,11) * pi / 180, ...
                  Kal(i,12) * pi / 180, ...
                  Kal(i,13), ...
                  Orig(i,11) * pi / 180, ...
                  Orig(i,12) * pi / 180, ...
                  Orig(i,13));

ENU_Lsq = LLH2ENU(Lsq(i,11) * pi / 180, ...
                  Lsq(i,12) * pi / 180, ...
                  Lsq(i,13), ...
                  Orig(i,11) * pi / 180, ...
                  Orig(i,12) * pi / 180, ...
                  Orig(i,13));



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

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


figure('Visible','off');
plot(Orig(i,2), ENU_Kal(:,1),'rx');
hold on;
plot(Orig(i,2), ENU_Lsq(:,1),'bx');
xlabel('GPS Time [Sec]');
ylabel('East RTK Fixed to Kalman Filter Error [m]');
legend('Kalman','LSQ');
grid on;
STD  = std(ENU_Kal(:,1));
RMS  = rms(ENU_Kal(:,1));
MEAN = mean(ENU_Kal(:,1));
disp(['East: Mean=',num2str(MEAN,'%.3f'),'m ', ...
      'sigma=',num2str(STD,'%.3f'),'m ', ...
      'RMS=',num2str(RMS,'%.3f'),'m']);
title (['Kalman East: Mean=',num2str(MEAN,'%.3f'),'m ', ...
        'sigma=',num2str(STD,'%.3f'),'m ', ...
        'RMS=',num2str(RMS,'%.3f'),'m']);

print('-dpng','EastError.png','-S960,720');

figure('Visible','off');
cdfplot(abs(ENU_Kal(:,1)),'r');
hold on;
cdfplot(abs(ENU_Lsq(:,1)),'b');
xlabel('abs(East Error) [m]');
ylabel('Cumulative Distribution');
legend('Kalman','LSQ');

% Sort the data and determine various percentiles
sort_out = sort(abs(ENU_Kal(:,1)));
Len = length(sort_out);
Fifty      = sort_out( floor(Len * 0.5));
SixtyEight = sort_out( floor(Len * 0.68));
NinetyFive = sort_out( floor(Len * 0.95));
disp([' CDF 50%=',num2str(Fifty,'%.3f'), ...
       'm 68%=',num2str(SixtyEight,'%.3f'), ...
       'm 95%=',num2str(NinetyFive,'%.3f'),'m']);
title('RTK versus Kalman filter');       
print('-dpng','East_CDF.png','-S960,720');


figure('Visible','off');
plot(Orig(i,2), ENU_Kal(:,2),'rx');
hold on;
plot(Orig(i,2), ENU_Lsq(:,2),'bx');
grid on;
xlabel('GPS Time [Sec]');
ylabel('North RTK Fixed to Kalman Filter Error [m]');
legend('Kalman','LSQ');
STD  = std(ENU_Kal(:,2));
RMS  = rms(ENU_Kal(:,2));
MEAN = mean(ENU_Kal(:,2));
disp(['North: Mean=',num2str(MEAN,'%.3f'),'m ', ...
      'sigma=',num2str(STD,'%.3f'),'m ', ...
      'RMS=',num2str(RMS,'%.3f'),'m']);
title(['Kalman North: Mean=',num2str(MEAN,'%.3f'),'m ', ...
      'sigma=',num2str(STD,'%.3f'),'m ', ...
      'RMS=',num2str(RMS,'%.3f'),'m']);

print('-dpng','NorthError.png','-S960,720');

figure('Visible','off');
cdfplot(abs(ENU_Kal(:,2)),'r');
hold on;
cdfplot(abs(ENU_Lsq(:,2)),'b');
xlabel('abs(North Error) [m]');
ylabel('Cumulative Distribution');
legend('Kalman','LSQ');

% Sort the data and determine various percentiles
sort_out = sort(abs(ENU_Kal(:,2)));
Len = length(sort_out);
Fifty      = sort_out( floor(Len * 0.5));
SixtyEight = sort_out( floor(Len * 0.68));
NinetyFive = sort_out( floor(Len * 0.95));
disp([' CDF 50%=',num2str(Fifty,'%.3f'), ...
       'm 68%=',num2str(SixtyEight,'%.3f'), ...
       'm 95%=',num2str(NinetyFive,'%.3f'),'m']);
title('RTK versus Kalman filter');       
print('-dpng','North_CDF.png','-S960,720');

figure('Visible','off');
plot(Orig(i,2), ENU_Kal(:,3),'rx');
hold on;
plot(Orig(i,2), ENU_Lsq(:,3),'bx');
xlabel('GPS Time [Sec]');
ylabel('Height RTK Fixed to Kalman Filter Error [m]');
legend('Kalman','LSQ');
grid on;
print('-dpng','HeightError.png','-S960,720');
STD  = std(ENU_Kal(:,3));
RMS  = rms(ENU_Kal(:,3));
MEAN = mean(ENU_Kal(:,3));
disp(['Height: Mean=',num2str(MEAN,'%.3f'),'m ', ...
      'sigma=',num2str(STD,'%.3f'),'m ', ...
      'RMS=',num2str(RMS,'%.3f'),'m']);


figure('Visible','off');
cdfplot(abs(ENU_Kal(:,3)),'r');
hold on;
cdfplot(abs(ENU_Lsq(:,3)),'b');
xlabel('abs(Height Error) [m]');
ylabel('Cumulative Distribution');
legend('Kalman','LSQ');

% Sort the data and determine various percentiles
sort_out = sort(abs(ENU_Kal(:,3)));
Len = length(sort_out);
Fifty      = sort_out( floor(Len * 0.5));
SixtyEight = sort_out( floor(Len * 0.68));
NinetyFive = sort_out( floor(Len * 0.95));
disp([' CDF 50%=',num2str(Fifty,'%.3f'), ...
       'm 68%=',num2str(SixtyEight,'%.3f'), ...
       'm 95%=',num2str(NinetyFive,'%.3f'),'m']);
title('RTK versus Kalman filter');       
print('-dpng','Height_CDF.png','-S960,720');


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

