Question

我做了一些工作，根据接收器与空间中已知点的距离对接收器的位置进行三角测量。我基本上从附近的广播点得到一组距离D [N]，并有一个查找表给我每个的X，Y和Z值。我需要找到接收器的X，Y和Z点。

我已经看到你可以使用trilateration在2D情况下解决这个问题，但它不适用于3D。我希望能够使用N个点来提高准确度，但我想我也可以使用最接近的4个点。

我的问题是我不知道如何以编程方式解决方程组，因此可以在我的程序中完成。我已经看过一些像Matlab这样的解决方案，但我没有相同的工具。

这似乎解决了这个问题，如果有人知道如何将Matlab翻译成C语言（我从未使用过Matlab）：Determine the position of a point in 3D space given the distance to N points with known coordinates

Answer 1

这是使用Matlab（这不是你要求的）的答案，它使用Nelder-Mead单纯形优化算法。因为您无法访问Matlab，所以您可以使用R（免费提供）。上面的代码很容易翻译成R，在R中你可以使用Nelder-Mead算法（用neldermead包代替'fminsearch'。对于R和Matlab（和Octave）之间的差异，请参阅：{{3 }}

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

Answer 2

这是我在C ++中的解决方案（应该很容易转换为普通的C）。它不使用任何高级代数，因此它不需要任何非标准库。当点数相当大（当点数增加时误差变小）时效果很好，所以最好将所有点传递给solve方法。它使用子梯度下降最小化差的平方和。

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;
}

具有n个已知点和距离的物体的位置

2 个答案: