3D Trilateration



  • Hi Guys,

    I am trying to translate some code into Qt 5.0.1 for Trilateration in 3D space. The concept is:

    If you have three 3D coordinates and a measured range from those three coordinates to an unknown 3D coordinate, calculate that coordinate.

    The answer I discovered on the Wikipedia Article: "Trilateration ":http://en.wikipedia.org/wiki/Trilateration

    Some code I discovered purports to carry this out (As follows)

    // P1 = array([xA, yA, zA])
    // P2 = array([xB, yB, zB])
    // P3 = array([xC, yC, zC])

    // #from wikipedia
    // #transform to get circle 1 at origin
    // #transform to get circle 2 on x axis
    // ex = (P2 - P1)/(numpy.linalg.norm(P2 - P1))
    // i = dot(ex, P3 - P1)
    // ey = (P3 - P1 - iex)/(numpy.linalg.norm(P3 - P1 - iex))
    // ez = numpy.cross(ex,ey)
    // d = numpy.linalg.norm(P2 - P1)
    // j = dot(ey, P3 - P1)

    // #from wikipedia
    // #plug and chug using above values
    // x = (pow(DistA,2) - pow(DistB,2) + pow(d,2))/(2d)
    // y = ((pow(DistA,2) - pow(DistC,2) + pow(i,2) + pow(j,2))/(2
    j)) - ((i/j)*x)

    // # only one case shown here
    // z = sqrt(pow(DistA,2) - pow(x,2) - pow(y,2))

    // #triPt is an array with ECEF x,y,z of trilateration point
    // triPt = P1 + xex + yey + z*ez

    I have attempted to duplicatre this is Qt as follows, but I seem to have misunderstood the correct usages of QVector3D, can somebody point out the error of my ways.

    @ QVector3D point1 = QVector3D((float) 100, (float) 10, (float) 10);
    QVector3D point2 = QVector3D((float) 200, (float) 40, (float) 20);
    QVector3D point3 = QVector3D((float) 100, (float) 40, (float) 100);

    float d1 = (float)40.31;
    float d2 = (float)40.31;
    float d3 = (float)32.00;
    
    qDebug() << "--------------";
    qDebug() << "Initial Values";
    qDebug() << "--------------";
    
    qDebug() << "Point1 : " << QString::number(point1.x()) << " " << QString::number(point1.y()) << " " << QString::number(point1.z());
    qDebug() << "Point2 : " << QString::number(point2.x()) << " " << QString::number(point2.y()) << " " << QString::number(point2.z());
    qDebug() << "Point3 : " << QString::number(point3.x()) << " " << QString::number(point3.y()) << " " << QString::number(point3.z());
    
    QVector3D ex = QVector3D(point1-point2) / (QVector3D(point1-point2).normalize());
    
    float i = QVector3D.dotProduct(ex, (point3-point1));
    
    QVector3D ey = QVector3D((point3-point1-i*ex)/(point3-point1-i*ex).normalize());
    
    
    QVector3D ez = QVector3D.crossProduct(ex,ey);
    
    float d = (point2-point1).normalize();
    
    float j = QVector3D.dotProduct(ey,(point3-point1));
    
    float x = (pow(d1,2) - pow(d2,2) + pow(d,2))/(2*d);
    float y = ((pow(d1,2) - pow(d3,2) + pow(i,2) + pow(j,2))/(2*j)) - ((i/j)*x);
    float z = sqrt(pow(d1,2) - pow(x,2) - pow(y,2));
    
    QVector3D triPt = P1 + x*ex + y*ey + z*ez;
    
    qDebug() << "--------------------------------------";
    qDebug() << " Point generated by Trilateration     ";
    qDebug() << "--------------------------------------";
    
    qDebug() << "New Point : " << QString::number(triPt.x()) << " " << QString::number(triPt.y()) << " " << QString::number(triPt.z());@

  • Moderators

    Probably you need to say what your actual problem is respectively you are seeing.

    One thing I am suggesting right away is using double instead of float. IIRC and based on "this":http://www.cplusplus.com/reference/cfloat/ float still means a reduced number of significant digits (6 only). I am not sure if modern compilers are not sometimes a bit more generous.

    Nevertheless, if the reduced significant bits are true, you are forcing trouble with squaring, sqrt and your differences. Most floating point computations are done in double (8-byte mode at least), each assignment to float is forcing back to 4 bytes. That is not very healthy or at least worth a check.



  • I suspect you already get compile time errors with this?
    ... e.g. you try to use the return-value of "normalize()" as an vector-object, but this function returns void... or you try to divide vector by another vector, I don't think there is a clear mathematical defined for that and there is no such operator on vector-objects

    Probably you miss-understood the behavior of "normalize()" ... in Qt it only normalizes the vector, it doesn't return the previous length, you have to use "length()"-function for calculating the length of an vector



  • Many thanks to you both, this is a little outside my main expertise, but, you have given me good direction. I will rehash accordingly.

    Kind regards



  • Here is the rehash - works fine after I discovered an error in Wikipedia article :-)

    @ QVector3D point1 = QVector3D((double) 100, (double) 20, (double) 10);
    QVector3D point2 = QVector3D((double) 200, (double) 20, (double) 10);
    QVector3D point3 = QVector3D((double) 100, (double) 20, (double) 110);

    double d1 = (double)92.736;
    double d2 = (double)81.240;
    double d3 = (double)92.736;
    
    qDebug() << "--------------";
    qDebug() << "Initial Values";
    qDebug() << "--------------";
    
    qDebug() << "Point1 : " << QString::number(point1.x()) << " " << QString::number(point1.y()) << " " << QString::number(point1.z());
    qDebug() << "Point2 : " << QString::number(point2.x()) << " " << QString::number(point2.y()) << " " << QString::number(point2.z());
    qDebug() << "Point3 : " << QString::number(point3.x()) << " " << QString::number(point3.y()) << " " << QString::number(point3.z());
    
    QVector3D ex = point2-point1;
    ex.normalize();
    
    double i = QVector3D::dotProduct(ex, (point3-point1));
    
    QVector3D ey = point1-point3-i*ex;
    ey.normalize();
    
    QVector3D ez = QVector3D::crossProduct(ex,ey);
    
    double d = sqrt((pow(point2.x()-point1.x(),2))+(pow(point2.y()-point1.y(),2))+(pow(point2.z()-point1.z(),2)));
    
    double j = QVector3D::dotProduct(ey,(point3-point1));
    
    double x = (pow(d1,2) - pow(d2,2) + pow(d,2))/(2*d);
    double y = ((pow(d1,2) - pow(d3,2) + pow(i,2) + pow(j,2))/(2*j)) - ((i/j)*x);
    double z = sqrt(pow(d1,2) - pow(x,2) - pow(y,2));
    
    QVector3D triPt = point1 + x*ex + y*ey + z*ez;
    
    qDebug() << "--------------------------------------";
    qDebug() << " Point generated by Trilateration     ";
    qDebug() << "--------------------------------------";
    
    qDebug() << "New Point : " << QString::number(triPt.x()) << " " << QString::number(triPt.y()) << " " << QString::number(triPt.z());@
    

    MANY THANKS for the help.


Log in to reply
 

Looks like your connection to Qt Forum was lost, please wait while we try to reconnect.