% Computer problem 3.5 on p.153 Heath
% A linear least squares problem.
% K. Ming Leung, 04/26/03, 04/20/05 (modified orbit plotting function)

clear all; hold off; bigscreen;
% the original data
Xvec = [1.02 0.95 0.87 0.77 0.67 0.56 0.44 0.30 0.16 0.01]';
Yvec = [0.39 0.32 0.27 0.22 0.18 0.15 0.13 0.12 0.13 0.15]';
% set low and upper limits along x and y in contour plot
lowLimx = -1; highLimx = 1.5;
lowLimy = -0.5; highLimy = 2;

% dimensions of the linear least squares problem
m = length(Xvec);   % number of data points (equations)
n = 5;              % number of unknown parameters
% matrix A for the linear least squares problem
AA = [Yvec.*Yvec  Xvec.*Yvec  Xvec  Yvec  ones(m,1)], 
%% Actually there is another way to set up the matrix.
%% But this version is probably what the author has in mind.
% Find the condition number of A
cond(AA),  
% vector b
Bvec = Xvec.*Xvec;
% Obtain the set of normal equations
AT = AA';
ATA = AT*AA;
% Obtain parameters by solving the normal equations
% using Matlab's "\" operator which uses factorization
Pnormal = ATA\(AT*Bvec),
% Parameters by Matlab's least squares 
% (using the "\" operator, which uses QR factorization for linear least squares)
Pmatlab = AA\Bvec,
% (Just for fun) Parameters by QR factorization
[QQ,RR] = qr(AA); RS = RR(1:n,1:n); 
Pqr = backsubstitution(RS,QQ'*Bvec,n),
% plot the orbit
conicSectionPlot_(Pmatlab,lowLimx,highLimx,lowLimy,highLimy,'b-');
hold on;
plot(Xvec,Yvec,'ok'); % plot orginal data

% RESULTS:
% The key point is the problem is rather ill-conditioned.
% The condition number for A is rather large:
%   688.429446869325
% The set of parameters obtained either from the set of normal equations,
% or directly from Matlab's "\" operator (or from QR factorization) are
% the almost same:
% Pnormal =
%          -2.63562548371284
%          0.143646182599806
%          0.551446963140239
%           3.22294033810535
%         -0.432894270264362
% Pmatlab =
%          -2.63562548371206
%          0.143646182598882
%          0.551446963140361
%           3.22294033810589
%          -0.43289427026445
% Pqr =
%          -2.63562548371211
%          0.143646182598895
%          0.551446963140359
%            3.2229403381059
%          -0.43289427026445
% The trajectory is an ellipse which fits the data points very nicely.

% Introduce random noise (perturbaton)
% set the lower and upper limits of the random noice
low = -0.005; high = -low;
% set the state for the random number generator
% so that we can all compare our results!
rand('state',17);
% Perturb the data by just a little.
Xpvec = Xvec + low+rand(m,1)*(high-low); 
Ypvec = Yvec + low+rand(m,1)*(high-low);
plot(Xpvec,Ypvec,'xr');    % plot the randomized data
% Set up the perturbed matrix A
AAP = [Ypvec.*Ypvec  Xpvec.*Ypvec  Xpvec  Ypvec  ones(m,1)]; 
% Find the condition number for this new A
cond(AAP),
% Set up the new b vec.
Bpvec = Xpvec.*Xpvec;
% Obtained the new set of parameters
Pp = AAP\Bpvec,
% plot the new trajectory for the perturbed data
conicSectionPlot_(Pp,lowLimx,highLimx,lowLimy,highLimy,'r-');
% 
% RESULTS
% The condition number is roughly about the same:
% ans =
%           601.275968475904
% The parameters obtained from the perturbed data differ
% quite a bit from those of the original data:
% Pp =
%          -3.90688742680392
%          0.912590648496475
%          0.448593489581328
%           3.08189423215092
%          -0.38967067714899
% The resulting trajectory is another ellipse which also
% fit the perturbed data very well.
% Although the differences in the perturbed data and the original data
% are very small, because the problem is ill-coonditioned, the parameters
% obtained are very sensitive to perturbations.
% If we do not set the state of the random number generator to a given value,
% the data will be perturbed slightly but differently every run.
% A rather different set of parameters is obtained each time.
% The trajectories are different ellipses that fit the data points well.
% 
% The problem is expected to be much less ill-conditioned if there were data
% points on the other side of the orbit.

% SVD for the original problem
% Set up a different plot style for the rank 1, 2, 3, and 4, approximations.
PlotStyle = ['b--'; 'r--'; 'g--'; 'c--'; 'm--'];
nn = n-1;   % nn = 4 here
% Compute the singular value decomposition for the original data
[UU, SS, VV] = svd(AA),
% Find the paramters for the rank 1, 2, 3, and 4 approximations,
% and plot the resulting trajectories:
Psvd = zeros(n,1);
% see p.138 Heath
for i=1:nn, Psvd = Psvd+(UU(:,i)'*Bvec/SS(i,i))*VV(:,i);
    %display the parameters
    Psvd, 
    % plot each of the resulting trajectories
    conicSectionPlot_(Psvd,lowLimx,highLimx,lowLimy,highLimy,PlotStyle(i,:)); 
end 

% RESULTS:
% The sets of parameters for the rank 1, 2, 3, and 4 apprximations:
% Psvd =
%         0.0189564037275549
%         0.0547271822700891
%          0.211730935389071
%         0.0736705462640955
%           0.33612063526295
% Psvd =
%          0.104419347020275
%          0.340136609715485
%          0.891788709416833
%          0.202487930977745
%         -0.171789387560633
% Psvd =
%          0.419164878435784
%          0.876871273877857
%          0.525345010533048
%          0.756138531245885
%         -0.167447774292797
% Psvd =
%         -0.652787158432152
%         -0.945114428403589
%          0.716821776633348
%           3.26103478591289
%         -0.479972861947773
% Notice that there are 1 or 2 very small singular values.
% Some of the low-ranking approximations do not give parameters
% that describe elliptical trajectories, 
% and their trajectories do not pass near the data points.
% They are not good approximations for the problem.
% Of course the rank 5 (full rank) gives exactly the same set of parameters as before.

%
% SVD for the perturbed problem
% Set up the plotting style for the perturbed problem
PlotStyleP = ['b:'; 'r:'; 'g:'; 'c:'; 'k:'];
% Singular decomposition for the perturbed data
[UP, SP, VP] = svd(AAP);
% Obtained the set of parameters for the rank 1, 2, 3, and 4 approximations
% for the perturbed problem
Ppsvd = zeros(n,1);
for i = 1:nn, Ppsvd = Ppsvd+(UP(:,i)'*Bpvec/SP(i,i))*VP(:,i); 
    % display the parameters
    i, Ppsvd, 
    % plot the trajectories
    conicSectionPlot_(Ppsvd,lowLimx,highLimx,lowLimy,highLimy,PlotStyleP(i,:)); 
end

% RESULTS:
% 
% i =
%      1
% Ppsvd =
%         0.0189577244895179
%         0.0548048689874096
%           0.21235576587836
%         0.0737516734981482
%           0.33673643101444
% i =
%      2
% Ppsvd =
%          0.103360115528493
%          0.338799970475543
%          0.892385935705324
%          0.200882304396559
%         -0.170927130680776
% i =
%      3
% Ppsvd =
%          0.412946849176994
%          0.863151838325771
%          0.534251868597918
%          0.750492419893392
%         -0.168221534221773
% i =
%      4
% Ppsvd =
%         -0.486077171572202
%         -0.872643455864119
%          0.726609005027054
%           3.03969064817557
%         -0.457785878827365
% Results are similar in nature compared with the unperturbed case.
% Of course the results for the parameters are rather different,
% even when compared with those obtained at the same rank approximations.

% total least squares treatment
% SVD for total least squares
[UT, ST, VT] = svd([AA Bvec]);
% Parameters obtained from total least squares 
Pt = -VT(1:n,n+1)/VT(n+1,n+1),
% plot the resulting trajectory
conicSectionPlot_(Pt,lowLimx,highLimx,lowLimy,highLimy,'y-'); 

% RESULT:
% The parameters obtained from total least squares:
% Pt =
%          -3.26928571249094
%          0.458589125973966
%          0.502397023142482
%           3.24555073664051
%          -0.42249403957785

% Repeat the same procedure using the perturbed data
% total least squares treatment
% SVD for total least squares
[UT, ST, VT] = svd([AAP Bpvec]);
% Parameters obtained from total least squares 
Pt = -VT(1:n,n+1)/VT(n+1,n+1),
% plot the resulting trajectory
conicSectionPlot_(Pt,lowLimx,highLimx,lowLimy,highLimy,'y--'); 

% RESULT
% Parameters are also sensitive to perturbations
% Pt =
%          -6.20863940979673
%           2.05236401642814
%          0.268827974691528
%           3.17424463445775
%         -0.352447851063949
%         
 