% Example of a Least Squares method
% for a noisy straight line
% Solve the normal equations using Cholesky factorization
% Example on p.108 of Heath

clear all; hold off; format long g;
t=[0; 0.2; 0.8; 1.0; 1.2; 1.9; 2.0; 2.1; 2.95; 3.0];
y=[0.01; 0.22; 0.76; 1.03; 1.18; 1.94; 2.01; 2.08; 2.9; 2.95];

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,