errors.m

％ ％ set some variables in the workspace to control behaviour: ％ ％ graphicsMode 0 no graphics, ％

% err = allErrors(GT,PATHx,PATHy)
% 
% GT is of the form [timestamp, x, y, theta]
% PATHx and PATHy are just vectors of the x and y coordinates along the
% path we are computing errors on.  For example, if you have a path KF with
% format identical to GT as given above, you could run:
% >> err = allErrors(GT, KF(:,2), KF(:,3)]

begin = 1;  % ignore points before this (to allow the path to converge, for example)
begin = 500;
zzend=size(zz,1);

if begin<zzend
    
    PATHx = zz(begin:end,2);  % x estimate
    PATHy = zz(begin:end,3);  % y estimate
    
    PATHx2 = zz(begin:end,7);
    PATHy2 = zz(begin:end,8);
    HEADINGS=zz(begin:end,13);
    
    
    P = zeros(size(zz,1),2);
    
    [per, par] = distanceParAndPerp(PATHx, PATHy, PATHx2, PATHy2, HEADINGS);
    P=[per par];
    
    %% Cross Track Errors:
    err.XTE_abs_avg = mean(abs(P(:,1)));
    err.XTE_abs_max = max(abs(P(:,1)));
    err.XTE_abs_std = std(abs(P(:,1)));
    
    %% Along Track Errors
    err.ATE_abs_avg = mean(abs(P(:,2)));
    err.ATE_abs_max = max(abs(P(:,2)));
    err.ATE_abs_std = std(abs(P(:,2)));
    
    GTx = PATHx2;
    GTy = PATHy2;
    
    x_err = GTx - PATHx;
    y_err = GTy - PATHy;
    cartesian_err = sqrt(x_err.^2+y_err.^2);
    
    %% Total Euclidean distance errors:
    err.cartesian_abs_avg = mean(cartesian_err);
    err.cartesian_abs_max = max(cartesian_err);
    err.cartesian_abs_std = std(cartesian_err);
    
    clear PATHx PATHy PATHx2 PATHy2 HEADINGS
    format short
    err
else
    disp('Specified starting point is greater than the length of the path')
end