Trilateration Method Android Java

2019-01-22 16:53发布

问题:

I have a little problem... Does anyone knows about how can i implement this?

I need a method in java like this:

public /*COORDINATE OBJECT*/ getTrilaterationPointBetween(
/*COORDINATE OBJECT*/ coordinate1, double distance1,
/*COORDINATE OBJECT*/ coordinate2, double distance2,
/*COORDINATE OBJECT*/ coordinate3, double distance3){

    //MAGIC

    return /*COORDINATE OBJECT*/;
}

In practice this method will receive 3 coordinates and the distance between "myLocation" with one of them...

I need the algorithm of the Trilateration!!

I did some searches and made some tests and the results are driving me into the middle of the pacific!!

About the OBJECT, is really simple, have 2 variables representing Latitude and Longitude.

I have tried this method:

static double[] MyTrilateration(Ponto ponto1, double dist1,
                                    Ponto ponto2, double dist2,
                                    Ponto ponto3, double dist3) {

        double[] tmpWAP1 = new double[3];
        double[] tmpWAP2 = new double[3];
        double[] tmpWAP3 = new double[3];

        double tmpLat2, tmpLong2, tmpLat3, tmpLong3;
        double tmpSlide, deg;
        double MyLat, MyLong;

        double[] MyLocation = new double[2];

        tmpLat2  = ponto2.getX() - ponto1.getX();
        tmpLong2 = ponto2.getY() - ponto1.getY();
        tmpLat3  = ponto3.getX() - ponto1.getX();
        tmpLong3 = ponto3.getY() - ponto1.getY();

        tmpSlide = Math.sqrt(Math.pow(tmpLat2,2)+Math.pow(tmpLong2,2));

        deg = (180/Math.PI)*Math.acos( Math.abs(tmpLat2)/Math.abs(tmpSlide));

        if( (tmpLat2>0 && tmpLong2>0) ) {
            deg = 360 - deg;
        }
        else if( (tmpLat2<0 && tmpLong2>0) ) {
            deg = 180 + deg;
        }
        else if( (tmpLat2<0 && tmpLong2<0)){
            deg = 180 - deg;
        }
        else if( (tmpLat2>0 && tmpLong2<0)) {
            deg = deg;
        }

        tmpWAP1[0] = 0.0;
        tmpWAP1[1] = 0.0;
        tmpWAP1[2] = dist1;
        tmpWAP2 = myRotation(tmpLat2, tmpLong2, dist2, deg);
        tmpWAP3 = myRotation(tmpLat3, tmpLong3, dist3, deg);


        MyLat = (Math.pow(tmpWAP1[2],2)-Math.pow(tmpWAP2[2],2)+Math.pow(tmpWAP2[0],2))/(2*tmpWAP2[0]);

        MyLong = (Math.pow(tmpWAP1[2],2)-Math.pow(tmpWAP3[2],2)-Math.pow(MyLat,2)
                +Math.pow(MyLat-tmpWAP3[0],2)+Math.pow(tmpWAP3[1], 2))/(2*tmpWAP3[1]);

        MyLocation = myRotation(MyLat, MyLong, 0, -deg);

        MyLocation[0] = MyLocation[0] + ponto1.getX();
        MyLocation[1] = MyLocation[1] + ponto1.getY();

        return MyLocation;
    }

and this method:

public String getCoordinateWith(
            Ponto a, Ponto b, Ponto c,
            Float dA,
            Float dB,
            Float dC) {
        Float W, Z, x, y, y2;
        W = dA*dA - dB*dB - a.getX()*a.getX() - a.getY()*a.getY() + b.getX()*b.getX() + b.getY()*b.getY();
        Z = dB*dB - dC*dC - b.getX()*b.getX() - b.getY()*b.getY() + c.getX()*c.getX() + c.getY()*c.getY();

        x = (W*(c.getY()-b.getY()) - Z*(b.getY()-a.getY())) / (2 * ((b.getX()-a.getX())*(c.getY()-b.getY()) - (c.getX()-b.getX())*(b.getY()-a.getY())));
        y = (W - 2*x*(b.getX()-a.getX())) / (2*(b.getY()-a.getY()));
        //y2 is a second measure of y to mitigate errors
        y2 = (Z - 2*x*(c.getX()-b.getX())) / (2*(c.getY()-b.getY()));
        y = (y + y2) / 2;
        return "Position: " + x + " , " + y;
    }

Thank you!

回答1:

I got it! That'is my method, works for me, is not 100% accurate! Works with z = 0 and only with 3 different points(2D coordinates).

The class "Ponto" is a simple class with two values (X and Y/Lat and Lon), "double".

public static Ponto getLocationByTrilateration(
                Ponto ponto1, double distance1,
                Ponto ponto2, double distance2,
                Ponto ponto3, double distance3){

        //DECLARACAO DE VARIAVEIS
        Ponto retorno = new Ponto();
        double[] P1   = new double[2];
        double[] P2   = new double[2];
        double[] P3   = new double[2];
        double[] ex   = new double[2];
        double[] ey   = new double[2];
        double[] p3p1 = new double[2];
        double jval  = 0;
        double temp  = 0;
        double ival  = 0;
        double p3p1i = 0;
        double triptx;
        double xval;
        double yval;
        double t1;
        double t2;
        double t3;
        double t;
        double exx;
        double d;
        double eyy;

        //TRANSFORMA OS PONTOS EM VETORES
        //PONTO 1
        P1[0] = ponto1.getX();
        P1[1] = ponto1.getY();
        //PONTO 2
        P2[0] = ponto2.getX();
        P2[1] = ponto2.getY();
        //PONTO 3
        P3[0] = ponto3.getX();
        P3[1] = ponto3.getY();

        //TRANSFORMA O VALOR DE METROS PARA A UNIDADE DO MAPA
        //DISTANCIA ENTRE O PONTO 1 E A MINHA LOCALIZACAO
        distance1 = (distance1 / 100000);
        //DISTANCIA ENTRE O PONTO 2 E A MINHA LOCALIZACAO
        distance2 = (distance2 / 100000);
        //DISTANCIA ENTRE O PONTO 3 E A MINHA LOCALIZACAO
        distance3 = (distance3 / 100000);

        for (int i = 0; i < P1.length; i++) {
            t1   = P2[i];
            t2   = P1[i];
            t    = t1 - t2;
            temp += (t*t);
        }
        d = Math.sqrt(temp);
        for (int i = 0; i < P1.length; i++) {
            t1    = P2[i];
            t2    = P1[i];
            exx   = (t1 - t2)/(Math.sqrt(temp));
            ex[i] = exx;
        }
        for (int i = 0; i < P3.length; i++) {
            t1      = P3[i];
            t2      = P1[i];
            t3      = t1 - t2;
            p3p1[i] = t3;
        }
        for (int i = 0; i < ex.length; i++) {
            t1 = ex[i];
            t2 = p3p1[i];
            ival += (t1*t2);
        }
        for (int  i = 0; i < P3.length; i++) {
            t1 = P3[i];
            t2 = P1[i];
            t3 = ex[i] * ival;
            t  = t1 - t2 -t3;
            p3p1i += (t*t);
        }
        for (int i = 0; i < P3.length; i++) {
            t1 = P3[i];
            t2 = P1[i];
            t3 = ex[i] * ival;
            eyy = (t1 - t2 - t3)/Math.sqrt(p3p1i);
            ey[i] = eyy;
        }
        for (int i = 0; i < ey.length; i++) {
            t1 = ey[i];
            t2 = p3p1[i];
            jval += (t1*t2);
        }
        xval = (Math.pow(distance1, 2) - Math.pow(distance2, 2) + Math.pow(d, 2))/(2*d);
        yval = ((Math.pow(distance1, 2) - Math.pow(distance3, 2) + Math.pow(ival, 2) + Math.pow(jval, 2))/(2*jval)) - ((ival/jval)*xval);

        t1 = ponto1.getX();
        t2 = ex[0] * xval;
        t3 = ey[0] * yval;
        triptx = t1 + t2 + t3;
        retorno.setX(triptx);
        t1 = ponto1.getY();
        t2 = ex[1] * xval;
        t3 = ey[1] * yval;
        triptx = t1 + t2 + t3;
        retorno.setY(triptx);

        return retorno;
    }