CODEWITHSUNDEEP
algorithmmatlabmathlocation
jtjenkins
jtjenkins

Reputation: 33

Position of object with n known points and distances

I'm doing some work with triangulating the position of a receiver based on its distance to known points in space. I basically get a set of distances D[N] from the nearby broadcast points, and have a lookup table to give me the X, Y, and Z values for each. I need to find the X, Y, and Z points of my receiver.

I've seen that you can use trilateration to solve this problem in 2D cases, but it doesn't work for 3D. I would like to be able to use N points in order to improve accuracy, but I suppose I could also use the closest 4 points.

My issue is I have no idea how to programmatically solve the system of equations algebraically so it can be done in my program. I've seen a few solutions using things like Matlab, but I don't have the same tools.

This seems to solve the problem, if someone knows how to translate Matlab into a C language (I've never used Matlab): Determine the position of a point in 3D space given the distance to N points with known coordinates

Upvotes: 0

Views: 1379

Answers (2)

kraskevich
kraskevich

Reputation: 18566

Here is my solution in C++(should be easy to convert to plain C). It does not use any advanced algebra so it does not require any non-standard libraries. It works good when the number of points is rather big(the error gets smaller when the number of points grows), so it is better to pass all the points that you have to the solve method. It minimizes the sum of squared differences using sub-gradient descent. 

const int ITER = 2000;
const double ALPHA = 2.0;
const double RATIO = 0.99;

double sqr(double a) {
  return a * a;
}

struct Point {
  double x;
  double y;
  double z;

  Point(double _x=0.0, double _y=0.0, double _z=0.0): x(_x), y(_y), z(_z) {}

  double dist(const Point &other) const {
    return sqrt(sqr(x - other.x) + sqr(y - other.y) + sqr(z - other.z));  
  }  

  Point operator + (const Point& other) const {
    return Point(x + other.x, y + other.y, z + other.z);
  }

  Point operator - (const Point& other) const {
    return Point(x - other.x, y - other.y, z - other.z);
  }

  Point operator * (const double mul) const {
    return Point(x * mul, y * mul, z * mul);
  }
};

Point solve(const vector<Point>& given, const vector<double>& dist) {
  Point res;
  double alpha = ALPHA;
  for (int iter = 0; iter < ITER; iter++) {
      Point delta;
      for (int i = 0; i < given.size(); i++) {
        double d = res.dist(given[i]);
        Point diff = (given[i] - res) * (alpha * (d - dist[i]) / max(dist[i], d));
        delta = delta + diff;
      }
      delta = delta * (1.0 / given.size()); 
      alpha *= RATIO;   
      res = res + delta;
  }
  return res;
}

Upvotes: 1

jmp111
jmp111

Reputation: 71

Here is an answer for using Matlab (which is not what you are asking for), which uses the Nelder-Mead simplex optimization algorithm. Because you do not have access to Matlab, you could use R (freely available). The code above is easily translatable to R and in R you can use the Nelder-Mead algorithm (to replace 'fminsearch' using the neldermead package. For differences between R and Matlab (and Octave), see: http://cran.r-project.org/doc/contrib/R-and-octave.txt

function Ate=GetCoordinate(Atr,Dte)
    % Atr = coordinates for known points
    % Dte = distances for receiver to each row in Atr
    % At5e = coordinate for receiver
    [~,ii]=sort(Dte,'ascend');
    Ate=mean(Atr(ii(1:4),:)); % (reasonable) start point
    [Ate,~]=fminsearch(@(Ate) ED(Ate,Atr,Dte),Ate); % Uses Nelder-Mead simplex algorithm to find optimum
end

function d=ED(Ate,Atr,Dte) % calculates the sum of the squared difference between the measured distances and distances based on coordinate Ate (for receiver)
    for k=1:size(Dte,1)
        d(k,1)=sqrt((Atr(k,:)-Ate)*(Atr(k,:)-Ate)'); % Euclidean distance
    end
    d=sqrt(sum((Dte-d).^2));
end

Upvotes: 0

Related Questions