Add R

1 files changed, 222 insertions, 0 deletions

diff --git a/R/multilateration.R b/R/multilateration.R
new file mode 100755
index 0000000..454d8f6
--- /dev/null
+++ b/R/multilateration.R
@@ -0,0 +1,222 @@
+# Librairies nécessaire : plotrix
+
+source(paste(dirname(sys.frame(1)$ofile),"/intersectionCercles.R",sep=""))
+library("plotrix")
+
+##############################
+# Retourne la position estimée pour les différentes distance
+# gwX = une matrice des coordonnées en X de chaque gateway (attention à l'odre avec gwY) exemple :
+#       gwX1,gwX2,gwX3
+# gwY = une matrice des coordonnées en Y de chaque gateway (attention à l'odre avec gwX)
+# distances = une matrice, exemple pour trois gateway avec 4 points à trouvées :
+#
+#  d1gw1,d2gw1,d3gw1,d4gw1
+#  d1gw2,d2gw2,d3gw2,d4gw2
+#  d1gw3,d2gw3,d3gw3,d4gw3
+#
+# stepByStep = si a TRUE alors affiche la multilatération pour chaque points
+# precisionPlot = nombre d'unité en X et en Y lors de l'affichage de la multilatération pour chaque points
+##############################
+multilateration=function(gwX,gwY,distances,stepByStep=FALSE,precisionPlot=80){
+
+  # Check parameters
+  if(!is.vector(gwX)){
+    stop("gwX is not a vector")
+  }
+  else if(!is.vector(gwY)){
+    stop("gwY is not a vector")
+  }
+  else if(!is.matrix(distances)){
+    stop("distances is not a matrix")
+  }
+  else if(length(gwX)!=length(gwY)){
+    stop("gwX and gwY haven't the same length")
+  }
+  else if(NROW(distances)!= length(gwX)){
+    stop("Number of rows of distances have not the same length of the gwX and gwY")
+  }
+  
+  # Init convenience variables
+  nbGw=length(gwX);
+  nbDistances=NCOL(distances)
+  
+  # Get the middle point of a segment
+  getMiddleOfSegment=function(x1,y1,x2,y2){
+    x=(x1+x2)/2;
+    y=(y1+y2)/2;
+    return(c(x,y));
+  }
+  
+  # Get line equation from two of his points y=ax+b
+  getLineEquation=function(x1,y1,x2,y2){
+    eq=NULL;
+    if(x1!=x2){
+      a=(y1-y2)/(x1-x2);
+      b=y1-a*x1;
+      eq=c(a,b);
+    }
+    return(eq);
+  }
+  
+  # Get line equation of 2 circles intersections points
+  getIntersectionLineEquation=function(circlesIntersections){
+    if(NROW(circlesIntersections)>1){
+      lineEquation=getLineEquation(
+        circlesIntersections[1,1],circlesIntersections[1,2],
+        circlesIntersections[2,1],circlesIntersections[2,2]);
+      if(is.null(lineEquation)){
+        return(circlesIntersections[1,1]);
+      }
+      return(lineEquation);
+    }
+    return(NULL);
+  }
+  
+  # Build solution for each distances
+  sol=NULL;
+  for(i in 1:nbDistances){
+    linearLines=NULL;
+    xLines=NULL;
+    currentSol=NULL;
+    circlesIntersections=NULL;
+    
+    # Build lines equation for linearLines and xLines
+    for(j in 1:(nbGw-1)){
+      mainGw=c(gwX[j],gwY[j]);
+      for(k in (j+1):nbGw){
+        slaveGw=c(gwX[k],gwY[k])
+        currentCirclesIntersections=getIntersection(mainGw[1],mainGw[2],distances[j,i],slaveGw[1],slaveGw[2],distances[k,i]);
+        circlesIntersections=rbind(circlesIntersections,currentCirclesIntersections);
+        lineEquation=getIntersectionLineEquation(currentCirclesIntersections)
+        if(length(lineEquation)==1){
+          xLines=rbind(xLines,lineEquation)
+        }
+        else{
+          linearLines=rbind(linearLines,lineEquation)
+        }
+      }
+    }
+
+    # Build lines intersections
+    intersections=NULL;
+    if(NROW(linearLines)>0||length(xLines)>0){
+      # Get intersections with xLines
+      sapply(xLines,function(x){
+        if(NROW(linearLines)>0){
+          apply(linearLines,1,function(eq){
+            intersections<<-rbind(intersections,c(x,eq[1]*x+eq[2]))
+          });
+        }
+       
+      });
+      
+     # Get linearLines intersections
+      if(NROW(linearLines)>1){
+        for(j in 1:(NROW(linearLines)-1)){
+          mainLL=c(linearLines[j,1],linearLines[j,2]);
+          for(k in (j+1):(NROW(linearLines))){
+            slaveLL=c(linearLines[k,1],linearLines[k,2]);
+            toSolve=matrix(c(-mainLL[1],-slaveLL[1],1,1), ncol=2,nrow=2);
+            tryCatch({
+              solution=solve(toSolve,c(mainLL[2],slaveLL[2]))
+              intersections=rbind(intersections,solution)
+            },error=function(error){});
+           
+          }
+        }
+      }
+    }
+    
+    # Build solution, if we have intersections
+    if(NROW(intersections)>0){
+      currentSol=c(mean(intersections[,1]),mean(intersections[,2]));
+      sol=cbind(sol,currentSol);
+    }
+    # If we don't have intersections (middle of segment)
+    else{
+      if(NROW(linearLines)>0||length(xLines)>0){
+        currentSol=getMiddleOfSegment(
+          circlesIntersections[1,1],circlesIntersections[1,2],
+          circlesIntersections[2,1],circlesIntersections[2,2]
+          );
+       # sol=cbind(sol,currentSol);
+      }
+    }
+
+    # If we plot current solution
+    if(stepByStep){
+      # Plot gateways
+      plot(gwX,gwY,asp=1,xlim=c(-precisionPlot,precisionPlot),ylim=c(-precisionPlot,precisionPlot),pch=17, col="orange")
+      # Plot circles
+      for(j in 1:nbGw){
+        draw.circle(gwX[j],gwY[j],distances[j,i],lwd=0.5);
+      }
+      # Plot circles intersections
+      if(!is.null(circlesIntersections)){
+        apply(circlesIntersections,1,function(row){
+          lines(row[1],row[2],type="p",col="blue",pch=20)
+        });
+      }
+      if(!is.null(intersections)){
+        apply(intersections,1,function(row){
+          lines(row[1],row[2],type="p",col="black",pch=20)
+        });
+      }
+      if(!is.null(linearLines)){
+        apply(linearLines,1,function(row){
+          abline(row[2],row[1]);
+        });
+      }
+      
+      if(!is.null(xLines)){
+        sapply(xLines,function(x){
+          abline(v=x);
+        });
+      }
+      if(!is.null(sol)){
+        lines(sol[1],sol[2],type="p",col="red", pch=16)
+      }
+     
+      readline(prompt = "Press enter for next step...")
+    }
+    
+  }
+  return(sol);
+}
+
+##############################
+# Multilateration avec optimisation de la taille des cercles (prendre le plus petit rayon possible)
+# radiusStep = pas de réduction de la taille du rayon
+##############################
+optimizeRadius=function(gwX,gwY,distances,stepByStep=FALSE,precisionPlot=80, radiusStep=1,zeroForNull=FALSE){
+  sol=NULL;
+  for(i in 1:NCOL(distances)){
+    factor=0
+    curDistances=as.matrix(distances[,i])-factor
+    # Try to find solution without optimisation (factor=0)
+    estimated=multilateration(gwX,gwY,curDistances,stepByStep=stepByStep,precisionPlot=precisionPlot)
+    # Repeat multilateration until radius = 0
+    while(sum(curDistances<=0)!=NROW(gwX)){
+      factor=factor+radiusStep
+      curDistances=curDistances-factor # Reduce circles radius
+      curDistances[curDistances<0]<-0 # Put zero in negative radius
+      e=multilateration(gwX,gwY,curDistances,stepByStep=stepByStep,precisionPlot=precisionPlot) # Temporary solution
+      if(!is.null(e)){ # A better solution is found
+        estimated=e
+      }
+      else if(!is.null(estimated)){ # No other solution is possible
+        break;
+      }
+    }
+    if(zeroForNull && is.null(estimated)){
+        sol=cbind(sol,c(0,0))
+      
+    }
+    else{
+      if(!is.null(estimated)){
+        sol=cbind(sol,estimated)
+      }
+    }
+  }
+  return(sol)
+}
\ No newline at end of file