274 lines
7.2 KiB
R
274 lines
7.2 KiB
R
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=""))
|
|
|