Simple Multilateration c++ math realisation?

Question

I'm trying to realize a multi trilateration in c++. I have three ranges with which I want to determine a certain point in 3d space. I've already looked up for solutions and found a usable python code snipped. Then I have rewritten the whole snipped in c++ but the results are kind of odd and of the expected results. Their must be some common mistakes which I just don't see (maybe it's too obvious lol). The test data is from here, and I know that this linear way of solving the gps trillat problem is not even remotely accurate but that's not important for my purpose. The resulting vector is: -3.39803e+29, 3.39803e+29, -3.39803e+29 which cannot be true.

/* Recursive function for finding determinant of matrix.
  n(rows) is current dimension of A[][]. */
double clacDeterminant(double **A, int rows)
{
  double D = 0; // Initialize result

  //  Base case : if matrix contains single element
  if (rows == 1)
      return A[0][0];

  int sign = 1;  // To store sign multiplier

  // Iterate for each element of first row
  for (int f = 0; f < rows; f++)
  {
      // Getting Cofactor of A[0][f]
      double **temp = getCofactor(A, 0, f, rows);
      D += sign * A[0][f] * clacDeterminant(A, rows - 1);

      // terms are to be added with alternate sign
      sign = -sign;
  }

  return D;
}

// Function to get adjoint of A[N][N] in adj[N][N].
double** calcAdjoint(double **A, int matrixArows)
{
  double** adj = allocate2Ddouble(matrixArows, posMTrillatASize);
  int sign = 0;

  for (int i=0; i<matrixArows; i++)
  {
      for (int j=0; j<posMTrillatASize; j++)
      {
          // Get cofactor of A[i][j]
          double **temp = getCofactor(A, i, j, posMTrillatASize);

          // sign of adj[j][i] positive if sum of row
          // and column indexes is even.
          sign = ((i+j)%2==0)? 1: -1;

          // Interchanging rows and columns to get the
          // transpose of the cofactor matrix
          adj[i][j] = (sign)*(clacDeterminant(A, posMTrillatASize-1));
      }
  }
  return adj;
}

// by https://www.programiz.com/cpp-programming/examples/matrix-multiplication-function modified by L.K.
// method assumes that matrices have same dim sizes
double** multiplyMatrices(double **matrixA, double **matrixB, int matrixArows)
{
  double** outputMatrix = allocate2Ddouble(matrixArows, posMTrillatASize);
  int i, j, k;

  // Initializing elements of matrix mult to 0.
  for(i = 0; i < matrixArows; ++i)
  {
    for(j = 0; j < posMTrillatASize; ++j)
    {
      outputMatrix[i][j] = 0;
    }
  }

  // Multiplying matrix matrixA and matrixB and storing in array mult.
  for(i = 0; i < matrixArows; ++i)
  {
    for(j = 0; j < posMTrillatASize; ++j)
    {
      for(k=0; k<posMTrillatASize; ++k)
      {
        outputMatrix[i][j] += matrixA[i][k] * matrixB[k][j];
      }
    }
  }
  return outputMatrix;
}

double* multiplyMatrixByVector(double **matrixA, double vectorB[])
{
  double vectorRes[posMTrillatASize];
  memset(vectorRes, 0, posMTrillatASize*sizeof(double));

  for (int i=0;i<posMTrillatASize;i++)
  {
      for (int j=0;j<posMTrillatASize;j++)
      {
          vectorRes[i]+= (matrixA[i][j]*vectorB[j]);
      }
  }

  return vectorRes;
}

double** transpose2DimMatrix(double **inputArr, int matrixArows, int transpose2DimMatrix)
{
  double **outputArr = allocate2Ddouble(transpose2DimMatrix, matrixArows);
  for (int i = 0; i < matrixArows; ++i)
  {
    for (int j = 0; j < transpose2DimMatrix; ++j)
    {
      outputArr[j][i] = inputArr[i][j];
    }
  }
  return outputArr;
}

// Function to calculate and store inverse, returns 0 if false
// matrix is singular by https://www.geeksforgeeks.org/adjoint-inverse-matrix/
double** calcInverse(double **A, int matrixArows)
{
  double** inverse = allocate2Ddouble(matrixArows, posMTrillatASize);
  // Find determinant of A[][]
  int det = clacDeterminant(A, posMTrillatASize);
  if (det == 0)
  {
      cout << "Singular matrix, can't find its inverse";
      return 0;
  }

  // Find adjoint
  double **adj = calcAdjoint(A, matrixArows);

  // Find Inverse using formula "inverse(A) = adj(A)/det(A)"
  for (int i=0; i<matrixArows; i++)
      for (int j=0; j<posMTrillatASize; j++)
          inverse[i][j] = adj[i][j]/double(det);

  return inverse;
}

double* leastSquareReg(double **matrixA, double vectorB[], int matrixArows, int matrixAcol)
{
  double **matrixATransposed = transpose2DimMatrix(matrixA, matrixArows, matrixAcol);
  double **matrixATransposedA = multiplyMatrices(matrixATransposed, matrixA, matrixAcol);
  double **matrixATransposedAInverse = calcInverse(matrixATransposedA, matrixArows);
  double **matrixATransposedAAdd = multiplyMatrices(matrixATransposedAInverse, matrixATransposed, matrixArows);
  double *finalX = multiplyMatrixByVector(matrixATransposedAAdd, vectorB);

  return finalX;
}

ecefPos trillatPosFromRange(satLocation finalSatPos, satRanges finalSatRanges)
{
  std::map<int, ecefPos>::iterator it_;
  std::map<int, double>::iterator finalSatRangesMap;
  int matrixArows, matrixAcol = 0;
  double x, y, z;
  double Am, Bm, Cm, Dm;
  double range;
  int nSat = finalSatPos.locations.size();
  ecefPos finalPos = { 0.0, 0.0, 0.0 };

  matrixAcol = posMTrillatASize;
  matrixArows = nSat;

  double **matrixA = allocate2Ddouble(nSat, 3);
  double vectorB[posMTrillatASize] = {};

  int i = 0;
  for (it_ = finalSatPos.locations.begin(); it_ != finalSatPos.locations.end(); it_++)
  {
    // look up for pseudo range with same sat id
    finalSatRangesMap = finalSatRanges.ranges.find(it_->first);
    range = finalSatRangesMap->second;

    if (it_ != finalSatPos.locations.end())
    {
      x = it_->second.x;
      y = it_->second.y;
      z = it_->second.z;
      Am = -2*x;
      Bm = -2*y;
      Cm = -2*z;
      Dm = EARTH_RADIUS_KM*EARTH_RADIUS_KM + (pow(x,2)+pow(y,2)+pow(z,2)) - pow(range,2);

      matrixA[i][0] = Am;
      matrixA[i][1] = Bm;
      matrixA[i][2] = Cm;
      vectorB[i] = Dm;
      i++;

    } else
    {
      std::cout << "could not find sat pos for user pos trilateration" << std::endl;
    }
  }

  // least square regression
  double *finalECEF = leastSquareReg(matrixA, vectorB, matrixArows, matrixAcol);

  finalPos.x = finalECEF[0];
  finalPos.y = finalECEF[1];
  finalPos.z = finalECEF[2];

  for (int i = 0; i < 5; ++i)
  {
    std::cout << finalECEF[i] << std::endl;
  }
  return finalPos;
}