Added N-Lateration function and factory dataset

Anthex
1 parent 5d3499c7
Showing 2 changed files with 46 additions and 8 deletions Show diff stats
main.py
structure.py
-from structure import RSSVector, Location, Cell, newCell, KNeighbors, resolve_barycenter, MarkovModel
+from structure import RSSVector, Location, Cell, newCell, KNeighbors, resolve_barycenter, MarkovModel, NLateration
 from random import random
 from math import floor
 Tf = [] #cells table
+#dataset : list of tuples representing emitters (Location, distance)
+dataset = [(Location(.5,.5,.5), 3.0), (Location(4.0,.0,.0), 2.0), (Location(4.0,5.0,5.0), 4.2), (Location(3.0,3.0,3.0), 2.5)]
+
 testSample = RSSVector(-26, -42, -13, -46)
 #known fingerprints
@@ -12,16 +15,22 @@ Tf = [[newCell(-38,-27,-54,-13,2,2),newCell(-74,-62,-48,-33,2,6),newCell(-13,-28
       [newCell(-17,-50,-44,-33,10,2), newCell(-27,-28,-32,-45,10,6), newCell(-30,-20,-60,-40,10,10)]]
 def main():
+        #### N-Lateration ####
+        NLat_result = NLateration(dataset)
+        print("\r\nN-Lateration :\nComputed location : " + NLat_result[0].toString())
+        print("With distance = " + str(round(NLat_result[1], 2)) + " m")
+
         #### K neighbours ####
-        print("\nk neighbors of test sample : ")
+        print("\nK-neighbors of test sample : ")
         neighborsCells = KNeighbors(Tf, testSample)
         for k in neighborsCells:
                 print("(", k.location.x, ";", k.location.y, ")")
         #### Distances ####
-        print ("\ndistances : " + str(testSample.distances))
+        print ("\nDistances : " + str(testSample.distances))
         #### Barycenter ####
+        print("\r\nWeighted barycenter :")
         a = resolve_barycenter(neighborsCells, testSample.distances)
         print(a.toString())
 from operator import itemgetter as ig
-from math import floor
-
+from math import floor, sqrt
+from numpy import arange
 class RSSVector():
     distances = []
     def __init__(self, n1, n2, n3, n4):
@@ -20,7 +20,7 @@ class RSSVector():
         and  v2.n3 == self.n3 and  v2.n4 == self.n4 else False
 class Location():
-    def __init__(self, x, y, z=0):
+    def __init__(self, x=0, y=0, z=0):
         self.x = x
         self.y = y
         self.z = z
@@ -28,7 +28,6 @@ class Location():
     def __eq__(self, loc2):
         return bool(self.x == loc2.x and self.y == loc2.y and self.z == loc2.z)
-
     def __mul__(self, multiplier):
         returnValue = Location(self.x, self.y, self.z)
         returnValue.x *= multiplier 
@@ -59,6 +58,9 @@ class Location():
     def toString(self):
         return "(" + str(self.x) + " ; " + str(self.y) + " ; " + str(self.z) + ")"
+    def distanceTo(self, loc):
+        return sqrt(pow(self.x - loc.x, 2) + pow(self.y - loc.y, 2) + pow(self.z - loc.z,2))
+
     def getPositionInArray(self, arraySize=3):
         '''
         Returns the unique ID of a fingerprint given its location
@@ -265,4 +267,31 @@ def resolve_barycenter(nC, d):
         + 1 / (1+d[1]/d[0]+d[1]/d[2]+d[1]/d[3])*nC[1].location \
         + 1 / (1+d[2]/d[1]+d[2]/d[0]+d[2]/d[3])*nC[2].location \
         + 1 / (1+d[3]/d[1]+d[3]/d[2]+d[3]/d[0])*nC[3].location 
-     
 \ No newline at end of file
+
+
+def NLateration(data, step=.1, xSize=0.0, ySize=0.0, zSize=0.0):
+    '''
+    Returns a tuple containing (Computed Location, Total distance)
+    :param data: Array of tuples containing (Location, distance) of known emitters
+    :param step: Increment of grid search (lower step => better precision => slower computation)
+    :param xSize: The X dimension of the cuboid containing all the emitters (automatically computed if not specified)
+    :param ySize: The Y dimension of the cuboid containing all the emitters (automatically computed if not specified)
+    :param zSize: The Z dimension of the cuboid containing all the emitters (automatically computed if not specified)
+    '''
+    minLoc = Location()
+    minDist = xMax = yMax = zMax = 0.0
+    for k in data:
+        minDist += abs(k[0].distanceTo(Location()) - k[1])
+        xMax = k[0].x if k[0].x > xMax and not xSize else k[0].x
+        yMax = k[0].y if k[0].y > yMax and not ySize else k[0].y
+        zMax = k[0].z if k[0].z > zMax and not zSize else k[0].z
+    for k in arange(0.1,xSize,step):
+        for l in arange(0.1,ySize,step):
+            for m in arange(0.1,zSize,step):
+                d = .0
+                for n in data:
+                    d += abs(n[0].distanceTo(Location(k,l,m)) - n[1])
+                if d < minDist:
+                    minDist = d
+                    minLoc = Location(round(k,2),round(l,2),round(m,2))
+    return (minLoc, minDist)
 \ No newline at end of file