Triangulation algorithm on grid with signal strength C#

Question

(The small dots in the corners are the nodes and the red dot is the person being tracked)

Coordinates:

Node   X    Y   Position
1      0    0   Top left
2    450    0   Top right
3      0  450   Bottom left
4    450  450   Bottom right

Person    X    Y
Red dot  84   68

Method to get signal strength:

(Just need the signal strength relative to other nodes, which it seem to achieve. Or am I wrong here?)

public int GetSignalStrength(OvalShape node)
{
    int xd = node.Left - this.person.Left;
    int yd = node.Top - this.person.Top;

    var signalStrength = Math.Sqrt((xd * xd) + (yd * yd));

    return Convert.ToInt32(-signalStrength);
}

Signal strengths:

Node   Signal Strength
1                 -108
2                 -372
3                 -391
4                 -529

Method to get coordinates of person:

(s1, s2, s3, s4 are the signal strengths above)

public int[] GetPositionInGrid(int s1, int s2, int s3, int s4)
{
    var tx1 = this.node1.Left;
    var ty1 = this.node1.Top;

    var tx2 = this.node2.Left;
    var ty2 = this.node2.Top;

    var tx3 = this.node3.Left;
    var ty3 = this.node3.Top;

    var tx4 = this.node4.Left;
    var ty4 = this.node4.Top;

    double w1 = ((double)s1) / ((double)(s1 + s2 + s3 + s4));
    double w2 = ((double)s2) / ((double)(s1 + s2 + s3 + s4));
    double w3 = ((double)s3) / ((double)(s1 + s2 + s3 + s4));
    double w4 = ((double)s4) / ((double)(s1 + s2 + s3 + s4));

    var px = ((tx1 * w1) + (tx2 * w2) + (tx3 * w3) + (tx4 * w4)) / (w1 + w2 + w3 + w4);
    var py = ((ty1 * w1) + (ty2 * w2) + (ty3 * w3) + (ty4 * w4)) / (w1 + w2 + w3 + w4);

    return new int[] { Convert.ToInt32(px), Convert.ToInt32(py) };
}

Person position:

x: 290
y: 296

As you can see I'm not that good at math and the "Person position" is way off. Not that it matters but it works if the person is in the middle of the grid.

I'm working with the assumption that if every node has the same signal strength the person is in the middle of the grid.

Can someone please help me with this? Been googling and bashing my head against the table for awhile now.

Answer 1

I think you got the term wrong, you should be searching for Trilateration. You can check out this good explanation of an algorithm.

Answer 2

You should actually only need 3 nodes to do this.

The basic concept here is that each signal strength tells you distance from the node. With no other information, you can construct a semicircle from each node with radius equal to the signal strength. Certainly, the person must lie somewhere on the semicircle.

So, with one node, we construct a semicircle and it results in an infinite number of points where the person could be.

With two nodes, we find that the two semicircles may intersect in as many as two locations. In fact, the two opposite nodes will intersect at two distinct points within the window's boundaries if the person is not in the exact center, but will intersect at only one point (the center) if the person is at the center of the screen.

With the introduction of the third node, the third semicircle is guaranteed to intersect the first two semicircles at one of the two points at which they intersect.

The location where these three nodes intersect is where the person resides.

As stated by the_lotus, this is a Trilateration problem.

Here's the function, you need (you can even cut s4 from the parameter list):

public int[] GetPositionInGrid(int s1, int s2, int s3, int s4)
{
  var px = ((s1 * s1) 
            - (s2 * s2) 
            + (this.node2.Left * this.node2.Left)) 
           / ((double)(2 * this.node2.Left));

  var py = ((s1 * s1) 
            - (s3 * s3) 
            + (this.node3.Left * this.node3.Left) 
            + (this.node3.Top * this.node3.Top)) 
           / (2 * this.node3.Top) 
           - (this.node3.Left / (double)this.node3.Top) 
           * px;

  return new int[] { Convert.ToInt32(px), Convert.ToInt32(py) };
}