3 Use this class to perform indoor positioning with the metraTec IPS+ system. The position of a receiver is evaluated by 4 UWB ranging of RF beacons placed in the environment. Requires a YAML file with the configuration of beacon positions. 7 Initialize the class by passing the directory of the config file. Then collect a number of beacon responses to ranging 8 requests (SRG) and use the parse_srg() function to get a list of beacon objects and their respective ranges from a list 10 Use this list as input for the trilaterate() function to estimate the position of the receiver in 3D space. 12 A few examples can be found in the main function below. 17 from scipy.optimize
import minimize
18 from shapely.geometry
import Point
19 from shapely.geometry.polygon
import Polygon
20 from positioning
import Positioning
24 """Class to extend the functionality of the Positioning class with UWB ranging capabilities.""" 27 minimization_params = dict(method=
'TNC',
28 options={
'disp':
True})
30 def __init__(self, config_dir, min_beacons=4, max_z=None, dilation=0.0):
32 Initialize the class with zone and beacon definitions from YAML configuration file. 33 :param config_dir: String: Directory of the YAML config file 34 :param min_beacons: Int: Minimum number of beacons to use for trilateration 35 :param max_z: Float: Maximum z position the receiver can have after ranging 38 Positioning.__init__(self, config_dir)
44 self.
eids = self.get_defined_beacons()
54 Get the top n beacons in terms of RSSI values from a list of beacon pings. 55 :param pings: [String]: list of beacon pings 56 :param n: Int: Number of top beacons to return 57 :return: [Beacon]: list of beacon objects ordered according to RSSI values 59 means = self.get_mean(pings)
63 for key, value
in sorted(means.items(), key=
lambda (k, v): v, reverse=
True):
67 if value < z.threshold:
82 Parse a list of SRG ranging responses of UWB beacons. For each message (if it is valid) return the positions, 83 frame id and estimated ranges. 84 :param responses: [String]: list of SRG responses from UWB beacons 85 :return: [(Beacon, Float)]: list beacon-object and range pairs 94 print(
'Encountered invalid SRG response: {}'.format(r))
97 beacon = self.get_beacon(split[1])
99 range = float(split[2]) / 1000.
101 ranges.append((beacon, range))
106 Get a list of beacons that are in range and are configured in the configuration YAML file from a list of 107 beacon pings recorded over a short duration. The returned beacons (EIDs) can be used for UWB ranging 108 :param pings: [String]: List of BCN pings 109 :param threshold: Int: Minimum RSSI value a beacon should have to be returned. 110 :return: [String]: List of EIDs that can be used for ranging 114 mean = self.get_mean(pings)
116 srtd = sorted(mean.items(), key=operator.itemgetter(1), reverse=
True)
119 if threshold
is not None and s[1] < threshold:
122 if s[0]
in self.
eids:
128 Estimate the position of the receiver by using trilateration with at least three UWB responses 129 :param ranges: [([Float, Float, Float], Float)]: list of position and range pairs (obtained from parse_srg) or 130 [(Beacon, Float)]: list of beacon object and range pairs (output from parse_srg()) 131 :return: [Float, Float, Float]: 3D coordinates of estimated receiver location 135 print(
'Not enough beacons for position estimation ({}/{})'.format(len(ranges), self.
min_beacons))
137 print(
'Using {} beacons for trilateration.'.format(len(ranges)))
139 points, poly_points, distances, dsum = [], [], [], 0
142 p = r[0].position
if r[0].__class__.__name__ ==
'Beacon' else r[0]
144 poly_points.append((p[0], p[1]))
145 distances.append(r[1])
149 for p, d
in zip(points, distances):
150 weight = 1. - d / dsum
151 initial = [initial[0] + p[0] * weight, initial[1] + p[1] * weight, initial[2] + p[2] * weight]
152 initial = [initial[0] / len(ranges), initial[1] / len(ranges), initial[2] / len(ranges)]
153 if self.
max_z is not None and initial[2] > self.
max_z:
154 initial[2] = self.
max_z 156 print(
'guess: ', initial)
158 bnds = ((
None,
None), (
None,
None), (
None, self.
max_z))
if self.
max_z is not None else None 160 print(
'final: ', res.x)
162 poly = Polygon(poly_points)
163 return poly.convex_hull.buffer(self.
dilation).contains(Point(res.x[0], res.x[1])), res.x
166 def mse(x, points, distances):
168 Cost-function to use for position estimation. Minimize the squared error of the distance between a variable 169 point x and each beacon and the measured UWB distance. mse = SUM(distance - dist(x, points)^2 170 :param x: [Float, Float, Float]: vector with variable components to be optimized 171 :param points: [[x, y, z], ...]: list of positions of the beacons used for UWB ranging 172 :param distances: [Float]: estimated distances from UWB ranging of above beacons 173 :return: Float: mean squared error for all beacon positions 176 for p, d
in zip(points, distances):
177 d_calc = math.sqrt((x[0]-p[0])**2 + (x[1]-p[1])**2 + (x[2]-p[2])**2)
182 if __name__ ==
'__main__':
183 """Testing the class and its methods.""" 185 pos =
PositioningPlus(
'/home/metratec/catkin_ws/src/indoor_positioning/config/zones.yml')
187 dummy_pings = [
'BCN 00124B00090593E6 -090',
'BCN 00124B00090593E6 -090',
'BCN 00124B00090593E6 -090',
188 'BCN 00124B00050CD41E -090',
'BCN 00124B00050CD41E -070',
'BCN 00124B00050CD41E -050',
189 'BCN 00124B00050CDC0A -070',
'BCN 00124B00050CDC0A -060',
'BCN 00124B00050CDC0A -090']
192 beacon = pos.get_beacon(
'00124B00090593E6')
194 zone = pos.get_zone(dummy_pings)
197 resp = [
'SRG 00124B00050CDA71 01650 -076 -081 047\r',
'SRG 00124B00090593E6 04300 -076 -080 105\r',
198 'SRG 00124B00050CD41E 00800 -079 -082 049\r']
200 in_range = pos.in_range(dummy_pings)
202 ranges = pos.parse_srg(in_range)
204 tril = pos.trilaterate(ranges)
def mse(x, points, distances)
def get_top_beacons(self, pings, n)
def in_range(self, pings, threshold=None)
def get_range(self, responses)
def __init__(self, config_dir, min_beacons=4, max_z=None, dilation=0.0)
def parse_srg(self, responses)
def trilaterate(self, ranges)