close all
clear all

%First set various errors to zero:

%Nav Error
U_scale = 1;
psi_offset = 0;

%DAT Range Error 
range_scale = 1.;  %Scale
range_offset = 0.;  %.2 Offset

%DAT Theta Error
theta_scale = 1.0;  %Scale
theta_offset = 0.*pi/180;  %Offset

posnError = 'DAT';
posnError = 'Both Nav and DAT'
posnError = 'Nav';

if( strncmpi(posnError,'nav',3) || strncmpi(posnError,'both',4))
  U_scale = .9;
  psi_offset = .05;
end
if( strncmpi(posnError, 'DAT',3) || strncmpi(posnError,'both',4))
  theta_offset = -2.*pi/180;  %Offset
  range_scale = .95;  %Scale
end


%Lag Error
n_lag = 340;
n_lag = 0;

%Actual Anchor Position
x_Anchor_Actual = 1;
y_Anchor_Actual = 16;

%Vehicle Initial Position.  Psi goes clockwise from the y axis. So, 
%Psi=0 is straight up the y axis.
x_0 = 0;
y_0 = 0;
psi_0 = .2;

%Actual Vehicle Velocity
%V_x = 0.0;
%V_y = 1.0;
U = 1.0;

dt = .01;
tf = 16.0;

t = 0:dt:tf;

x_Actual = zeros(1,length(t));
y_Actual = zeros(1,length(t));
psi_Actual = zeros(1,length(t));


x_Nav = zeros(1,length(t));
y_Nav = zeros(1,length(t));
psi_Nav = zeros(1,length(t));

for i = 1:length(t)
    if(i == 1)
        x_Actual(1) = x_0;
        y_Actual(1) = y_0;
        psi_Actual(1) = psi_0;
        %
        x_Nav(1) = x_0;
        y_Nav(1) = y_0;
        psi_Nav(1) = psi_0;
      else
        % Constant heading:
        %psi_Actual(i) = psi_Actual(i-1);
        %
        % Close the loop:
        if( i > n_lag + 1 )
          psi_Actual(i) = atan2(x_Anchor_Nav_lag(i-1)-x_Nav(i-1),y_Anchor_Nav_lag(i-1)-y_Nav(i-1)) - psi_offset;
        else
          psi_Actual(i) = psi_Actual(i-1);
        end
        %psi_Actual(i) = atan2(x_Anchor_Nav(i-1)-x_Nav(i-1),y_Anchor_Nav(i-1)-y_Nav(i-1)) - psi_offset;
        x_Actual(i) = x_Actual(i-1)+U*sin(psi_Actual(i))*dt;
        y_Actual(i) = y_Actual(i-1)+U*cos(psi_Actual(i))*dt;
        %
        psi_Nav(i)  = psi_Actual(i)+psi_offset;
        x_Nav(i) = x_Nav(i-1)+(U*U_scale)*sin(psi_Nav(i))*dt;
        y_Nav(i) = y_Nav(i-1)+(U*U_scale)*cos(psi_Nav(i))*dt;
    end

    R_Actual = sqrt((x_Actual(i)-x_Anchor_Actual)^2+ (y_Actual(i)-y_Anchor_Actual)^2);
    R_Measured(i) = range_scale*R_Actual + range_offset;

    theta_Measured(i) = atan2(x_Anchor_Actual-x_Actual(i),y_Anchor_Actual-y_Actual(i));
    theta_Measured(i) = theta_scale*theta_Measured(i)+theta_offset;

    x_Anchor_Nav(i) = x_Nav(i)+R_Measured(i)*sin(theta_Measured(i));
    y_Anchor_Nav(i) = y_Nav(i)+R_Measured(i)*cos(theta_Measured(i));


    if(i<=n_lag)
      x_Anchor_Nav_lag(i) = NaN;
      y_Anchor_Nav_lag(i) = NaN;
    else
      x_Anchor_Nav_lag(i) = x_Anchor_Nav(i-n_lag);
      y_Anchor_Nav_lag(i) = y_Anchor_Nav(i-n_lag);
    end

    if( y_Nav(i) > y_Anchor_Nav_lag(i) )
      break;
    end

end

if( i < length(t) )
  t = t(1:i);
  x_Nav = x_Nav(1:i);
  y_Nav = y_Nav(1:i);
  psi_Nav = psi_Nav(1:i);
  x_Actual = x_Actual(1:i);
  y_Actual = y_Actual(1:i);
  psi_Actual = psi_Actual(1:i);
end

fprintf('Miss Distance = %5.1f m.\n',R_Actual);

figure('Position',[  1186         253         959        1234])
mksize = 10^2;
set(gcf,'defaultaxesfontsize',20)
titleStr = [ posnError ' Error'];


ah1 = subplot(1,2,1);
plot(x_Anchor_Actual,y_Anchor_Actual,'kx');
hold all
plot(x_Actual,y_Actual,'ko');
scatter(x_Nav,y_Nav,mksize, jet(length(t)), 'filled','o');
scatter(x_Anchor_Nav,y_Anchor_Nav,mksize,jet(length(t)),'x');
%plot(x_Anchor_Nav,y_Anchor_Nav,'rx');
if( n_lag > 0 )
  plot(x_Anchor_Nav_lag,y_Anchor_Nav_lag,'gx');
end
grid on;
xlim([-1 4]);
ylim([-1 17]);
legend('Actual Anchor Location', ...
       'Actual Vehicle Location', ...
       'Navigated Vehicle Location', ...
       'Navigated Anchor Location', ...
       'Navigated Anchor Location w/ Lag', ...
       'Location','SouthEast');
title(titleStr,'Earth Fixed View');


ah2 = subplot(1,2,2);
plot(0,0,'ko');
hold all
plot(x_Anchor_Actual-x_Actual,y_Anchor_Actual-y_Actual,'kx');
plot(x_Anchor_Nav-x_Nav,y_Anchor_Nav-y_Nav,'rx');
if( n_lag > 0 )
  plot(x_Anchor_Nav_lag-x_Nav,y_Anchor_Nav_lag-y_Nav,'gx');
end
%set(gca,'ydir','reverse');
legend('Vehicle Center', ...
       'Actual Anchor Location', ...
       'Navigated Anchor Location', ...
       'Navigated Anchor Location w/ Lag', ...
       'Location','NorthWest');
title(titleStr,'Vehicle Fixed View');
linkaxes(get(gcf, 'children'), 'xy')
grid on

%title('Pure DAT Error')





