Add R
This commit is contained in:
Votre Nom
parent
f37f200792
commit
19b2667210
5 changed files with 656 additions and 0 deletions
77
R/intersectionCercles.R
Executable file
77
R/intersectionCercles.R
Executable file
|
|
@ -0,0 +1,77 @@
|
|||
|
||||
##############################
|
||||
# Calcule les ou le point d'intersection de deux cercle :
|
||||
# Cercle 1 de coordonnés (a1,b1) de rayon d1
|
||||
# Cercle 2 de coordonnés (a2,b2) de rayon d2
|
||||
# Precision = nombre de chiffre aprés la virgule
|
||||
##############################
|
||||
getIntersection=function(a1,b1,d1,a2,b2,d2,precision=5){
|
||||
# Compute E (cf paper)
|
||||
computeE=function(a1,b1,a2,b2){
|
||||
numerateur=-2*a1+2*a2;
|
||||
denominateur=-2*b1+2*b2;
|
||||
return(numerateur/denominateur);
|
||||
}
|
||||
|
||||
# Compute F (cf paper)
|
||||
computeF=function(a1,b1,d1,a2,b2,d2){
|
||||
numerateur=a1^2+b1^2-a2^2-b2^2-d1^2+d2^2;
|
||||
denominateur=-2*b1+2*b2;
|
||||
return(numerateur/denominateur);
|
||||
}
|
||||
|
||||
# Compute intersections if b1 != b2
|
||||
computeYNotEqual=function(a1,b1,d1,a2,b2,d2){
|
||||
E=computeE(a1,b1,a2,b2);
|
||||
F=computeF(a1,b1,d1,a2,b2,d2);
|
||||
A=E^2+1;
|
||||
B=(-2*a1+2*E*F+2*E*b1);
|
||||
C=2*F*b1+b1^2+a1^2+F^2-d1^2;
|
||||
|
||||
values=polyroot(c(C,B,A));
|
||||
x=c();
|
||||
y=c();
|
||||
for(i in 1:length(values)){
|
||||
if(round(Im(values[i]),digit=precision)==0){
|
||||
x=c(x,Re(values[i]));
|
||||
y=c(y,-Re(values[i])*E-F);
|
||||
}
|
||||
}
|
||||
if(length(x)==0){
|
||||
return(NULL);
|
||||
}
|
||||
return(matrix(c(x,y),nrow=length(x),ncol=2));
|
||||
}
|
||||
|
||||
# Compute intersections if b1 == b2 and a1 != a2
|
||||
computeYEqualAndXNotEqual=function(a1,b1,d1,a2,b2,d2){
|
||||
G=-((a1^2-a2^2-d1^2+d2^2)/(-2*a1+2*a2));
|
||||
A=1;
|
||||
B=-2*b1;
|
||||
C=G^2-2*G*a1+a1^2+b1^2-d1^2;
|
||||
values=polyroot(c(C,B,A));
|
||||
x=c();
|
||||
y=c();
|
||||
for(i in 1:length(values)){
|
||||
if(round(Im(values[i]),digit=precision)==0){
|
||||
x=c(x,G);
|
||||
y=c(y,Re(values[i]));
|
||||
}
|
||||
}
|
||||
if(length(y)==0){
|
||||
return(NULL);
|
||||
}
|
||||
return(matrix(c(x,y),nrow=length(y),ncol=2));
|
||||
|
||||
}
|
||||
|
||||
# Compute intersections
|
||||
if(b1!=b2){
|
||||
return(computeYNotEqual(a1,b1,d1,a2,b2,d2));
|
||||
}
|
||||
else if(a1!=a2){
|
||||
return(computeYEqualAndXNotEqual(a1,b1,d1,a2,b2,d2));
|
||||
}
|
||||
# No intersection found
|
||||
return(NULL);
|
||||
}
|
||||
274
R/loadDataExp.R
Normal file
274
R/loadDataExp.R
Normal file
|
|
@ -0,0 +1,274 @@
|
|||
source(paste(dirname(sys.frame(1)$ofile),"/multilateration.R",sep=""))
|
||||
source(paste(dirname(sys.frame(1)$ofile),"/tools.R",sep=""))
|
||||
|
||||
|
||||
gw1=read.csv("~/data3/records/anchor_0")
|
||||
gw2=read.csv("~/data3/records/anchor_1")
|
||||
gw3=read.csv("~/data3/records/anchor_2")
|
||||
turtle=read.csv("~/data3/gps.csv")
|
||||
calibration=23.105559
|
||||
compass=data.frame(78,83,69,87)
|
||||
colnames(compass)<-c("N","S","E","W")
|
||||
|
||||
toDeg=function(deg,min,sec){ return(deg+(min/60)+(sec/3600))}
|
||||
|
||||
getDist=function(rssi){
|
||||
lambda=0.34855535170329033
|
||||
pt=0.050118723363
|
||||
gt=1
|
||||
gr=1
|
||||
left=1/(pt*gt*gr)
|
||||
right=10^((rssi-30)/10)
|
||||
den=sqrt(left*right)
|
||||
frac=1/den
|
||||
return((lambda/(4*pi))*frac)
|
||||
|
||||
}
|
||||
|
||||
addGPS=function(pos){
|
||||
rownames(pos)<- as.character((200+(1:NROW(pos))))
|
||||
plot<<-plot+geom_path(data=pos,aes(x=X1,y=X2),col="red",size=I(0.2))
|
||||
}
|
||||
addEst=function(pos){
|
||||
|
||||
rownames(pos)<- as.character((100+(1:NROW(pos))))
|
||||
|
||||
plot<<-plot+geom_point(data=pos,aes(x=X1,y=X2),col="purple",size=I(1))
|
||||
}
|
||||
addLim=function(){
|
||||
plot<<- plot+xlim(c(-40,50)) +ylim(c(-1,70))+coord_fixed()
|
||||
}
|
||||
toDF=function(vect){
|
||||
|
||||
}
|
||||
|
||||
|
||||
getRelCoord=function(baseGW, relGW){
|
||||
R=6371000 # Rayon de la terre
|
||||
alpha=do.call(toDeg,as.list(baseGW[5:7]))
|
||||
beta=do.call(toDeg,as.list(relGW[5:7]))
|
||||
if(baseGW[8]!=relGW[8]){
|
||||
beta=-beta
|
||||
}
|
||||
if(baseGW[8]==compass["W"]){
|
||||
alpha=-alpha
|
||||
}
|
||||
if(relGW[8]==compass["W"]){
|
||||
beta=-beta
|
||||
}
|
||||
x=R*sin((beta-alpha)*pi/180)
|
||||
|
||||
alpha=do.call(toDeg,as.list(baseGW[1:3]))
|
||||
beta=do.call(toDeg,as.list(relGW[1:3]))
|
||||
if(baseGW[4]!=relGW[4]){
|
||||
beta=-beta
|
||||
}
|
||||
if(baseGW[4]==compass["S"]){
|
||||
alpha=-alpha
|
||||
}
|
||||
if(relGW[4]==compass["S"]){
|
||||
beta=-beta
|
||||
}
|
||||
y=R*sin((beta-alpha)*pi/180)
|
||||
|
||||
|
||||
return(c(x,y))
|
||||
}
|
||||
|
||||
#g1=c(21,20,28.39,compass$S,55,29,28.69,compass$E)
|
||||
#g2=c(21,20,28.34,compass$S,55,29,29.44,compass$E)
|
||||
#g3=c(21,20,27.70,compass$S,55,29,28.70,compass$E)
|
||||
|
||||
#g1=c(21,20,27.534,compass$S,55,29,27.941,compass$E)
|
||||
#g2=c(21,20,25.57,compass$S,55,29,28.35,compass$E)
|
||||
#g3=c(21,20,26.64,compass$S,55,29,28.098,compass$E)
|
||||
|
||||
g1=c(21,20,28.806,compass$S,55,29,29.081,compass$E)
|
||||
g2=c(21,20,28.704,compass$S,55,29,28.157,compass$E)
|
||||
g3=c(21,20,27.864,compass$S,55,29,29.526,compass$E)
|
||||
|
||||
a=getRelCoord(g1,g2)
|
||||
b=getRelCoord(g1,g3)
|
||||
c=rbind(c(0,0),a,b)
|
||||
|
||||
plot=ggplot(data=data.frame(c))+geom_point(aes(x=X1,y=X2),colour="orange",shape=17,size=I(4))+coord_fixed()+ylab("Y (m)") + xlab("X (m)")
|
||||
|
||||
#Do multilateration
|
||||
dist1=rbind(unlist(gw1[12]))
|
||||
dist2=rbind(unlist(gw2[12]))
|
||||
dist3=rbind(unlist(gw3[12]))
|
||||
dist=rbind(dist1,dist2,dist3)
|
||||
dist=getDist(dist+calibration)
|
||||
# estimated=NULL
|
||||
# estimated=multilateration(c[,1],c[,2],dist)
|
||||
# print(NCOL(estimated))
|
||||
#
|
||||
#
|
||||
# if(NROW(estimated)>0){
|
||||
# estimated=data.frame(t(estimated)) # generate warnings
|
||||
# colnames(estimated)=c("X1","X2")
|
||||
# addEst(estimated)
|
||||
# }
|
||||
# print(max(c(max(estimated[,1]),max(estimated[,2]))))
|
||||
# # Display Real position
|
||||
# real=NULL
|
||||
# for(i in 1:NROW(turtle)){
|
||||
# tt=as.vector(c(unlist(turtle[i,][2:5]),unlist(turtle[i,][6:9])))
|
||||
# real=rbind(real,getRelCoord(g1,tt))
|
||||
# }
|
||||
# if(NROW(real)>0){
|
||||
# real=data.frame(real)# generate warnings
|
||||
# addGPS(real)
|
||||
# }
|
||||
#
|
||||
# # Plot graphic
|
||||
# #addLim();
|
||||
# print(plot)
|
||||
###########################################################
|
||||
# for(i in 1:NROW(turtle)){
|
||||
# plot=ggplot(data=data.frame(c))+geom_point(aes(x=X1,y=X2),colour="orange",shape=17,size=I(4))#+xlim(c(-5,24)) +ylim(c(-1,22))+coord_fixed()
|
||||
#
|
||||
# estimated=multilateration(c[,1],c[,2],as.matrix(dist[,i]),stepByStep=TRUE)
|
||||
#
|
||||
#
|
||||
# if(NROW(estimated)>0){
|
||||
# estimated=data.frame(t(estimated))
|
||||
# colnames(estimated)=c("X1","X2")
|
||||
# plot=plot+geom_point(data=estimated,aes(x=X1,y=X2),col="purple",size=I(2))
|
||||
# }
|
||||
# # Display Real position
|
||||
# real=NULL
|
||||
# #for(i in 1:NROW(turtle)){
|
||||
# tt=as.vector(c(unlist(turtle[i,][2:5]),unlist(turtle[i,][6:9])))
|
||||
# real=rbind(real,getRelCoord(g1,tt))
|
||||
# #}
|
||||
# if(NROW(real)>0){
|
||||
# real=data.frame(real)
|
||||
# plot=plot+geom_point(data=real,aes(x=X1,y=X2),col="red",size=I(1.5),shape=3)
|
||||
# }
|
||||
#
|
||||
# # Plot graphic
|
||||
# #print(plot)
|
||||
# #readline(prompt="Press [enter] to continue")
|
||||
# }
|
||||
# ##########################################
|
||||
# for(i in 1:NROW(dist[1,])){
|
||||
#
|
||||
# estimated=multilateration(c[,1],c[,2],as.matrix(dist[,i]))
|
||||
#
|
||||
# plot=ggplot(data=data.frame(c))+geom_point(aes(x=X1,y=X2),colour="orange",shape=17,size=I(4))+coord_fixed()
|
||||
#
|
||||
# if(NROW(estimated)>0){
|
||||
# estimated=data.frame(t(estimated))
|
||||
# colnames(estimated)=c("X1","X2")
|
||||
# plot=plot+geom_point(data=estimated,aes(x=X1,y=X2),col="purple",size=I(2))
|
||||
# }
|
||||
# # Display Real position
|
||||
# real=NULL
|
||||
# #for(i in 1:NROW(turtle)){
|
||||
# tt=as.vector(c(unlist(turtle[i,][2:5]),unlist(turtle[i,][6:9])))
|
||||
# real=rbind(real,getRelCoord(g1,tt))
|
||||
# #}
|
||||
# if(NROW(real)>0){
|
||||
# real=data.frame(real)
|
||||
# plot=plot+geom_point(data=real,aes(x=X1,y=X2),col="red",size=I(1.5),shape=3)
|
||||
# }
|
||||
#
|
||||
# # Plot graphic
|
||||
# if(NROW(estimated)!=0){
|
||||
# print(plot)
|
||||
# readline(prompt="Press [enter] to continue")
|
||||
# }
|
||||
# }
|
||||
#########################################
|
||||
# estF=NULL
|
||||
# realF=NULL
|
||||
# for(i in 1:NROW(turtle)){
|
||||
# cal=0
|
||||
# estimated=multilateration(c[,1],c[,2],as.matrix(dist[,i]))
|
||||
# while(NROW(estimated)==0){
|
||||
# cal=cal+1
|
||||
# curDist=as.matrix(dist[,i])-cal
|
||||
# curDist[curDist<1]<-1
|
||||
# estimated=multilateration(c[,1],c[,2],curDist)
|
||||
# if(sum(curDist==1)==3){
|
||||
# break
|
||||
# }
|
||||
# }
|
||||
# if(NROW(estimated)>0){
|
||||
# estF=rbind(estF,t(estimated))
|
||||
# }
|
||||
# else{
|
||||
# estF=rbind(estF,c(0,0))
|
||||
# }
|
||||
#
|
||||
#
|
||||
# #estimated=multilateration(c[,1],c[,2],as.matrix(dist[,i])-cal)
|
||||
#
|
||||
#
|
||||
# if(NROW(estimated)>0){
|
||||
# estimated=data.frame(t(estimated))
|
||||
# colnames(estimated)=c("X1","X2")
|
||||
# }
|
||||
# # Display Real position
|
||||
# real=NULL
|
||||
# #for(i in 1:NROW(turtle)){
|
||||
# tt=as.vector(c(unlist(turtle[i,][2:5]),unlist(turtle[i,][6:9])))
|
||||
# real=rbind(real,getRelCoord(g1,tt))
|
||||
# #}
|
||||
# if(NROW(real)>0){
|
||||
# real=data.frame(real)
|
||||
# realF=rbind(realF,real)
|
||||
# }
|
||||
#
|
||||
#
|
||||
# }
|
||||
# estF=data.frame(estF)
|
||||
# realF=data.frame(realF)
|
||||
# plot=ggplot(data=data.frame(c))+geom_point(aes(x=X1,y=X2),colour="orange",shape=17,size=I(4))#+xlim(c(-5,24)) +ylim(c(-1,22))+coord_fixed()
|
||||
# plot=plot+geom_point(data=estF,aes(x=X1,y=X2,colour=seq(1,NROW(estF),by=5)),size=I(2))
|
||||
# plot=plot+geom_point(data=realF,aes(x=X1,y=X2),col="red",size=I(1.5),shape=3)
|
||||
#
|
||||
# # Plot graphic
|
||||
# print(plot)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
estimated=optimizeRadius(c[,1],c[,2],as.matrix(dist), radiusStep = 0.1, zeroForNull = TRUE)
|
||||
#estimated=multilateration(c[,1],c[,2],as.matrix(dist))
|
||||
|
||||
estimated=data.frame(t(estimated))
|
||||
colnames(estimated)=c("X1","X2")
|
||||
plot=plot+geom_point(data=estimated,aes(x=X1,y=X2),col="purple",size=I(2))
|
||||
|
||||
# Display Real position
|
||||
real=NULL
|
||||
for(i in 1:NROW(turtle)){
|
||||
tt=as.vector(c(unlist(turtle[i,][2:5]),unlist(turtle[i,][6:9])))
|
||||
real=rbind(real,getRelCoord(g1,tt))
|
||||
}
|
||||
if(NROW(real)>0){
|
||||
real=data.frame(real)
|
||||
plot=plot+geom_point(data=real,aes(x=X1,y=X2),col="red",size=I(1.5),shape=3)
|
||||
}
|
||||
|
||||
# Plot graphic
|
||||
addLim()
|
||||
print(plot)
|
||||
error=NULL
|
||||
nbFound=0
|
||||
for(i in 1:NROW(estimated)){
|
||||
x1=estimated[i,1]
|
||||
y1=estimated[i,2]
|
||||
x2=real[i,1]
|
||||
y2=real[i,2]
|
||||
if(x1 !=0 && y1!=0){
|
||||
error=c(error,computeCartDist(x1,y1,x2,y2))
|
||||
nbFound=nbFound+1
|
||||
}
|
||||
}
|
||||
errorMean=mean(error)
|
||||
print(paste("Mean error : ",errorMean, " for ",nbFound," points, donc ",nbFound*100/NROW(estimated),"% de points trouves",sep=""))
|
||||
|
||||
222
R/multilateration.R
Executable file
222
R/multilateration.R
Executable file
|
|
@ -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)
|
||||
}
|
||||
50
R/sx1276.R
Executable file
50
R/sx1276.R
Executable file
|
|
@ -0,0 +1,50 @@
|
|||
# Changer les paramètres plus bas et éxécuter ce fichier afin de calculer la durée
|
||||
# d'émission d'une trame en Lora sur le module sx1276
|
||||
# Voir sx1276 datasheet pour le détails de ces paramètres :
|
||||
PL=10
|
||||
SF=12
|
||||
IH=0
|
||||
DE=0
|
||||
CR=1
|
||||
CRC=0
|
||||
BW=500*10^3
|
||||
nPreamble=6
|
||||
|
||||
# Cf SX1276 datasheet
|
||||
getNPayload=function(PL,SF,IH,DE,CR,CRC){
|
||||
numerateur=8*PL-4*SF+28+16*CRC-20*IH;
|
||||
denominateur=4*(SF-2*DE);
|
||||
ceil=ceiling(numerateur/denominateur);
|
||||
np=(8+max(ceil*(CR+4),0));
|
||||
return(np);
|
||||
}
|
||||
|
||||
# Cf SX1276 datasheet
|
||||
getTPrembule=function(nPreamble,Rs){
|
||||
return((nPreamble+4.25)*(1/Rs));
|
||||
}
|
||||
|
||||
# Cf SX1276 datasheet
|
||||
getTPayload=function(nPayload,Rs){
|
||||
return(nPayload*(1/Rs));
|
||||
}
|
||||
|
||||
# Cf SX1276 datasheet
|
||||
getRs=function(BW,SF){
|
||||
Rs=BW/(2^SF)
|
||||
return(Rs);
|
||||
}
|
||||
|
||||
getTPacket=function(PL,SF,IH,DE,CR,CRC,BW,nPreamble){
|
||||
Rs=getRs(BW,SF);
|
||||
print(paste("Symbol duration",Rs,"s"))
|
||||
tPrembule=getTPrembule(nPreamble,Rs);
|
||||
print(paste("Preambule duration",tPrembule,"s"))
|
||||
nPayload=getNPayload(PL,SF,IH,DE,CR,CRC);
|
||||
tPayload=getTPayload(nPayload,Rs);
|
||||
print(paste("Payload duration",tPayload,"s"))
|
||||
return(tPrembule+tPayload);
|
||||
}
|
||||
|
||||
# Afficher le calcule
|
||||
print(paste("Packet duration",getTPacket(PL,SF,IH,DE,CR,CRC,BW,nPreamble),"s"))
|
||||
33
R/tools.R
Executable file
33
R/tools.R
Executable file
|
|
@ -0,0 +1,33 @@
|
|||
# Compute cartesian distance of two points
|
||||
computeCartDist=function(x1,y1,x2,y2){
|
||||
return(sqrt((x2-x1)^2+(y2-y1)^2));
|
||||
}
|
||||
|
||||
# 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 the middle point of a segment
|
||||
getMiddleOfSegment=function(x1,y1,x2,y2){
|
||||
x=(x1+x2)/2;
|
||||
y=(y1+y2)/2;
|
||||
return(c(x,y));
|
||||
}
|
||||
|
||||
# Convert dBm to Watt
|
||||
dBm2W=function(pdBm){
|
||||
return((10^(pdBm/10))/1000);
|
||||
}
|
||||
|
||||
# Convert Watt to dBm
|
||||
W2dBm=function(pW){
|
||||
return(10*log10(1000*pW));
|
||||
}
|
||||
|
||||
Loading…
Add table
Reference in a new issue