trilateration

Multiliteration implementation with inaccurate distance data

烂漫一生 — Sun, 07 Feb 2021 02:20:36 +0000

Multiliteration implementation with inaccurate distance data 烂漫一生 2021-02-07 10:20:36

问题

I am trying to create an android smartphone application which uses Apples iBeacon technology to determine the current indoor location of itself. I already managed to get all available beacons and calculate the distance to them via the rssi signal.

Currently I face the problem, that I am not able to find any library or implementation of an algorithm, which calculates the estimated location in 2D by using 3 (or more) distances of fixed points with the condition, that these distances are not accurate (which means, that the three "trilateration-circles" do not intersect in one point).

I would be deeply grateful if anybody can post me a link or an implementation of that in any common programming language (Java, C++, Python, PHP, Javascript or whatever). I already read a lot on stackoverflow about that topic, but could not find any answer I were able to convert in code (only some mathematical approaches with matrices and inverting them, calculating with vectors or stuff like that).

EDIT

I thought about an own approach, which works quite well for me, but is not that efficient and scientific. I iterate over every meter (or like in my example 0.1 meter) of the location grid and calculate the possibility of that location to be the actual position of the handset by comparing the distance of that location to all beacons and the distance I calculate with the received rssi signal.

Code example:

public Location trilaterate(ArrayList<Beacon> beacons, double maxX, double maxY)
{
    for (double x = 0; x <= maxX; x += .1)
    {
        for (double y = 0; y <= maxY; y += .1)
        {
            double currentLocationProbability = 0;
            for (Beacon beacon : beacons)
            {
                // distance difference between calculated distance to beacon transmitter
                // (rssi-calculated distance) and current location:
                // |sqrt(dX^2 + dY^2) - distanceToTransmitter|
                double distanceDifference = Math
                    .abs(Math.sqrt(Math.pow(beacon.getLocation().x - x, 2)
                                   + Math.pow(beacon.getLocation().y - y, 2))
                         - beacon.getCurrentDistanceToTransmitter());
                // weight the distance difference with the beacon calculated rssi-distance. The
                // smaller the calculated rssi-distance is, the more the distance difference
                // will be weighted (it is assumed, that nearer beacons measure the distance
                // more accurate)
                distanceDifference /= Math.pow(beacon.getCurrentDistanceToTransmitter(), 0.9);
                // sum up all weighted distance differences for every beacon in
                // "currentLocationProbability"
                currentLocationProbability += distanceDifference;
            }
            addToLocationMap(currentLocationProbability, x, y);
            // the previous line is my approach, I create a Set of Locations with the 5 most probable locations in it to estimate the accuracy of the measurement afterwards. If that is not necessary, a simple variable assignment for the most probable location would do the job also
        }
    }
    Location bestLocation = getLocationSet().first().location;
    bestLocation.accuracy = calculateLocationAccuracy();
    Log.w("TRILATERATION", "Location " + bestLocation + " best with accuracy "
                           + bestLocation.accuracy);
    return bestLocation;
}

Of course, the downside of that is, that I have on a 300m² floor 30.000 locations I had to iterate over and measure the distance to every single beacon I got a signal from (if that would be 5, I do 150.000 calculations only for determine a single location). That's a lot - so I will let the question open and hope for some further solutions or a good improvement of this existing solution in order to make it more efficient.

Of course it has not to be a Trilateration approach, like the original title of this question was, it is also good to have an algorithm which includes more than three beacons for the location determination (Multilateration).

回答1:

If the current approach is fine except for being too slow, then you could speed it up by recursively subdividing the plane. This works sort of like finding nearest neighbors in a kd-tree. Suppose that we are given an axis-aligned box and wish to find the approximate best solution in the box. If the box is small enough, then return the center.

Otherwise, divide the box in half, either by x or by y depending on which side is longer. For both halves, compute a bound on the solution quality as follows. Since the objective function is additive, sum lower bounds for each beacon. The lower bound for a beacon is the distance of the circle to the box, times the scaling factor. Recursively find the best solution in the child with the lower lower bound. Examine the other child only if the best solution in the first child is worse than the other child's lower bound.

Most of the implementation work here is the box-to-circle distance computation. Since the box is axis-aligned, we can use interval arithmetic to determine the precise range of distances from box points to the circle center.

_{P.S.: Math.hypot is a nice function for computing 2D Euclidean distances.}

回答2:

Instead of taking confidence levels of individual beacons into account, I would instead try to assign an overall confidence level for your result after you make the best guess you can with the available data. I don't think the only available metric (perceived power) is a good indication of accuracy. With poor geometry or a misbehaving beacon, you could be trusting poor data highly. It might make better sense to come up with an overall confidence level based on how well the perceived distance to the beacons line up with the calculated point assuming you trust all beacons equally.

I wrote some Python below that comes up with a best guess based on the provided data in the 3-beacon case by calculating the two points of intersection of circles for the first two beacons and then choosing the point that best matches the third. It's meant to get started on the problem and is not a final solution. If beacons don't intersect, it slightly increases the radius of each up until they do meet or a threshold is met. Likewise, it makes sure the third beacon agrees within a settable threshold. For n-beacons, I would pick 3 or 4 of the strongest signals and use those. There are tons of optimizations that could be done and I think this is a trial-by-fire problem due to the unwieldy nature of beaconing.

import math

beacons = [[0.0,0.0,7.0],[0.0,10.0,7.0],[10.0,5.0,16.0]] # x, y, radius

def point_dist(x1,y1,x2,y2):
    x = x2-x1
    y = y2-y1
    return math.sqrt((x*x)+(y*y))

# determines two points of intersection for two circles [x,y,radius]
# returns None if the circles do not intersect
def circle_intersection(beacon1,beacon2):
    r1 = beacon1[2]
    r2 = beacon2[2]
    dist = point_dist(beacon1[0],beacon1[1],beacon2[0],beacon2[1])
    heron_root = (dist+r1+r2)*(-dist+r1+r2)*(dist-r1+r2)*(dist+r1-r2)
    if ( heron_root > 0 ):
        heron = 0.25*math.sqrt(heron_root)
        xbase = (0.5)*(beacon1[0]+beacon2[0]) + (0.5)*(beacon2[0]-beacon1[0])*(r1*r1-r2*r2)/(dist*dist)
        xdiff = 2*(beacon2[1]-beacon1[1])*heron/(dist*dist) 
        ybase = (0.5)*(beacon1[1]+beacon2[1]) + (0.5)*(beacon2[1]-beacon1[1])*(r1*r1-r2*r2)/(dist*dist)
        ydiff = 2*(beacon2[0]-beacon1[0])*heron/(dist*dist) 
        return (xbase+xdiff,ybase-ydiff),(xbase-xdiff,ybase+ydiff)
    else:
        # no intersection, need to pseudo-increase beacon power and try again
        return None

# find the two points of intersection between beacon0 and beacon1
# will use beacon2 to determine the better of the two points
failing = True
power_increases = 0
while failing and power_increases < 10:
    res = circle_intersection(beacons[0],beacons[1])
    if ( res ):
        intersection = res
    else:
        beacons[0][2] *= 1.001
        beacons[1][2] *= 1.001
        power_increases += 1
        continue
    failing = False

# make sure the best fit is within x% (10% of the total distance from the 3rd beacon in this case)
# otherwise the results are too far off
THRESHOLD = 0.1

if failing:
    print 'Bad Beacon Data (Beacon0 & Beacon1 don\'t intersection after many "power increases")'
else:
    # finding best point between beacon1 and beacon2
    dist1 = point_dist(beacons[2][0],beacons[2][1],intersection[0][0],intersection[0][1])
    dist2 = point_dist(beacons[2][0],beacons[2][1],intersection[1][0],intersection[1][1])
    if ( math.fabs(dist1-beacons[2][2]) < math.fabs(dist2-beacons[2][2]) ):
        best_point = intersection[0]
        best_dist = dist1
    else:
        best_point = intersection[1]
        best_dist = dist2
    best_dist_diff = math.fabs(best_dist-beacons[2][2])
    if best_dist_diff < THRESHOLD*best_dist:
        print best_point
    else:
        print 'Bad Beacon Data (Beacon2 distance to best point not within threshold)'

If you want to trust closer beacons more, you may want to calculate the intersection points between the two closest beacons and then use the farther beacon to tie-break. Keep in mind that almost anything you do with "confidence levels" for the individual measurements will be a hack at best. Since you will always be working with very bad data, you will defintiely need to loosen up the power_increases limit and threshold percentage.

回答3:

You have 3 points : A(xA,yA,zA), B(xB,yB,zB) and C(xC,yC,zC), which respectively are approximately at dA, dB and dC from you goal point G(xG,yG,zG). Let's say cA, cB and cC are the confidence rate ( 0 < cX <= 1 ) of each point. Basically, you might take something really close to 1, like {0.95,0.97,0.99}. If you don't know, try different coefficient depending of distance avg. If distance is really big, you're likely to be not very confident about it.

Here is the way i'll do it :

var sum = (cA*dA) + (cB*dB) + (cC*dC);
dA = cA*dA/sum;
dB = cB*dB/sum;
dC = cC*dC/sum;

xG = (xA*dA) + (xB*dB) + (xC*dC);
yG = (yA*dA) + (yB*dB) + (yC*dC);
xG = (zA*dA) + (zB*dB) + (zC*dC);

Basic, and not really smart but will do the job for some simple tasks.

EDIT

You can take any confidence coef you want in [0,inf[, but IMHO, restraining at [0,1] is a good idea to keep a realistic result.

来源：https://stackoverflow.com/questions/25852053/multiliteration-implementation-with-inaccurate-distance-data

标签

algorithm

localization

ibeacon

indoor-positioning-system

trilateration

Issues implementing the Fang Algorithm for TDOA Trilateration

家住魔仙堡 — Mon, 26 Oct 2020 11:46:30 +0000

Issues implementing the Fang Algorithm for TDOA Trilateration 家住魔仙堡 2020-10-26 19:46:30

来源：https://stackoverflow.com/questions/58228533/issues-implementing-the-fang-algorithm-for-tdoa-trilateration

标签

algebra

trilateration

Issues implementing the Fang Algorithm for TDOA Trilateration

元气小坏坏 — Mon, 26 Oct 2020 11:44:31 +0000

Issues implementing the Fang Algorithm for TDOA Trilateration 元气小坏坏 2020-10-26 19:44:31

来源：https://stackoverflow.com/questions/58228533/issues-implementing-the-fang-algorithm-for-tdoa-trilateration

标签

algebra

trilateration

Trilateration of a signal using Time Difference of Arrival

99封情书 — Wed, 01 Jan 2020 18:00:33 +0000

Trilateration of a signal using Time Difference of Arrival 99封情书 2020-01-02 02:00:33

问题

I am having some trouble to find or implement an algorithm to find a signal source. The objective of my work is to find the sound emitter position.

To accomplish this I am using three microfones. The technique that I am using is multilateration that is based on the time difference of arrival.

The time difference of arrival between each microfones are found using Cross Correlation of the received signals.

I already implemented the algorithm to find the time difference of arrival, but my problem is more on how multilateration works, it's unclear for me based on my reference, and I couldn't find any other good reference for this that are free/open.

If you have some references on how I can implement a multilateration algorithm, or some other trilateration algorithm that I can use based on time difference of arrival it would be a great help.

Thanks in advance.

回答1:

The point you are looking for is the intersection of three hyperbolas. I am assuming 2D here since you only use 3 receptors. Technically, you can find a unique 3D solution but as you likely have noise, I assume that if you wanted a 3D result, you would have taken 4 microphones (or more).

The wikipedia page makes some computations for you. They do it in 3D, you just have to set z = 0 and solve for system of equations (7).

The system is overdetermined, so you will want to solve it in the least squares sense (this is the point in using 3 receptors actually).

回答2:

I can help you with multi-lateration in general.

Basically, if you want a solution in 3d - you have to have at least 4 points and 4 distances from them (2-give you the circle in which is the solution - because that is the intersection between 2 spheres, 3 points give you 2 possible solutions (intersection between 3 spheres) - so, in order to have one solution - you need 4 spheres). So, when you have some points (4+) and the distance between them (there is an easy way to transform the TDOA into the set of equations for just having the length type distances /not time/) you need a way to solve the set of equations. First - you need a cost function (or solution error function, as I call it) which would be something like

err(x,y,z) = sum(i=1..n){sqrt[(x-xi)^2 + (y-yi)^2 + (z-zi)^2] - di}

where x, y, z are coordinates of the current point in the numerical solution and xi, yi, zi and di are the coordinates and distance towards the ith reference point. In order to solve this - my advice is NOT to use Newton/Gauss or Newton methods. You need first and second derivative of the aforementioned function - and those have a finite discontinuation in some points in space - hence that is not a smooth function and these methods won't work. What will work is direct search family of algorithms for optimization of functions (finding minimums and maximums. in our case - you need minimum of the error/cost function).

That should help anyone wanting to find a solution for similar problem.

来源：https://stackoverflow.com/questions/5929327/trilateration-of-a-signal-using-time-difference-of-arrival

标签

Multi-point trilateration algorithm in Java

不羁岁月 — Sat, 21 Dec 2019 01:28:18 +0000

Multi-point trilateration algorithm in Java 不羁岁月 2019-12-21 09:28:18

问题

I'm trying to implement a trilateration algorithm into my Android app to determine a user's indoor location. I'm using ultra-wideband beacons to get the distances to fixed points. I was able to adapt the method suggested in Trilateration Method Android Java as follows:

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);

}

Using this approach gives me a user location, but is not terribly accurate. How can I extend this to use more than 3 known locations/distances? Ideally N number of points where N>=3.

回答1:

When formulated in the correct manner, the multilateration problem is an optimization problem.

Most scholarly examples, like the one on wikipedia, deal with exactly three circles and assume perfectly accurate information. These circumstances allow for much simpler problem formulations with exact answers, and are usually not satisfactory for practical situations like the one you describe.

The problem in R² or R³ euclidean space with distances that contain measurement error, an area (ellipse) or volume (ellipsoid) of interest is usually obtained instead of a point. If a point estimate is desired instead of a region, the area centroid or volume centroid should be used. R² space requires at least 3 non-degenerate points and distances to obtain a unique region; and similarly R³ space requires at least 4 non-degenerate points and distances to obtain a unique region.

Here is a open source java library that will easily meet your needs: https://github.com/lemmingapex/Trilateration

It uses a popular nonlinear least squares optimizer, the Levenberg-Marquardt algorithm, from 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);

回答2:

I found this solution in an e-book;

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

I coded this into a Java example and it seems to work pretty well for 3 circles. However, I have no idea how to adapt this formula to cover trilateration with a 4th and 5th point in the solution. My maths is just not that good.

My code for the formula is here;

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);

}

I would ideally like a code solution that could work with 3+ nodes and also, where multiple points were used, would weight the solution more towards the point derived from nodes with small radius values.

Anyone got any ideas?

Either how to expand the book formula for 4+ nodes, or a better code implementation?

来源：https://stackoverflow.com/questions/30336278/multi-point-trilateration-algorithm-in-java

标签

java

android

gps

indoor-positioning-system

trilateration

How to improve accuracy of indoor positioning?

一曲冷凌霜 — Wed, 18 Dec 2019 02:30:27 +0000

How to improve accuracy of indoor positioning? 一曲冷凌霜 2019-12-18 10:30:27

问题

I should be developing an indoor positioning system for some place , so I started by gathering info about how to develop such a system , the question I am up to now is : what controls the accuracy of positioning and how to improve it ? I found different APIs and projects with various accuracies , for example : ericsson indoor positioning API provides an accuracy within 10 meters , while Qubulus API provides an accuracy within 4 meters , and I met some projects like iDOCNET which claims to provide an accuracy of about 1.2 meters. So, what is the main component of the indoor navigating systems that controls the accuracy ?

回答1:

I'm working on a similar project and I tested a couple of the existing tools.

You can find some interesting info about IPS accuracy/precision/resolution here:

Stackoverflow thread on IPS resolution

Read the answer from Carol Politi of TRX Systems, in particular.

In short, the precision depends mainly on the type and density of the radio beacons used as reference signals (that can be GSM/WDCMA/UMTS network cells, wi-fi access points, Bluetooth dongles/beacons, etc.). As a consequence, to improve your system's precision you have to use all of the existing/available radio sources (phone cells AND wi-fi access points) AND, maybe, you have to add/install your own reference points (most likely wi-fi routers).

As long as I was able to see from my experiments so far, the actual precision you can expect from such radio-based systems is as following.

Phone network: 1 to 2 Km
Wi-Fi: 10 to 150 m (most often 40 - 150 m)
Bluetooth: 4 to 10 m (maybe better with Nokia technology, that uses BT 3.0 and special beacons)

Using different technologies together just gives you the precision of the best one. For example, when you use cell phones AND wi-fi access points as reference points, you just get a resolution of 10 to 150 m. Nothing better.

For example/inspiration regarding wi-fi-fingerprint-based systems, look at: Redpin .

The only way to get room-level resolution, using ONLY radio signals (radio multilateration), seems to be Bluetooth. Nokia has developed something for this.

An effective way to improve the resolution of the whole system is to pair a radio-based positioning system (like wi-fi fingerprinting) with a map-based one (Google for "pathfinding": the same map navigation technology used in many 2D games).

This way, you enforce your whole system to pinpoint your user just where he/she can actually be (in a aisle, inside a room), escluding the not-walkable areas (like the inner part of a wall or a not-accessible part of the building). This makes your calculated navigation path much more sensible but in long corridors and aisles the resolution can still be quite bad (5 to 10 m or worse).

Another way is known as "sensor fusion": add to the radio-based system the position/movement knowledge that come from the accelerometer, compass and other sensors that are built-in in the user's mobile device.

Such hybrid systems are already available on the market and can give you a resolution up to 2 - 4 m (room-level, aisle-level) WITHOUT installing any auxiliary radio beacon (such systems are also known as "infrastructureless indoor positioning systems"). A few of these systems use a pathfinding algorithm as well.

For example/inspiration regarding hybrid systems, look at: Footpath .

For an even more inspiring project, see UnLoc by Duke University: UnLoc at Gizmag and UnLoc at Duke .

If you need an even better resolution, most likely you have to install your own Bluetooth beacons (and/or use Nokia technology).

来源：https://stackoverflow.com/questions/12098122/how-to-improve-accuracy-of-indoor-positioning

标签

wifi

trilateration

indoor-positioning-system

How to improve accuracy of indoor positioning?

牧云@^-^@ — Wed, 18 Dec 2019 02:30:05 +0000

How to improve accuracy of indoor positioning? 牧云@^-^@ 2019-12-18 10:30:05

问题

回答1:

I'm working on a similar project and I tested a couple of the existing tools.

You can find some interesting info about IPS accuracy/precision/resolution here:

Stackoverflow thread on IPS resolution

Read the answer from Carol Politi of TRX Systems, in particular.

As long as I was able to see from my experiments so far, the actual precision you can expect from such radio-based systems is as following.

Phone network: 1 to 2 Km
Wi-Fi: 10 to 150 m (most often 40 - 150 m)
Bluetooth: 4 to 10 m (maybe better with Nokia technology, that uses BT 3.0 and special beacons)

For example/inspiration regarding wi-fi-fingerprint-based systems, look at: Redpin .

The only way to get room-level resolution, using ONLY radio signals (radio multilateration), seems to be Bluetooth. Nokia has developed something for this.

For example/inspiration regarding hybrid systems, look at: Footpath .

For an even more inspiring project, see UnLoc by Duke University: UnLoc at Gizmag and UnLoc at Duke .

If you need an even better resolution, most likely you have to install your own Bluetooth beacons (and/or use Nokia technology).

来源：https://stackoverflow.com/questions/12098122/how-to-improve-accuracy-of-indoor-positioning

标签

wifi

trilateration

indoor-positioning-system

Trilateration Formula (Programming)

孤人 — Fri, 13 Dec 2019 09:30:45 +0000

Trilateration Formula (Programming) 孤人 2019-12-13 17:30:45

问题

I'm currently trying to develop a trilateration application to track beacons using 3 phones. I converted code I found in python over to c# but I'm having trouble getting it to work. This is my code:

public double[] getPosition(double phoneADistance, double phoneBDistance, double phoneCDistance)
    {
        //meterToFeet is just a conversion method which takes the distance parameter and multiplies it by 3.28.
        double PhoneADist = meterToFeet(phoneADistance);
        double PhoneBDist = meterToFeet(phoneBDistance);
        double PhoneCDist = meterToFeet(phoneCDistance);

        //The phone's x and y coordinates are pre-set
        Vector<double> P1 = new DenseVector(new[] { PhoneA_x, PhoneA_y });
        Vector<double> P2 = new DenseVector(new[] { PhoneB_x, PhoneB_y });
        Vector<double> P3 = new DenseVector(new[] { PhoneC_x, PhoneC_y });

        var ex = (P2 - P1) / (P2 - P1).L2Norm();
        var i = ex.DotProduct(P3 - P1);
        var ey = (P3 - P1 - i * ex) / (P3 - P1 - i * ex).L2Norm();
        var d = (P2 - P1).L2Norm();
        var j = ey.DotProduct(P3 - P1);

        var x = (Math.Pow(PhoneADist, 2) - Math.Pow(PhoneBDist, 2) + Math.Pow(d, 2)) / (2 * d);
        var y = ((Math.Pow(PhoneADist, 2) - Math.Pow(PhoneCDist, 2) + Math.Pow(i, 2) + Math.Pow(j, 2)) / (2 * j)) - ((i / j) * x);


        double[] answer = new double[] { x, y };
        Console.Write(x + " " + y);
        return answer;
    }

When I run this method

Test case #1:

PhoneA_x&y = (0,0)
PhoneB_x&y = (100,0)
PhoneC_x&y = (50,100)
phoneADistance = 0
phoneBDistance = 100
phoneCDistance = 111.803

it returns (-488.195, -366.147)

Test Case #2:

PhoneA_x&y = (0,0)
PhoneB_x&y = (100,0)
PhoneC_x&y = (50,100)
phoneADistance = 25
phoneBDistance = 25
phoneCDistance = 25

it returns (50, 37.5)

回答1:

It's very sloppy but this is my new algorithm so far.

I just took the excel spreadsheet from the site that Simon linked and converted into C# code.

There is still a lot of clean up to do.

It's still in testing process so I'm not 100 with the results, but from the testings I've done so far it seems pretty accurate.

    public double[] getPosition(double phoneADistance, double phoneBDistance, double phoneCDistance)
    {
        double[] answer = new double[] { 0, 0 };
        double PhoneADist = meterToFeet(phoneADistance);
        double PhoneBDist = meterToFeet(phoneBDistance);
        double PhoneCDist = meterToFeet(phoneCDistance);

        Vector<double> P1 = new DenseVector(new[] { PhoneA_x, PhoneA_y });
        Vector<double> P2 = new DenseVector(new[] { PhoneB_x, PhoneB_y });
        Vector<double> P3 = new DenseVector(new[] { PhoneC_x, PhoneC_y });

        //Translate values for the three points
        var B3 = PhoneA_x;
        var C3 = PhoneA_y;
        var D3 = phoneADistance;
        var B4 = PhoneB_x;
        var C4 = PhoneB_y;
        var D4 = phoneBDistance;
        var B5 = PhoneC_x;
        var C5 = PhoneC_y;
        var D5 = phoneCDistance;
        //Translate P1 to Origin
        var B8 = B3 - B3;
        var C8 = C3 - C3;
        var D8 = D3;
        var B9 = B4 - B3;
        var C9 = C4 - C3;
        var D9 = D4;
        var B10 = B5 - B3;
        var C10 = C5 - C3;
        var D10 = D5;
        //Find Calculation Values
        var B13 = Math.Atan2(C9, B9); ;
        var B14 = Math.Atan2(C10, B10);
        var B15 = Math.Sqrt(Math.Pow(B4 - B3, 2) + Math.Pow(C4 - C3, 2));
        var B16 = Math.Sqrt(Math.Pow(B5 - B3, 2) + Math.Pow(C5 - C3, 2));
        //Polar Coordinates for the Rotated System
        //var B20 = 0;
        //var C20 = 0;
        var D20 = D3;
        var B21 = B15;
        //var C21 = 0;
        var D21 = D4;
        var B22 = B16;
        var C22 = B14 - B13;
        var D22 = D5;
        //Rectangular Coordinates for the Rotated System
        //var B26 = 0;
        //var C26 = 0;
        var D26 = D3;
        var B27 = B21;
        //var C27 = 0;
        var D27 = D4;
        var B28 = B22 * Math.Cos(C22);
        var C28 = B22 * Math.Sin(C22);
        var D28 = D5;
        //Coordinates of Roated Solution
        var B31 = (Math.Pow(D3, 2) - Math.Pow(D4, 2) + Math.Pow(B27, 2)) / (B27 * 2);
        var B32 = Math.Sqrt(Math.Pow(D3, 2) - Math.Pow(B31, 2));
        var D32 = -B32;
        //Convert to Polar
        var B35 = Math.Sqrt(Math.Pow(B31, 2) + Math.Pow(B32, 2));
        var B36 = Math.Atan2(B32, B31);
        var D36 = Math.Atan2(D32, B31);
        //Unrotate
        var B39 = B35;
        var B40 = B36 + B13;
        var D40 = D36 + B13;
        //Rectangular Coordinates
        var B43 = B39 * Math.Cos(B40);
        var D43 = B39 * Math.Cos(D40);
        var B44 = B39 * Math.Sin(B40);
        var D44 = B39 * Math.Sin(D40);
        //Untranslate
        var B47 = B43 + B3;
        var D47 = D43 + B3;
        var B48 = B44 + C3;
        var D48 = D44 + C3;
        var x = B47;
        var y = B48;
        //Return Answer
        if (!Double.IsNaN(x) || !Double.IsNaN(y))
        {
            answer = new double[] { x, y };
            Console.Write(x + " " + y);
        }
        return answer;
    }

来源：https://stackoverflow.com/questions/41958970/trilateration-formula-programming

标签

algorithm

math

trilateration

Localizing a point using distances to three other points in 3-D

陌路散爱 — Wed, 11 Dec 2019 02:11:46 +0000

Localizing a point using distances to three other points in 3-D 陌路散爱 2019-12-11 10:11:46

问题

Assume that we have 4 points in 3-D (P1, P2, P3, P4). If the coordinates of those points are given with their euclidian distances to a fifth point P5 (r1, r2, r3, r4), how to calculate the coordinates of P5?

In this post, answer of Don Reba is perfect for 2-D. But how do I extend it to 3-D?

Here is my code for 2D:

    static void localize(double[] P1, double[] P2, double[] P3, double r1, double r2, double r3)
    {
        double[] ex = normalize(difference(P2, P1));
        double i = dotProduct(ex, difference(P3, P1));
        double[] ey = normalize(difference(difference(P3, P1), scalarProduct(i, ex)));
        double d = magnitude(difference(P2, P1));
        double j = dotProduct(ey, difference(P3, P1));
        double x = ((r1*r1) - (r2*r2) + (d*d)) / (2*d);
        double y = (((r1*r1) - (r3*r3) + (i*i) + (j*j)) / (2*j)) - ((i*x) / j);
        System.out.println(x + " " + y);

    }

I want to overload the function with the signature

static void localize(double[] P1, double[] P2, double[] P3, double[] P4, double r1, double r2, double r3, double r4)

回答1:

The Wikipedia trilateriation article describes the answer. The calculation steps are:

e_x = (P2 - P1) / ‖P2 - P1‖
i = e_x(P3 - P1)
e_y = (P3 - P1 - i · e_x) / ‖P3 - P1 - i · e_x‖
d = ‖P2 - P1‖
j = e_y(P3 - P1)
x = (r₁² - r₂² + d²) / 2d
y = (r₁² - r₃² + i² + j²) / 2j - ix / j
z = ±sqrt(r₁² - x² - y²)

回答2:

You need to solve system of four equations (i=1..4, Di is distance to ith point)

(X-Xi)^2+(Y-Yi)^2+(Z-Zi)^2=Di^2

It is possible to solve system of three equations and use fourth to choose proper solution (from two).

This is how GPS works (where time delays are for distances).

In GPS receivers optimization methods are frequently used, especially when many satellites are available and algebraic solution may be instable.

来源：https://stackoverflow.com/questions/23400351/localizing-a-point-using-distances-to-three-other-points-in-3-d

标签

math

geometry

trilateration

Trilateration of a signal using Time Difference(TDOA)

烈酒焚心 — Sat, 07 Dec 2019 19:02:20 +0000

Trilateration of a signal using Time Difference(TDOA) 烈酒焚心 2019-12-08 03:02:20

问题

I am having some trouble to find or implement an algorithm to find a signal source. The objective of my work is to find the sound emitter position.

To accomplish this I am using three vibration sensors. The technique that I am using is multilateration that is based on the time difference of arrival.

The time difference of arrival between each sensor are found using Cross Correlation of the received signals.

I already implemented the algorithm to find the time difference of arrival, but my problem is more on how multilateration works, it's unclear for me based on my reference, and I couldn't find any other good reference for this that are free/open.

I saw this post Trilateration using TDOA But I can't figure out how to solve the set of equations(7) of the wikipedia page of multilateration as i have only the three TDOA.

Any help on this would be much appreciated

回答1:

You have three sensor coordinates A,B,C, unknown coordinate of signal source P, unknown time of signal start t0, and three times of signal registration ta, tb, tc.

Example: Let's sensor A caught a signal in 12:00:05, sensor B - in 12:00:00, sensor C - 12:00:07. So assign time differences: ta=5, tb=0, tc=7

Squared distances from sensors to source correspond to times of signal walk with velocity v (speed of sound in air or another environment)

(Ax-Px)^2 + (Ay-Py)^2 = (v*(ta-t0))^2   {1}
(Bx-Px)^2 + (By-Py)^2 = (v*(tb-t0))^2   {2}
(Cx-Px)^2 + (Cy-Py)^2 = (v*(tc-t0))^2   {3}

Open the brackets, then subtract equations ({2}-{1}, {3}-{2},{1}-{3}) to discard squares of unknown terms.

 Ax^2-2*Ax*Px + Px^2 + Ay^2-2*Ay*Py + Py^2 = v^2*(ta^2 - 2*ta*t0 + t0^2)
 Bx^2-2*Bx*Px + Px^2 + By^2-2*By*Py + Py^2 = v^2*(tb^2 - 2*tb*t0 + t0^2)
 Cx^2-2*Cx*Px + Px^2 + Cy^2-2*Cy*Py + Py^2 = v^2*(tc^2 - 2*tc*t0 + t0^2)

 Bx^2-Ax^2 -2*(Bx-Ax)*Px + By^2-Ay^2 -2*(By-Ay)*Py = v^2*(tb^2-ta^2 -2*(tb-ta)*t0)    {1'}
 Cx^2-Bx^2 -2*(Cx-Bx)*Px + Cy^2-By^2 -2*(Cy-By)*Py = v^2*(tc^2-tb^2 -2*(tc-tb)*t0)    {2'}
 Ax^2-Cx^2 -2*(Ax-Cx)*Px + Ay^2-Cy^2 -2*(Ay-Cy)*Py = v^2*(ta^2-tc^2 -2*(ta-tc)*t0)    {3'}

Now you have system of three linear equations with three unknowns. It might be solved with some widespread algorithms - Gauss elimination, LU decomposition etc.

Note that solution precision strongly depends on small errors in coordinates and time measurements (this method is not very robust).

回答2:

Geometrically, a hyperbola represents the cloud of points with a constant difference in distance between two points. You have 3 points, but taken pairwise, the time differences between the 3 possible pairs will allow you to draw 3 hyperbolas. Look for a spot at or between where the hyperbolas intersect on a plot. Or solve the equivalent algebra (least squares).

来源：https://stackoverflow.com/questions/36176167/trilateration-of-a-signal-using-time-differencetdoa

标签