function RoundTrip
% Program to add noise to a sinusoidal test function and take two
% derivatives, and then use numerical integration method to take
% two integrals to see how well we recover the original function.

% Define sinusoidal test function
npts = 21;
t = linspace(0,1,npts)';
a = 1;
b = 2;
tf = 1;
w = 2*pi/tf;
q = a*sin(w*t)+b;

% Add noise to original data
noisemag = 0.05;
qnoisy = q+noisemag*(2*rand(npts,1)-1);

plot(t,q,'k-','LineWidth',2)
hold on
plot(t,qnoisy,'ko','LineWidth',2)
set(gca,'FontSize',14)
legend('Original','Noisy')
pause

% Calculate first derivative of original data analytically and numerically
% and of noisy data numerically. Use forward difference for first
% derivative calculations.
qp = w*a*cos(w*t);
h = t(2,1)-t(1,1);

qporig = zeros(npts,1);
qpnoisy = zeros(npts,1);

for i = 1:npts
    if i == npts
        % Use second data point as first sample point by assuming
        % periodicity
        qporig(i,1) = (q(2,1)-q(i,1))/h;
        qpnoisy(i,1) = (qnoisy(2,1)-qnoisy(i,1))/h;
    else
        qporig(i,1) = (q(i+1,1)-q(i,1))/h;
        qpnoisy(i,1) = (qnoisy(i+1,1)-qnoisy(i,1))/h;
    end
end

hold off
plot(t,qp,'k-','LineWidth',2)
hold on
plot(t,qporig,'ko','LineWidth',2)
plot(t,qpnoisy,'kx','LineWidth',2)
set(gca,'FontSize',14)
legend('Analytical','Numerical orig','Numerical noisy')
title('Forward difference - first derivative')
% Note that we have two sources of error:
% 1) Discretization with finite step size h (and note the systematic nature
% of the error), and
% 2) Noise in the data.
pause

% Now repeat the first derivative calculations using central differencing
for i = 1:npts
    if i == 1
        % Use next to last data point as second sample point by assuming
        % periodicity
        qporig(i,1) = (q(i+1,1)-q(npts-1,1))/(2*h);
        qpnoisy(i,1) = (qnoisy(i+1,1)-qnoisy(npts-1,1))/(2*h);
    elseif i == npts
        % Use second data point as second sample point by assuming
        % periodicity
        qporig(i,1) = (q(2,1)-q(i-1,1))/(2*h);
        qpnoisy(i,1) = (qnoisy(2,1)-qnoisy(i-1,1))/(2*h);
    else
        qporig(i,1) = (q(i+1,1)-q(i-1,1))/(2*h);
        qpnoisy(i,1) = (qnoisy(i+1,1)-qnoisy(i-1,1))/(2*h);
    end
end

hold off
plot(t,qp,'k-','LineWidth',2)
hold on
plot(t,qporig,'ko','LineWidth',2)
plot(t,qpnoisy,'kx','LineWidth',2)
set(gca,'FontSize',14)
legend('Analytical','Numerical orig','Numerical noisy')
title('Central difference - first derivative')
% Note that we have two sources of error:
% 1) Discretization with finite step size h (but note that the systematic
% backward shift in the estimate is now gone), and
% 2) Noise in the data.
pause

% Now calculate the second derivative with forward differencing
qpp = -w^2*a*sin(w*t);

qpporig = zeros(npts,1);
qppnoisy = zeros(npts,1);

for i = 1:npts
    if i == npts
        % Use second and third data points as first and second sample
        % points by assuming periodicity
        qpporig(i,1) = (q(3,1)-2*q(2,1)+q(i,1))/(h^2);
        qppnoisy(i,1) = (qnoisy(3,1)-2*qnoisy(2,1)+qnoisy(i,1))/(h^2);
    elseif i == npts-1
        qpporig(i,1) = (q(2,1)-2*q(i+1,1)+q(i,1))/(h^2);
        qppnoisy(i,1) = (qnoisy(2,1)-2*qnoisy(i+1,1)+qnoisy(i,1))/(h^2);
    else
        qpporig(i,1) = (q(i+2)-2*q(i+1,1)+q(i,1))/(h^2);
        qppnoisy(i,1) = (qnoisy(i+2)-2*qnoisy(i+1,1)+qnoisy(i,1))/(h^2);
    end
end

hold off
plot(t,qpp,'k-','LineWidth',2)
hold on
plot(t,qpporig,'ko','LineWidth',2)
plot(t,qppnoisy,'kx','LineWidth',2)
set(gca,'FontSize',14)
legend('Analytical','Numerical orig','Numerical noisy')
title('Forward difference - second derivative')
% Note that we have two sources of error:
% 1) Discretization with finite step size h (and note the systematic nature
% of the error), and
% 2) Noise in the data.
pause

% Now repeat the second derivative calculation with central differencing
for i = 1:npts
    if i == 1
        % Use next to last data point as second sample point by assuming
        % periodicity
        qpporig(i,1) = (q(i+1,1)-2*q(i,1)+q(npts-1,1))/(h^2);
        qppnoisy(i,1) = (qnoisy(i+1,1)-2*qnoisy(i,1)+qnoisy(npts-1,1))/(h^2);
    elseif i == npts
        % Use second data point as second sample point by assuming
        % periodicity
        qpporig(i,1) = (q(2,1)-2*q(i,1)+q(i-1,1))/(h^2);
        qppnoisy(i,1) = (qnoisy(2,1)-2*qnoisy(i,1)+qnoisy(i-1,1))/(h^2);
    else
        qpporig(i,1) = (q(i+1,1)-2*q(i,1)+q(i-1,1))/(h^2);
        qppnoisy(i,1) = (qnoisy(i+1,1)-2*qnoisy(i,1)+qnoisy(i-1,1))/(h^2);
    end
end

hold off
plot(t,qpp,'k-','LineWidth',2)
hold on
plot(t,qpporig,'ko','LineWidth',2)
plot(t,qppnoisy,'kx','LineWidth',2)
set(gca,'FontSize',14)
legend('Analytical','Numerical orig','Numerical noisy')
title('Central difference - second derivative')
% Note that we have two sources of error:
% 1) Discretization with finite step size h (but note that the systematic
% backward shift in the estimate is now gone), and
% 2) Noise in the data.
pause

% Now try integrating the central difference acceleration data twice to see
% how well we can recover the position data. Just use the midpoint rule
% for simplicity.
qporiginteg = zeros(npts,1);
qpnoisyinteg = zeros(npts,1);

for i = 1:npts
    if i == 1
        qporiginteg(i,1) = (qpporig(i,1))*(h/2)+qp(i,1);
        qpnoisyinteg(i,1) = (qppnoisy(i,1))*(h/2)+qp(i,1);
    elseif i == npts
        qporiginteg(i,1) = (qpporig(i,1))*(h/2)+qporiginteg(i-1,1);
        qpnoisyinteg(i,1) = (qppnoisy(i,1))*(h/2)+qpnoisyinteg(i-1,1);
    else
        qporiginteg(i,1) = (qpporig(i,1))*h+qporiginteg(i-1,1);
        qpnoisyinteg(i,1) = (qppnoisy(i,1))*h+qpnoisyinteg(i-1,1);
    end
end

hold off
plot(t,qp,'k-','LineWidth',2)
hold on
plot(t,qporiginteg,'ko','LineWidth',2)
plot(t,qpnoisyinteg,'kx','LineWidth',2)
set(gca,'FontSize',14)
legend('Analytical','Numerical orig','Numerical noisy')
title('Midpoint method - first derivative')
pause

qoriginteg = zeros(npts,1);
qnoisyinteg = zeros(npts,1);

for i = 1:npts
    if i == 1
        qoriginteg(i,1) = (qporig(i,1))*(h/2)+q(i,1);
        qnoisyinteg(i,1) = (qpnoisy(i,1))*(h/2)+q(i,1);
    elseif i == npts
        qoriginteg(i,1) = (qporig(i,1))*(h/2)+qoriginteg(i-1,1);
        qnoisyinteg(i,1) = (qpnoisy(i,1))*(h/2)+qnoisyinteg(i-1,1);
    else
        qoriginteg(i,1) = (qporig(i,1))*h+qoriginteg(i-1,1);
        qnoisyinteg(i,1) = (qpnoisy(i,1))*h+qnoisyinteg(i-1,1);
    end
end

hold off
plot(t,q,'k-','LineWidth',2)
hold on
plot(t,qoriginteg,'ko','LineWidth',2)
plot(t,qnoisyinteg,'kx','LineWidth',2)
set(gca,'FontSize',14)
legend('Analytical','Numerical orig','Numerical noisy')
title('Midpoint method - position')
pause

% Finally, try the two Matlab functions diff and gradient to show what they
% produce for numerical differentiation results
qporig = gradient(q,h);
qpnoisy = gradient(qnoisy,h);
hold off
plot(t,qp,'k-','LineWidth',2)
hold on
plot(t,qporig,'ko','LineWidth',2)
plot(t,qpnoisy,'kx','LineWidth',2)
set(gca,'FontSize',14)
legend('Analytical','Numerical orig','Numerical noisy')
title('Gradient function - first derivative')
pause

qpporig = gradient(qp,h);
qppnoisy = gradient(qpnoisy,h);
hold off
plot(t,qpp,'k-','LineWidth',2)
hold on
plot(t,qpporig,'ko','LineWidth',2)
plot(t,qppnoisy,'kx','LineWidth',2)
set(gca,'FontSize',14)
legend('Analytical','Numerical orig','Numerical noisy')
title('Gradient function - second derivative')

% Note that the diff command does not give us what we want. It only
% provides the differences between adjacent elements and can not
% accommodate a given step size h.