4. Simple Multilateration Mathe Realisation?

Simple Multilateration Mathe Realisation?

  • M

    Ich versuche eine Multi-Trilateration in C ++ zu realisieren. Ich habe drei Abstände, mit denen ich einen bestimmten Punkt im 3D-Raum bestimmen möchte. Ich habe bereits nach Lösungen gesucht und einen verwendbaren Python-Code abgeschnitten gefunden. Dann habe ich das Ganze in C ++ umgeschrieben, aber die Ergebnisse sind irgendwie seltsam und anders als die erwarteten Ergebnissen. Es müssen einige simple Fehler sein, die ich einfach nicht sehe (vielleicht ist es zu offensichtlich, lol).
    Die Testdaten stammen von hier, und ich weiß, dass diese lineare Methode zur Lösung des GPS-Trillat-Problems nicht einmal im entferntesten genau sind, das ist aber für meinen Zweck nicht wichtig.
    Ergebniss Vektor is: -3.39803e+29, 3.39803e+29, -3.39803e+29; kann aber icht stimmen.
    Ich weiß das meine Frage nicht wirklich konkret ist aber ich arbeite alleine und bin noch Schüler, evtl. ist auch in meiner Methodik ein Fehler und das lässt sich so nicht berechnen, deswegen ist die Frage sehr allgemein.
    Grüße aus Franken

    /* 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;
}
    0
  • W

    Vorbemerkungen

    • überleg dir die Sprache. Insbesondere die vielen Pointer und Doppelpointer sehen eher nach C als C++ aus.
    • ein double** ist kein 2d-Array, sondern du hast hier einen Pointer (bzw. einen Pointer, der möglicherweise auf ein 1d-Array zeigt) auf Pointer. Damit sind deine Daten NICHT hintereinander im Speicher. Du hast die Pointer für die Zeilen alle hintereinander im Speicher und die Zeilenendaten innerhalb je einer Zeile sind hintereinander, aber es ist nicht sichergestellt, dass die 2 Zeilen hintereinaner liegen. In der Regel ist es besser (effizienter und auch einfacher zu programmieren), einen großen Speicherbereich für die gesamte Matrix zu haben und die Position mit zeile * nSpalten + spalte zu errechnen.
    • du hast viele allocate2d-Calls, aber ich sehe kein delete in deinem Code, d.h. du hast Speicherlecks.
    • das ist kein vollständiges Programm, wir können es also nicht einfach so testen

    Wie solltest du vorgehen:

    • stelle sicher, dass du keine globalen Variablen verwendest. Insbesondere scheint posMTrillatASize global zu sein. Warum aber sollte eine Funktion wie multiplyMatrices davon abhängen? Matrix*Matrix sollte unabhängig davon sein und mit jedem Paar von Matrizen funktionieren, die größenkompatibel für Multiplikation sind, d.h. Anzahl Spalten der linken = Anzahl Zeilen der rechten Matrix.
    • stelle für jede einzelne Funktion sicher, dass sie wie gewünscht funktioniert. Dazu schreibst du am besten Tests. Für das transpose2DimMatrix könntest du z.B. eine Matrix (123456)\left(\begin{array}{cc}1&2\\3&4\\5&6 \end{array}\right) reingeben und sicherstellen, dass (135246)\left(\begin{array}{cc}1&3&5\\2&4&6\end{array}\right) rauskommt. Und so weiter. Dann machst du noch ein paar weitere Tests, z.B. für 1-zeilige oder 1-spaltige Matrizen. Je mehr verschiedene Tests, desto besser. Diese Tests schreibst du als Code, sodass du sie jederzeit wiederholen kannst.
    • wenn du sicher bist, dass die einzelnen Funktionen korrekt sind, dann stelle sicher, dass die Kombination auch passt.
    0
Anmelden zum Antworten