C#加权坐标的三角定位算法

最近在做一个室内定位的项目，用到三角定位算法，在这记录一下。

参数说明：Dictionary<(double, double), double> 为Dictionary<(x, y), 到未知点位的距离> 。

public static (double, double) WeightedTrilateration(Dictionary<(double, double), double> distances)
    {
      if (distances.Count < 3)
      {
        throw new ArgumentException("至少需要三个坐标点的距离！");
      }

      double sumX = 0;
      double sumY = 0;
      double sumW = 0;

      foreach (var kvp in distances)
      {
        double x1 = kvp.Key.Item1;
        double y1 = kvp.Key.Item2;
        double d = kvp.Value;

        double w = 1 / Math.Pow(d, 2);
        sumX += w * x1;
        sumY += w * y1;
        sumW += w;
      }

      double x = sumX / sumW;
      double y = sumY / sumW;

      return (x, y);
    }