From a9e059d6121c719cb6f7fd8278483fbb6beb03d5 Mon Sep 17 00:00:00 2001 From: Alexis Paques Date: Mon, 6 May 2019 15:28:05 +0200 Subject: [PATCH] Init from normal Multilateration repo: https://github.com/AlexisTM/Multilateration --- README.md | 13 +++- multilateration_tdoa/__init__.py | 10 +++ multilateration_tdoa/engine.py | 86 ++++++++++++++++++++++++++ multilateration_tdoa/geometry.py | 103 +++++++++++++++++++++++++++++++ multilateration_tdoa/methods.py | 75 ++++++++++++++++++++++ setup.py | 15 +++++ test.py | 36 +++++++++++ 7 files changed, 337 insertions(+), 1 deletion(-) create mode 100644 multilateration_tdoa/__init__.py create mode 100644 multilateration_tdoa/engine.py create mode 100644 multilateration_tdoa/geometry.py create mode 100644 multilateration_tdoa/methods.py create mode 100644 setup.py create mode 100644 test.py diff --git a/README.md b/README.md index 275d3fa..2497cef 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,13 @@ # MultilaterationTDOA -Multilateration from TDoA measurements. +This library will be used for TDoA localization from a [Bitcraze loco positioning system](https://www.bitcraze.io/loco-pos-system/). + +This library fits your use-case if: + +* **Setup**: + * Multiple fixd anchors doing TWR between them + * Tags receiving data from the anchors and computing the Time Difference on Arrival of messages between the messages of Anchors +* **Measurements**: + * Position anchor 1 (m) + * Position anchor 2 (m) + * TDoA (m, you can take the time divided by the speed of light for the conversion) + diff --git a/multilateration_tdoa/__init__.py b/multilateration_tdoa/__init__.py new file mode 100644 index 0000000..69986d9 --- /dev/null +++ b/multilateration_tdoa/__init__.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from .engine import Engine, Anchor +from .geometry import Point, Circle +from .methods import LSEMethod + +__all__ = ['Engine', 'Anchor', + 'Point', 'Circle', + 'LSEMethod'] diff --git a/multilateration_tdoa/engine.py b/multilateration_tdoa/engine.py new file mode 100644 index 0000000..ee4a508 --- /dev/null +++ b/multilateration_tdoa/engine.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import numpy as np +from scipy.optimize import minimize +from .geometry import Point, Circle +from .methods import LSEMethod +from time import time + + +class Anchor(object): + def __init__(self, ID, position, measure = None): + self.position = position + self.ID = str(ID) + self.last_seen = time() + self.measure = measure + + @property + def position(self): + return self._position + + @position.setter + def position(self, position): + self._position = Point(position) + + def add_measure(self, data): + self.measure = data + self.last_seen = time() + + def valid(self, now = None, timeout = 0.5): + if now is None: + now = time() + if self.measure is None: + print("No measure yet... " + str(self)) + return False + if now - self.last_seen > timeout: + print("Last seen is too great! " + str(self.last_seen)) + return False + return True + + def get(self): + return Circle(self.position, self.measure) + + def __repr__(self): + return self.__str__() + + def __str__(self): + return "".join(['Anchor:', self.ID, '@', self.position.__str__()]) + + +class Engine(object): + def __init__(self, goal=[None, None, None], timeout=0.1): + self.anchors = {} + self.method = LSEMethod(goal) + self.timeout = timeout + + def add_anchor(self, ID, position): + """Add a certain ID""" + if ID in self.anchors: + self.anchors[ID].position = position + else: + self.anchors[ID] = Anchor(ID, position) + + def add_measure_id(self, ID, measure): + """Add a measurement for a certain anchor ID""" + if ID in self.anchors: + self.anchors[ID].add_measure(measure) + else: + print("anchor " + str(ID) + " does not exist yet") + + def add_measure(self, position, measure, ID=None): + """Distance measurement from an anchor position""" + if ID is None: + ID = str(position) + + if ID not in self.anchors: + self.anchors[ID] = Anchor(ID, position, measure) + else: + self.anchors[ID] = measure + + def solve(self): + cA = [] + for ID, anchor in self.anchors.items(): + if anchor.valid(self.timeout): + cA.append(anchor.get()) + return self.method.solve(cA) diff --git a/multilateration_tdoa/geometry.py b/multilateration_tdoa/geometry.py new file mode 100644 index 0000000..ffa0bd8 --- /dev/null +++ b/multilateration_tdoa/geometry.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from __future__ import division +from math import pi, cos, sin, atan, acos +import numpy as np + +class Circle(object): + def __init__(self, p, r): + self.c = p + self.r = float(r) + + def __repr__(self): + return self.__str__() + + def __str__(self): + return "".join(['Circle[' + self.c.__str__() + ':' + str(self.r) + ']']) + + def __eq__(self, other): + return self.c == other.c and self.r == other.r + +class Point(object): + def __init__(self, x, y=0, z=0): + if(type(x) in [list, tuple, np.ndarray]): + self.x = x[0] if len(x) >= 1 else 0.0 + self.y = x[1] if len(x) >= 2 else 0.0 + self.z = x[2] if len(x) >= 3 else 0.0 + else: + self.x = x + self.y = y + self.z = z + + def __str__(self): + return "".join(['Point(' + str(self.x) + ',' + str(self.y) + ',' + str(self.z) + ')']) + + def __eq__(self, other): + return self.__dict__ == other.__dict__ + + def __sub__(self, other): + if isinstance(other, Point): + tx = self.x - other.x + ty = self.y - other.y + tz = self.z - other.z + else: + tx = self.x - other.dx + ty = self.y - other.dy + tz = self.z - other.dz + return Point(tx, ty, tz) + + def __add__(self, other): + if isinstance(other, Point): + tx = self.x + other.x + ty = self.y + other.y + tz = self.z + other.z + else: + tx = self.x + other.dx + ty = self.y + other.dy + tz = self.z + other.dz + return Point(tx, ty, tz) + + def __mul__(self, other): + return Point(other * self.x, other * self.y, other * self.z) + + def __rmul__(self, other): + return Point(other * self.x, other * self.y, other * self.z) + + def __div__(self, other): + return Point(self.x / other, self.y / other, self.z / other) + + def __neg__(self): + x = -self.x + y = -self.y + z = -self.z + return Point(x, y, z) + + def __getitem__(self, id): + return [self.x, self.y, self.z][id] + + def area(self): + return 0.0 + + def dist(self, other): + return ((self[0] - other[0]) ** 2 + (self[1] - other[1]) ** 2 + (self[2] - other[2]) ** 2) ** 0.5 + + def std(self): + return [self.x, self.y, self.z] + + def c2s(self): + R = self.dist(Point(0, 0, 0)) + lg = atan(self.y / self.x) + lat = acos(self.z / R) + return (lg, lat, R) + + def transform(self, p, rot): + px = cos(rot) * self.x + sin(rot) * self.y + py = -sin(rot) * self.x + cos(rot) * self.y + p_t = Point(px, py) + return p_t - p + + def rot(self, a): + px = cos(a) * self.x - sin(a) * self.y + py = sin(a) * self.x + cos(a) * self.y + return Point(px, py) diff --git a/multilateration_tdoa/methods.py b/multilateration_tdoa/methods.py new file mode 100644 index 0000000..96c231c --- /dev/null +++ b/multilateration_tdoa/methods.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import numpy as np +from scipy.optimize import minimize +from .geometry import Point + +from sys import version_info +if (version_info > (3, 0)): + xrange = range + + +class Method(object): + def __init__(self, goal): + self.goal = goal + self.last_result = Point(0,0,0) + + def update(self, cA): + raise NotImplementedError + + +class LSEMethod(Method): + def __init__(self, goal): + Method.__init__(self, goal) + + def cost_function(self, x, c, r): + e = 0 + # Other ways to converge to a certain axis value + # Force an axis + # print x + # # This force an axis value + # for i, value in enumerate(goal): + # if value is not None: + # x[i] = value + + # current = [0,0,0] + # for i, value in enumerate(goal): + # if value is not None: + # current[i] = value + # else: + # current[i] = x[i] + + for i in xrange(len(c)): + e += (c[i].dist(x)- r[i]) ** 2 + return e + + def solve(self, cA): + # cA is a cicle array [Circle(), Circle()] representing measurements + # l = number of circles + l = len(cA) + # r = radiuses of the circles (distance measured) + r = [w.r for w in cA] + # c = Point(), center of the circles (anchor position) + c = [w.c for w in cA] + # S = the sum of all radiuses + S = sum(r) + # W = Normalized 1/distances [(Sum - distance) / (Nmeasures-1)*Sum] + W = [(S - w) / ((l - 1) * S) for w in r] + p0 = self.last_result # Initialized Point + for i in range(l): + # p0 += Normalized distance * centers + p0 = p0 + W[i] * c[i] + x0 = np.array([p0.x, p0.y, p0.z]) + # self.cost_function = the function to be minimized + # x0 = the initial estimation that gets iterated + # Extra arguments: + # c = Point(), anchor positions + # r = all measurements + for i, value in enumerate(self.goal): + if value is not None: + x0[i] = value + + result = minimize(self.cost_function, x0, args=(c, r), method='BFGS') + ans = list(result.x) + return Point(ans) diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..0f64a82 --- /dev/null +++ b/setup.py @@ -0,0 +1,15 @@ +import setuptools +from distutils.core import setup + +setup( + name='MultilaterationTDOA', + version='0.2.0', + author='AlexisTM', + author_email='alexis.paques@gmail.com', + packages=['multilateration_tdoa'], + scripts=[], + url='https://github.com/AlexisTM/MultilaterationTDOA', + license='LICENSE.txt', + description='Multilateration library for TDoA localization.', + long_description=open('README.md').read(), +) diff --git a/test.py b/test.py new file mode 100644 index 0000000..37e6d91 --- /dev/null +++ b/test.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from multilateration_tdoa import Engine +from math import sqrt + +engine = Engine(goal=[None, None, None]) # To fix that the resulting height should be 2 meters +# P=multilateration.Project() # for 3D, simply do not set a goal +engine.add_anchor('anchor_A',(1,0,1)) +engine.add_anchor('anchor_B',(-1,0,1)) +engine.add_anchor('anchor_C',(1,1,1)) +engine.add_measure_id('anchor_A',sqrt(2)) +engine.add_measure_id('anchor_B',sqrt(2)) +engine.add_measure_id('anchor_C',sqrt(3)) +print(engine.solve()) + +# Or using anchors linked to measurements directly +engine2 = Engine(goal=[None, None, 2.0]) +# If the ID is not given, it will be generated to str(position_of_anchor) +engine2.add_measure((1,0,1), sqrt(2), ID="anchor_A") +engine2.add_measure((-1,0,1), sqrt(2), ID="anchor_B") +engine2.add_measure((1,1,1), sqrt(3)) + +print(engine2.solve()) + +engine3 = Engine(goal=[None, None, None]) # To fix that the resulting height should be 2 meters +# P=multilateration.Project() # for 3D, simply do not set a goal +engine3.add_anchor('anchor_A',(1,0,1)) +engine3.add_anchor('anchor_B',(-1,0,1)) +engine3.add_anchor('anchor_C',(1,1,1)) +engine3.add_anchor('anchor_D',(-1,-1,-1)) +engine3.add_measure_id('anchor_A',sqrt(2)) +engine3.add_measure_id('anchor_B',sqrt(2)) +engine3.add_measure_id('anchor_C',sqrt(3)) +engine3.add_measure_id('anchor_D',sqrt(3)) +print(engine3.solve())