Двухмерная трилатерация с использованием линейного метода наименьших квадратов в C#

Я делаю двумерную трилатерацию, используя MathNet для матриц и векторов. Это мой код:

        public static double[] trilaterate2DLinear(double[] pA, double[] pB, double[] pC, double rA, double rB, double rC) {
        //Convert doubles to vectors for processing
        Vector<double> vA = Vector<double>.Build.Dense(pA);
        Vector<double> vB = Vector<double>.Build.Dense(pB);
        Vector<double> vC = Vector<double>.Build.Dense(pC);

        //Declare elements of b vector
        //bBA = 1/2 * (rA^2 - rB^2 + dBA^2)
        double[] b = {0, 0};
        b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) + Math.Pow(getDistance(pB, pA), 2));
        b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) + Math.Pow(getDistance(pC, pA), 2));
        //Convert b array to vector form
        Vector<double> vb = Vector<double>.Build.Dense(b);

        //Build A array
        //A =   {x2 -x1, y2 - y1}
        //      {x3 - x1, y3 - y1}
        double[,] A =  { { pB[0] - pA[0], pB[1] - pA[1] }, { pC[0] - pA[0], pC[1] - pA[1] } };
        //Convert A to Matrix form
        Matrix<double> mA = Matrix<double>.Build.DenseOfArray(A);
        //Declare Transpose of A matrix;
        Matrix<double> mAT = mA.Transpose();

        //Declare solution vector x to 0
        Vector<double> x = Vector<double>.Build.Dense(2);

        //Check if A*AT is non-singular (non 0 determinant)
        if (mA.Multiply(mAT).Determinant() == 0)
        {
            //x = ((AT * A)^-1)*AT*b
            x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
        }
        else 
        { 
            //TODO case for A*AT to be singular
            x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
        }

        //final position is x + vA
        //return as double so as not
        return (x.Add(vA)).ToArray();
    }

    //Gets the Euclidean distance between two points
    private static double getDistance(double[] p1, double[] p2)
    {
        //d^2 = (p1[0] - p2[0])^2 + (p1[1] - p2[1]);
        double distSquared = Math.Pow((p1[0] - p2[0]),2) + Math.Pow((p1[1] - p2[1]),2);
        return Math.Sqrt(distSquared);
    }

pA, pB и pC — координаты маяков, а rA, rB и rC — расстояния от каждого маяка до пользователя. Есть ли что-то очевидное, что я делаю неправильно? Может быть, нужно изменить порядок умножения матриц, но я недостаточно знаком с линейным методом наименьших квадратов, чтобы иметь возможность отслеживать матрицы и говорить.