close all
clear all


%Nav Error
epsilon_x = .02; %.02;
epsilon_y = .02; %.02;  % 1% of distance travelled

%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

%Lag Error
n_lag = 0;


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

%Vehicle Initial Position
x_0 = 0;
y_0 = 0;

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

dt = 1;
tf = 16.0;

t = 0:dt:tf;

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

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


for i = 1:length(t)
    if(i == 1)
        x_Actual(1) = x_0;
        y_Actual(1) = y_0;
        x_Nav(1) = x_0;
        y_Nav(1) = y_0;
    else
        x_Actual(i) = x_Actual(i-1)+V_x*dt;
        y_Actual(i) = y_Actual(i-1)+V_y*dt;
        x_Nav(i) = x_Nav(i-1)+(V_x+epsilon_x)*dt;
        y_Nav(i) = y_Nav(i-1)+(V_y+epsilon_y)*dt;
    end

    R_Measured(i) = sqrt((x_Actual(i)-x_Anchor_Actual)^2+ (y_Actual(i)-y_Anchor_Actual)^2);
    R_Measured(i) = range_scale*R_Measured(i) + 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


end


figure('Position',[  1186         253         959        1234])
mksize = 10^2;
set(gcf,'defaultaxesfontsize',20)
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 3]);
ylim([-1 17]);
legend('Actual Anchor Location', ...
       'Actual Vehicle Location', ...
       'Navigated Vehicle Location', ...
       'Navigated Anchor Location', ...
       'Navigated Anchor Location w/ Lag', ...
       'Location','SouthEast');
title("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
legend('Vehicle Center', ...
       'Actual Anchor Location', ...
       'Navigated Anchor Location', ...
      'Navigated Anchor Location w/ Lag', ...
       'Location','NorthWest');
title("Vehicle Fixed View");
linkaxes(get(gcf, 'children'), 'xy')
grid on





