aboutsummaryrefslogtreecommitdiff
path: root/R/loadDataExp.R
diff options
context:
space:
mode:
Diffstat (limited to 'R/loadDataExp.R')
-rw-r--r--R/loadDataExp.R274
1 files changed, 274 insertions, 0 deletions
diff --git a/R/loadDataExp.R b/R/loadDataExp.R
new file mode 100644
index 0000000..a18d1b8
--- /dev/null
+++ b/R/loadDataExp.R
@@ -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=""))
+
generated by cgit v1.2.3 (git 2.46.0) at 2026-04-15 21:14:22 +0000