3D Trilateration

ApteryxOz

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))/(2j)) - ((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());@

koahnig

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.

sl.sy.ifm

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

ApteryxOz

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

ApteryxOz

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.