% Example of a Least Squares method
% 1D motion under gravity
% Solve the normal equations using Cholesky factorization
% Example on p.108 of Heath

clear all; hold off; format long g;
t=(0:0.5:10)';
y=[2.9 2.7 4.8 5.3 7.1 7.6 7.7 7.6 9.4 9.0 9.6 10.0 10.2 9.7 8.3 8.4 9.0 8.3 6.6 6.7 4.1]';
plot(t,y,'.','MarkerSize',14); hold on; 
xlabel('Time, t','fontSize',18);
ylabel('Distance, y','fontSize',18);
pause;

m=length(t);
n=3;
A=[ones(m,1) t t.^2];   % Set up matrix A
b=y;
AT = A.';
ATA = AT*A;             %
ATb = AT*b;
% Solve the system of normal equations
L=Cholesky(ATA); 
y=forwardSubstitution(L,ATb,n);
x=backSubstitution(L.',y,n);
%x = ATA\ATb,
r = A*x-b; r2 = r.'*r, normr=sqrt(r2),

% Plot the resulting cubic
tPlot=0:0.01:10;
yPlot=(x(3)*tPlot+x(2)).*tPlot+x(1);
plot(tPlot,yPlot,'r-','lineWidth',2);
normAx=norm(A*x), 
normb=norm(b), 
cosTheta=normAx/normb,