Thuật toán trilateration đa điểm trong Java

Question

Tôi đang cố triển khai thuật toán trilateration vào ứng dụng Android của mình để xác định vị trí trong nhà của người dùng. Tôi đang sử dụng đèn hiệu siêu rộng để có khoảng cách tới các điểm cố định. Tôi đã có thể thích nghi với phương pháp đề xuất trong Trilateration Method Android Java như sau:

public LatLng getLocationByTrilateration(
     LatLng location1, double distance1, 
     LatLng location2, double distance2, 
     LatLng location3, double distance3){ 

    //DECLARE VARIABLES 

    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 tripty; 
    double xval; 
    double yval; 
    double t1; 
    double t2; 
    double t3; 
    double t; 
    double exx; 
    double d; 
    double eyy; 

    //TRANSALTE POINTS TO VECTORS 
    //POINT 1 
    P1[0] = location1.latitude; 
    P1[1] = location1.longitude; 
    //POINT 2 
    P2[0] = location2.latitude; 
    P2[1] = location2.longitude; 
    //POINT 3 
    P3[0] = location3.latitude; 
    P3[1] = location3.longitude; 

    //TRANSFORM THE METERS VALUE FOR THE MAP UNIT 
    //DISTANCE BETWEEN POINT 1 AND MY LOCATION 
    distance1 = (distance1/100000); 
    //DISTANCE BETWEEN POINT 2 AND MY LOCATION 
    distance2 = (distance2/100000); 
    //DISTANCE BETWEEN POINT 3 AND MY LOCATION 
    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 = location1.latitude; 
    t2 = ex[0] * xval; 
    t3 = ey[0] * yval; 
    triptx = t1 + t2 + t3; 

    t1 = location1.longitude; 
    t2 = ex[1] * xval; 
    t3 = ey[1] * yval; 
    tripty = t1 + t2 + t3; 


    return new LatLng(triptx,tripty); 

} 

Sử dụng phương pháp này mang lại cho tôi một vị trí người sử dụng, nhưng không phải là terribly chính xác. Làm thế nào tôi có thể mở rộng điều này để sử dụng hơn 3 địa điểm/khoảng cách đã biết? Lý tưởng nhất là N số điểm mà N> = 3.

Answer 1

Tôi đã tìm thấy giải pháp này trong sách điện tử;

https://books.google.co.uk/books?id=Ki2DMaeeHpUC&pg=PA78

tôi mã này vào một ví dụ Java và có vẻ như làm việc khá tốt trong 3 vòng tròn. Tuy nhiên, tôi không có ý tưởng làm thế nào để thích ứng với công thức này để trang trải trilateration với một điểm thứ 4 và thứ 5 trong giải pháp. Toán học của tôi chỉ là không tốt.

Mã của tôi cho công thức ở đây;

private void findCenter() { 
    int top = 0; 
    int bot = 0; 
    for (int i=0; i<3; i++) { 
     Circle c = circles.get(i); 
     Circle c2, c3; 
     if (i==0) { 
      c2 = circles.get(1); 
      c3 = circles.get(2); 
     } 
     else if (i==1) { 
      c2 = circles.get(0); 
      c3 = circles.get(2); 
     } 
     else { 
      c2 = circles.get(0); 
      c3 = circles.get(1); 
     } 

     int d = c2.x - c3.x; 

     int v1 = (c.x * c.x + c.y * c.y) - (c.r * c.r); 
     top += d*v1; 

     int v2 = c.y * d; 
     bot += v2; 

    } 

    int y = top/(2*bot); 
    Circle c1 = circles.get(0); 
    Circle c2 = circles.get(1); 
    top = c2.r*c2.r+c1.x*c1.x+c1.y*c1.y-c1.r*c1.r-c2.x*c2.x-c2.y*c2.y-2*(c1.y-c2.y)*y; 
    bot = c1.x-c2.x; 
    int x = top/(2*bot); 

    imHere = new Circle(x,y,5); 

} 

tôi sẽ lý tưởng như một giải pháp mã mà có thể làm việc với 3+ nút và cũng có, nơi nhiều điểm đã được sử dụng, sẽ là giải pháp trọng lượng nhiều hơn đối với các điểm xuất phát từ các nút với giá trị bán kính nhỏ .

Bất kỳ ai có ý tưởng nào?

Hoặc làm cách nào để mở rộng công thức sách cho hơn 4 nút hoặc triển khai mã tốt hơn?

Answer 2

Khi được xây dựng theo cách chính xác, vấn đề đa cấp là vấn đề tối ưu hóa.

Hầu hết các ví dụ học thuật, như ví dụ trên wikipedia, xử lý chính xác ba vòng kết nối và giả sử thông tin chính xác hoàn toàn. Những trường hợp này cho phép xây dựng nhiều công thức vấn đề đơn giản hơn với câu trả lời chính xác và thường không thỏa đáng cho các tình huống thực tế như tình huống bạn mô tả.

Vấn đề ở R hoặc R không gian Euclide với khoảng cách có chứa sai số đo, một khu vực (hình elip) hoặc khối lượng (ellipsoid) quan tâm thường thu được thay vì một điểm. Nếu một ước tính điểm là mong muốn thay vì một khu vực, khu vực centroid hoặc khối lượng centroid nên được sử dụng. R không gian đòi hỏi ít nhất 3 điểm không thoái hóa và khoảng cách để có được một vùng duy nhất; và tương tự R không gian đòi hỏi ít nhất 4 điểm không thoái hóa và khoảng cách để có được một vùng duy nhất.

Đây là một nguồn thư viện java mở rằng sẽ dễ dàng đáp ứng nhu cầu của bạn: https://github.com/lemmingapex/Trilateration

Nó sử dụng một phi tuyến hình vuông ít nhất phổ biến tối ưu, thuật toán Levenberg-Marquardt, từ Apache Commons Math.

double[][] positions = new double[][] { { 5.0, -6.0 }, { 13.0, -15.0 }, { 21.0, -3.0 }, { 12.42, -21.2 } }; 
double[] distances = new double[] { 8.06, 13.97, 23.32, 15.31 }; 

NonLinearLeastSquaresSolver solver = new NonLinearLeastSquaresSolver(new TrilaterationFunction(positions, distances), new LevenbergMarquardtOptimizer()); 
Optimum optimum = solver.solve(); 

// the answer 
double[] calculatedPosition = optimum.getPoint().toArray(); 

// error and geometry information 
RealVector standardDeviation = optimum.getSigma(0); 
RealMatrix covarianceMatrix = optimum.getCovariances(0);