%% The linear algebra behind the Global Postitioning System

%% Symbolic Variables
	syms x y z zt s

%% The Input Data from Four Satalites
	disp('Data received from four satelites');
	SatXYZ = [
				 .94,  1.2, 2.3; 
				   0, 1.35, 2.41;
				 2.3,  .94, 1.2;
				2.41, 	 0, 1.35;
	]	

	% The data in the example
	SendTime = [1.23; 1.08; .74; .23]

%	SatXYZ = [
%	1.12,	2.1,	1.4;
%	0,	1.53,	2.3;
%	1.4, 	1.12,	2.1;
%	2.3,	0,	1.53;
%	];
%
%	SendTime = [1.06; .56; 1.16; .75];

%% The non-linear expressions
	for i = 1:4
		NonLinearExpression{i} = (x-SatXYZ(i,1))^2 + (y-SatXYZ(i,2))^2 + ...
			(z-SatXYZ(i,3))^2 - .22*(zt-SendTime(i))^2;
		disp(NonLinearExpression{i})
	end

%% Obtain a linear system from the non-linear one
	for i = 1:3;
			LinearExpression{i}   = simplify(NonLinearExpression{1}-NonLinearExpression{i+1});
			AugmentedMatrix(i,:)  = flipud(coeffs(LinearExpression{i})');
			disp(LinearExpression{i})
	end

	
%% Create the augmented matrix from the expression			
	AugmentedMatrix(:,end) = -AugmentedMatrix(:,end);
	disp(double(AugmentedMatrix));

%% Find the solution to the linear system
	B = double(rref(AugmentedMatrix))
	xx = B(1,5) - B(1,4)*s;
	yy = B(2,5) - B(2,4)*s;
	zz = B(3,5) - B(3,4)*s;


%% The linear system has infinite solutions, one of which solves the non-linear system. 
%  Find the value of s which solves the non-linear system
	P    = subs(NonLinearExpression{1},{x,y,z,zt},{xx,yy,zz,s});

	% Make the polynomial look pretty
	S    = double(coeffs(P));
	Poly = [num2str(S(3)),'x^2 + ',num2str(S(2)),'x + ',num2str(S(1))]

	% Find the roots of the polynomial, choose the positive one
	s    = double(solve(P))
	t    = max(s);
	X    = subs(xx,tt)
	Y    = subs(yy,tt)
	Z    = subs(zz,tt)

%% Convert to long/lat
	r    = sqrt(X^2+Y^2+Z^2)
	long = 180/pi*atan(Y/X);
	lat  = 90 - 180/pi*acos(Z/r);

	LongLat = [long,lat]	
	disp('In a left-handed coordinate system, the ship is located just off the coast of Rhode Island. ');


