% Plot estimation results from TerrainAidLog.cc
%
%
% Created By: Debbie Meduna
% July 10, 2008
%
% Last Modified:
% November 15, 2010
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
 
% check if mmseVarZ field exists
if(~isfield(ta, 'mmseVarZ'))
    ta.mmseVarZ = ta.mmseVarE;
end

% check if attitude angle fields exists
estimateAttitude = 0;
if(isfield(ta, 'mmsePhi'))
    estimateAttitude = 1;
end

estHomerLocUTM = [4074974.02965359, 590656.488851003];

tercomEst = [ta.data(:,ta.mleN), ta.data(:,ta.mleE), ta.data(:,ta.mleZ)];
xTrue = [ta.data(:,ta.navN) ta.data(:,ta.navE) ta.data(:,ta.depth)];
mmseEst = [ta.data(:,ta.mmseN), ta.data(:,ta.mmseE), ta.data(:,ta.mmseZ)];
tercomCov = [ta.data(:,ta.mmseVarN), ta.data(:,ta.mmseVarE) ta.data(:,ta.mmseVarZ)];
if(estimateAttitude)
    tercomEst = [tercomEst ta.data(:,ta.mlePhi), ta.data(:,ta.mleTheta),ta.data(:,ta.mlePsi)];
    mmseEst = [mmseEst ta.data(:,ta.mmsePhi), ta.data(:,ta.mmseTheta),ta.data(:,ta.mmsePsi)];
    tercomCov = [tercomCov ta.data(:,ta.mmseVarPhi), ta.data(:,ta.mmseVarTheta),ta.data(:,ta.mmseVarPsi)];  
end

time = [ta.data(:,ta.navTime)];
dvlBeams = [ta.data(:,ta.beam)];
dvlBeamStatus = [ta.data(:,ta.dvlStatus)];
attitude = [ta.data(:, [ta.roll ta.pitch ta.yaw])];

% check if dvl velocities are recorded
if(isfield(ta, 'dvlVelocity'))
    dvlVelocities = [ta.data(:,ta.dvlVelocity)];
    dvlValid = ta.data(:,ta.dvlValid);
    gpsValid = ta.data(:,ta.gpsValid);
end

numPings = [1:1:length(time)]';
indices = numPings;
a = find(mmseEst(:,2) > 0,1,'first');
indices = indices(a:end);
x = ta.data(indices,ta.navTime);

% CONVERGENCE plots -XYZ
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute confidence intervals based on gaussian-like distribution
XconfInterval = sqrt(2.5002777*tercomCov(:,1));
YconfInterval = sqrt(2.5002777*tercomCov(:,2));
ZconfInterval = sqrt(2.5002777*tercomCov(:,3));

figure;
set(gcf, 'Name', 'TRN x,y,z estimates');
set(gcf, 'color', 'w');
subplot(3,1,1)
fill([x' x(end:-1:1)'], [(mmseEst(indices,1) - xTrue(indices,1) + XconfInterval(indices))',...
    (mmseEst(indices(end:-1:1),1) - xTrue(indices(end:-1:1),1) - XconfInterval(indices(end:-1:1)))'],...
    [0.9 0.9 0.9], 'Linestyle', '-', 'EdgeColor', [0.5 0.5 0.5], 'Linewidth', 1);
ylabel('North (m)');
hold on;
plot(x, tercomEst(indices,1)-xTrue(indices,1), '.-');
plot(x, mmseEst(indices,1) - xTrue(indices,1), 'r');
hold off;
title('Estimation bias: TRN estimate - Nav estimate');
legend( '90% confidence interval','MLE bias', 'MMSE bias');
grid on;

subplot(3,1,2)
fill([x' x(end:-1:1)'], [(mmseEst(indices,2) - xTrue(indices,2) + YconfInterval(indices))',...
    (mmseEst(indices(end:-1:1),2) - xTrue(indices(end:-1:1),2) - YconfInterval(indices(end:-1:1)))'],...
    [0.9 0.9 0.9], 'Linestyle', '-', 'EdgeColor', [0.5 0.5 0.5], 'Linewidth', 1);
hold on;
ylabel('East (m)');
plot(x, tercomEst(indices,2)-xTrue(indices,2), '.-');
plot(x, mmseEst(indices,2) - xTrue(indices,2), 'r');
hold off;
grid on;

subplot(3,1,3)
fill([x' x(end:-1:1)'], [(mmseEst(indices,3) - xTrue(indices,3) + ZconfInterval(indices))',...
    (mmseEst(indices(end:-1:1),3) - xTrue(indices(end:-1:1),3) - ZconfInterval(indices(end:-1:1)))'],...
    [0.9 0.9 0.9], 'Linestyle', '-', 'EdgeColor', [0.5 0.5 0.5], 'Linewidth', 1);
hold on;
ylabel('Depth (m)');
xlabel('Time (s)');
plot(x, tercomEst(indices,3) - xTrue(indices,3), '.-');
plot(x, mmseEst(indices,3) - xTrue(indices,3), 'r');
hold off;
grid on;

% CONVERGENCE plots - Phi/Theta/Psi
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(estimateAttitude)
    % Compute confidence intervals based on gaussian-like distribution
    PHconfInterval = sqrt(2.5002777*tercomCov(:,4));
    TconfInterval = sqrt(2.5002777*tercomCov(:,5));
    PconfInterval = sqrt(2.5002777*tercomCov(:,6));

    % make psi estimates between -pi and pi
    psiBiasMLE = unwrap(tercomEst(indices,6) - attitude(indices,3));
    psiBiasMMSE = unwrap(mmseEst(indices,6) - attitude(indices,3));

    figure;
    set(gcf, 'Name', 'TRN attitude estimates');
    set(gcf, 'color', 'w');
    subplot(3,1,1)
    fill([x' x(end:-1:1)'], [(mmseEst(indices,4) - attitude(indices,1) + PHconfInterval(indices))'*180/pi,...
        (mmseEst(indices(end:-1:1),4) - attitude(indices(end:-1:1),1) - PHconfInterval(indices(end:-1:1)))'*180/pi],...
        [0.9 0.9 0.9], 'Linestyle', '-', 'EdgeColor', [0.5 0.5 0.5], 'Linewidth', 1);
    ylabel('Phi (^o)');
    hold on;
    plot(x, (tercomEst(indices,4) - attitude(indices,1))*180/pi, '.-');
    plot(x, (mmseEst(indices,4) - attitude(indices,1))*180/pi, 'r');
    hold off;
    title('Estimation bias: TRN estimate - Nav estimate');
    legend( '90% confidence interval','MLE bias', 'MMSE bias');
    grid on;

    subplot(3,1,2)
    fill([x' x(end:-1:1)'], [(mmseEst(indices,5) - attitude(indices,2) + TconfInterval(indices))'*180/pi,...
        (mmseEst(indices(end:-1:1),5) - attitude(indices(end:-1:1),2) - TconfInterval(indices(end:-1:1)))'*180/pi],...
        [0.9 0.9 0.9], 'Linestyle', '-', 'EdgeColor', [0.5 0.5 0.5], 'Linewidth', 1);
    hold on;
    ylabel('Theta (^o)');
    plot(x, (tercomEst(indices,5) - attitude(indices,2))*180/pi, '.-');
    plot(x, (mmseEst(indices,5) - attitude(indices,2))*180/pi, 'r');
    hold off;
    grid on;
    
    subplot(3,1,3)
    fill([x' x(end:-1:1)'], [(psiBiasMMSE + PconfInterval(indices))'*180/pi,...
        (psiBiasMMSE(end:-1:1) - PconfInterval(indices(end:-1:1)))'*180/pi],...
        [0.9 0.9 0.9], 'Linestyle', '-', 'EdgeColor', [0.5 0.5 0.5], 'Linewidth', 1);
    hold on;
    ylabel('Psi (^o)');
    xlabel('Time (s)');
    plot(x, psiBiasMLE*180/pi, '.-');
    plot(x, psiBiasMMSE*180/pi, 'r');
    hold off;
    grid on;
end


% BEAM Measurement plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = ta.data(:,ta.navTime);
figure;
set(gcf, 'Name', 'TRN: DVL ranges');
set(gcf, 'color', 'w');
subplot(2,1,1)
plot(x,dvlBeams);
title('Raw Dvl ranges and status');
ylabel('DVL Ranges');
grid on;

subplot(2,1,2)
plot(x,dvlBeamStatus);
ylabel('DVL Beam Status');
xlabel('Time (s)');

% VELOCITY Measurement Plots
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(isfield(ta, 'dvlVelocity'))
    figure;
    set(gcf, 'Name', 'TRN: DVL velocities');
    subplot(2,1,1)
    plot(x,dvlVelocities);
    ylabel('DVL Velocities (m/s)');
    set(gcf, 'color', 'w');
    legend('vx', 'vy', 'vz');
    title('Raw Dvl velocities and status');
    grid on;

    subplot(2,1,2)
    plot(x,gpsValid);
    hold on;
    plot(x,dvlValid,'r');
    if(isfield(ta,'bottomLock'))
        plot(x,ta.data(:,ta.bottomLock),'g.-')
        legend('gpsValid', 'dvlValid', 'bottomLock');
    else
        legend('gpsValid', 'dvlValid');
    end
    ylabel('Status');
    xlabel('Time (s)');
end

%% Estimated vehicle trajectory over the terrain
a = find(mmseEst(:,2) > 0,1,'first');
start = xTrue(1,1:2);

figure;
set(gcf, 'Name', 'TRN: Estimated Vehicle Tracks');
set(gcf, 'color', 'w');
plot(xTrue(:,2)-start(2),xTrue(:,1)-start(1), '.-');
hold on;
%plot(tercomEst(a:end,2)-start(2),tercomEst(a:end,1)-start(1), 'g.-');
plot(mmseEst(a:end,2)-start(2),mmseEst(a:end,1)-start(1), 'r.-');
plot(estHomerLocUTM(2)-start(2), estHomerLocUTM(1)-start(1), 'k*', 'MarkerSize', 10, 'LineWidth', 1.5);
axis equal;
title('Estimated vehicle tracks in relation to homer');
xlabel('Eastings (m)');
ylabel('Northings (m)');
grid on;
hold off;
legend('IMU', 'TRN MMSE', 'Homer');








