  function P4TestScript()
% Checks out Position.m
% We'll generate data from a spiral...
% If 
%                x(t)   =  (2-t)*cos(rho*t)
%                y(t)   =  (2-t)*sin(rho*t)
% then
%                x'(t)  =  -cos(rho*t) - (2-t)*rho*sin(rho*t)
%                y'(t)  =  -sin(rho*t) + (2-t)*rho*cos(rho*t)
% and
%                x''(t) =  2*rho*sin(rho*t) - (2-t)*rho^2*cos(rho*t)
%                y''(t) = -2*rho*cos(rho*t) - (2-t)*rho^2*sin(rho*t)

% Generate the spiral and display...
rho = 20;
N = 200;
tVals = linspace(0,1,N)';
x = (2-tVals).*cos(rho*tVals);
y = (2-tVals).*sin(rho*tVals);
plot(x,y)
axis  equal off
shg
% Generate the accelerations and add a little noise at m equally-spaced
% times..
m = 30;
t = linspace(0,1,m)';
noise = 2.0;
ax =  2*rho*sin(rho*t) - rho^2*(2-t).*cos(rho*t) + noise*randn(m,1);  x0 = 2; v0 = -1;
ay = -2*rho*cos(rho*t) - rho^2*(2-t).*sin(rho*t) + noise*randn(m,1);  y0 = 0; u0 = 2*rho;
% Compute the x-distance spline and the y-distance spline...
Sx = Position(ax,t,x0,v0);
Sy = Position(ay,t,y0,u0);

% Take a look at the predicted locations...
hold on
xSpline = ppval(Sx,tVals);
ySpline = ppval(Sy,tVals);
for k=1:N
    h = plot(xSpline(k),ySpline(k),'or');
    shg
    pause(.01)
    delete(h)
end
hold off

clc
% Compare with CVL

% Compute the x-distance spline and the y-distance spline...
SxCVL = PositionCVL(ax,t,x0,v0);
xSplineCVL = ppval(SxCVL,tVals);
err = norm(xSplineCVL -xSpline);
title(sprintf('|xSplineYou - xSplineCVL| = %10.3e\n',err),'fontsize',14)



