function [ solution ] = solveMultilateration( sourcePos, distVec )
%SOLVEMULTILATERATION solves multilateration for 4+ points
%   sourcePos: 3xn matrix of known positions
%   distVec: 1xn vector of known distances from each source position to
%   solution point
%   solution: 3x1 output position

n = length(distVec);

origin = sourcePos(:,1);

diffVec = distVec(2:n) - distVec(1);
ABC = zeros(n-2,3);
D = zeros(n-2,1);

for i = 2:length(diffVec);
    newPos = sourcePos(:,i)-origin;
    ABC(i-1,:) = 2*newPos';
    D(i-1) = distVec(1)^2-distVec(i)^2+sum(newPos.^2);
end

solution = ABC\D;
solution = solution + origin;

end

