% ECE 707 HW 2 Exmaple code
% Least Square Fitting of a 3D plane

point_number = 100; % number of points
xy_range = 10;      % the range of x and y both in [-xy_range ~ xy_range]
d_range = 10;       % the range of d
noise_sigma = 1;    % standard deviation of Guassian noise

%% Data preparation
% Parameters of the given plane, using '_0' as suffix to distinguish
N_0 = rand(3,1)-0.5;
N_0 = N_0/norm(N_0);
a_0 = N_0(1);
b_0 = N_0(2);
c_0 = N_0(3);
d_0 = (rand(1,1)-0.5)*2*d_range;

% Generate points on plane
x_0 = (rand(point_number, 1) - 0.5)*2*xy_range;
y_0 = (rand(point_number, 1) - 0.5)*2*xy_range;
z_0 = -(a_0*x_0 + b_0*y_0 - d_0 )/c_0;

% Add Noise onto the points position
x = x_0 + randn(point_number,1)*noise_sigma;
y = y_0 + randn(point_number,1)*noise_sigma;
z = z_0 + randn(point_number,1)*noise_sigma;

%% Least Square Fitting 
%create U matrix 
x_mean = mean(x);
y_mean = mean(y);
z_mean = mean(z);
U =  [ x-x_mean, y-y_mean, z-z_mean ];

%least squares solution
[V,D] = eig(U'*U);

%Sort eigenvalue in ascending order.
[sorted_value,ind] = sort(diag(D));

%The eigen vector with the minimum eigen value is the LS solution
a = V(1,ind(1));
b = V(2,ind(1));
c = V(3,ind(1));
d = a*x_mean + b*y_mean + c*z_mean;

%% Plotting
% plot the given plane.
subplot(1,2,1);
[X,Y] = meshgrid( -xy_range : 1 : xy_range );
Z = -(a_0*X + b_0*Y -d_0 )/c_0;
mesh( X,Y,Z);  hold on
plot3(x,y,z,'.');
title('Given plane with data points');

% plot the fitted plane.
subplot(1,2,2);
[X,Y] = meshgrid( -xy_range : 1 : xy_range );
Z = -(a*X + b*Y -d )/c;
mesh(X,Y,Z); hold on
plot3(x,y,z,'.');
title('Fitted plane with data points');