In [1]:
from unetpy import *
from sympy import *
In [2]:
%run utility.py
In [3]:
# List of IP addresses of the depolyed modems
ip_address = ['localhost', 'localhost']
In [4]:
# Connection to Node 1
node1gw = UnetGateway(ip_address[0], 1101)
node1 = node1gw.agentForService(Services.NODE_INFO)
In [5]:
node1
Out[5]:
In [6]:
node1.location
Out[6]:
In [7]:
# Connection to Node 2
node2gw = UnetGateway(ip_address[1], 1102)
node2 = node2gw.agentForService(Services.NODE_INFO)
In [8]:
node2
Out[8]:
In [9]:
node2.location
Out[9]:
Now, that the network simulator is setup with two known locations of the modem in the local coordinates system, we set out to compute the GPS location of the thrid node. We need to measure the distance from the two known locations to the modem we are trying to locate. This can be achived using ranging funtionality in UnetStack.
In [10]:
# List of locations
locations = [node1.location, node2.location]
# The GPS location which is set as origin
gps_origin = (43.933887, 15.443404)
# gps_origin = tuple(node1.origin)
# GPS location of the node we are trying to localize
ground_truth = (43.933636, 15.443643)
In [11]:
plot(locations, gps_origin, ground_truth)
Out[11]:
In [12]:
ranging_node1 = node1gw.agentForService(Services.RANGING)
node1gw.subscribe(ranging_node1)
In [13]:
ranging_node1 << org_arl_unet_phy.RangeReq(to=3)
Out[13]:
In [14]:
rnf1 = node1gw.receive(RangeNtf, 5000)
if rnf1 is not None:
range1 = rnf1.getRange()
print('The range from Node 1 is : ' + str(range1) + ' m.')
else:
print('Range not measured, try again.')
In [15]:
flush_modem(node1gw)
In [16]:
ranging_node2 = node2gw.agentForService(Services.RANGING)
node2gw.subscribe(ranging_node2)
In [17]:
ranging_node2 << org_arl_unet_phy.RangeReq(to=3)
Out[17]:
In [18]:
rnf2 = node2gw.receive(RangeNtf, 5000)
if rnf2 is not None:
range2 = rnf2.getRange()
print('The range from Node 2 is : ' + str(range2) + ' m.')
else:
print('Range not measured, try again.')
In [19]:
flush_modem(node2gw)
The distances are measured using acoustic ranging as shown above.
The Geometric circle intersection method is widely used in literature. Conceptually, the idea is straightforward. Referring to the figure above, there are two circles that can be formed with the reference modems at the center. There are only two possible location where these circles can intersect. As noted before, a third reference modem can remove this ambiguity, however, in the absence of the third reference modem, the two possible locations are shown with red marker in the figure at the two intersection points of the circle. With the assumption that the unknown node is deployed only on one side of the XY plane, the position of the target node can be computed uniquely.
Let us denote the unknown/target node’s (Node 3) location as $(x_1, x_2)$. The known position of the Node 1 is $(a_1, a_2)$ and Node 2 is $(b_1, b_2)$. The measured distances to Node 3 from Node 1 and Node 2 are denoted by $r_1$ and $r_2$ respectively.
Given the above information, two second order equations in two variables $x_1$ and $x_2$ can be written as follows:
$(x_1 - a_1)^2 + (x_2 - a_2)^2 - r_1^2 = 0$
$(x_1 - b_1)^2 + (x_2 - b_2)^2 - r_2^2 = 0$
We use the symbolic manipulation toolbox sympy in python to compute the analytic expression for computing $(x_1, x_2)$. The details are as given below:
In [20]:
x1, x2, a1, a2, b1, b2, r1, r2 = symbols('x1 x2 a1 a2 b1 b2 r1 r2')
Write the variable $x_1$ in terms of all other known parameters and variable $x_2$
In [21]:
x1 = ((a1**2 - b1**2) + (a2**2 - b2**2) - (r1**2 - r2**2) -2*x2*(a2-b2))/(2*(a1-b1))
Compute the expression for $x_2$ symbolically
In [22]:
expr = solveset(Eq((x1 - a1)**2 + (x2 - a2)**2 - r1**2, 0), x2)
Now we know the expression for $x_2$ in terms of all known parameters. Therefore, it's value can be computed using substitution as shown below:
In [23]:
x2_sol = list(expr.subs([(a1, node1.location[0]), (a2, node1.location[1]), \
(b1, node2.location[0]), (b2, node2.location[1]), \
(r1, range1), (r2, range2)]).evalf())
There are two possible solutions. The decision is based on the value of $x_1$, since our modem which is being localized lies on the right side of the y-axis, we take the decision based on the sign of $x_1$.
In [24]:
x2_sol
Out[24]:
The value of $x_1$ is computed for both values of computed $x_2$.
In [25]:
x1_sol = []
x1_sol.append( x1.subs([(a1, node1.location[0]), (a2, node1.location[1]), \
(b1, node2.location[0]), (b2, node2.location[1]), \
(r1, range1), (r2, range2), \
(x2, x2_sol[0])]).evalf() )
x1_sol.append( x1.subs([(a1, node1.location[0]), (a2, node1.location[1]), \
(b1, node2.location[0]), (b2, node2.location[1]), \
(r1, range1), (r2, range2), \
(x2, x2_sol[1])]).evalf() )
In [26]:
x1_sol
Out[26]:
In [27]:
locations.append([x1_sol[0], x2_sol[0], 0.0])
locations.append([x1_sol[1], x2_sol[1], 0.0])
In [28]:
plot(locations, gps_origin, ground_truth)
Out[28]:
The map is updated with the localized node's location with a red circle as shown in the figure above.