close all;
clear all;
clc;

%% Parameters of the system
%% This is the parameters we need to estimate
% Center
C = [30 , 20, 15];
% Radius
R = 12;


% Number of points
N = 1000;
% Noise
k = 0.002;

%% Create noisy sphere
alpha = 2*pi*rand(1,N);
beta  = 2*pi*rand(1,N);
noise = k*2*randn(1,N)-1;
Points = C + [ R*noise.*cos(alpha) ; R*noise.*sin(alpha).*cos(beta) ; R*noise.*sin(alpha).*sin(beta)  ]';


%% Draw the noisy sphere
plot3 (Points(:,1) , Points(:,2), Points(:,3), 'b.');
axis square equal;
grid on;
xlabel ('X');
ylabel ('Y');
zlabel ('Z');


%% Prepare matrices
A = [Points(:,1) Points(:,2) Points(:,3) ones(N,1)];
B = [Points(:,1).*Points(:,1) + Points(:,2).*Points(:,2) + Points(:,3).*Points(:,3)];

%% Least square approximation
X=pinv(A)*B;


%% Calculate sphere parameters
xc = X(1)/2
yc = X(2)/2
zc = X(3)/2
r = sqrt(4*X(4) + X(1)*X(1) + X(2)*X(2) + X(3)*X(3) )/2

%% Draw the fitted sphere
[X, Y, Z] = sphere;
X = xc + X*r;
Y = yc + Y*r;
Z = zc + Z*r;
hold on;
surf (X, Y, Z, 'FaceAlpha', 0.2, 'EdgeAlpha', 0.1);
legend ('Points', 'Least-square sphere');