Init from normal Multilateration repo: https://github.com/AlexisTM/Mu…

…ltilateration

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

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']

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)

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)

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)

setup.py

@@ -0,0 +1,15 @@
+import setuptools
+from distutils.core import setup
+
+setup(
+    name='MultilaterationTDOA',
+    version='0.2.0',
+    author='AlexisTM',
+    author_email='[email protected]',
+    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(),
+)

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