def calcPosition_Multilateration(d,nSensor,nAnchor,sigma,radiorange,iteN,typeE,typeC):
print "Multilateraitonによる位置計算を開始"
print ""
locSensorNs = []
RMSDs =[]
RMSDs_modified=[]
i=0
breachN = 0
while i-breachN < iteN:
[posX,locX,disX,conX,noisyDisX,trueDisX,estPosX,cLevel] = problems.createProblem_random(d,nSensor,nAnchor,sigma,radiorange,i)
multi.localizeByMultilateration(nSensor,nAnchor,sigma,radiorange,locX,conX,estPosX,noisyDisX,cLevel,typeE,typeC)
[nLocSensor,preLocSensor] = helper.analyseLocalizability(nSensor,nAnchor,locX)
locSensorNs.append(nLocSensor)
rmsd = helper.calcRMSD(nSensor,locX,posX,estPosX)
RMSDs.append(rmsd)
if rmsd <1.5:
RMSDs_modified.append(rmsd)
else: breachN +=1
i+=1
print rmsd
perLocSensor = sum(locSensorNs)/nSensor/iteN
perMinLocSensor = min(locSensorNs)/nSensor
perMaxLocSensor = max(locSensorNs)/nSensor
[aveRMSD,minRMSD,maxRMSD] = helper.analyseRMSDs(RMSDs)
[aveRMSD_modified,minRMSD_modified,maxRMSD_modified]= helper.analyseRMSDs(RMSDs_modified)
return [perLocSensor,perMinLocSensor,perMaxLocSensor,aveRMSD,minRMSD,maxRMSD,aveRMSD_modified,breachN]
def noiseImpactSearch(d,nSensor,nAnchor,sigmas,radiorange):
graph.setLimit()
for i in range(len(sigmas)):
graph.setGraph()
[posX,locX,disX,conX,noisyDisX,trueDisX,estPosX,cLevel] = problems.createProblem_random(d,nSensor,nAnchor,sigma,radiorange,0)
multi.localizeByMultilateration(nSensor,nAnchor,locX,conX,estPosX,noisyDisX,cLevel,0,0)
rmsd = helper.calcRMSD(nSensor,locX,posX,estPosX)
graph.plotPoint2D(estPosX,"")
print rmsd
graph.show()
def paramSearch(d,nSensor,nAnchor,radiorange,iteN):
sumLocSensor = 0
minNLocSensor = sys.maxint
maxNLocSensor = 0
for i in range(iteN):
[posX,locX,disX,conX,noisyDisX,trueDisX,estPosX,cLevel] = problems.createProblem_random(d,nSensor,nAnchor,sigma,radiorange,i)
tri.checkLocalizability(nSensor,locX,conX)
[nLocSensor,preLocSensor] = helper.analyseLocalizability(nSensor,nAnchor,locX)
sumLocSensor += nLocSensor
minNLocSensor = min(minNLocSensor,nLocSensor)
maxNLocSensor = max(maxNLocSensor,nLocSensor)
print nLocSensor
aveNLocSensor = sumLocSensor / iteN
perLocSensor = aveNLocSensor/nSensor
perMinLocSensor = minNLocSensor / nSensor
perMaxLocSensor = maxNLocSensor / nSensor
return [perLocSensor,perMinLocSensor,perMaxLocSensor]
def calcPosition_Trilateration(d,nSensor,nAnchor,sigma,radiorange,iteN,type):
print "Trilaterationによる位置計算を開始"
locSensorNs = []
RMSDs =[]
for i in range(iteN):
[posX,locX,disX,conX,noisyDisX,trueDisX,estPosX,cLevel] = problems.createProblem_random(d,nSensor,nAnchor,sigma,radiorange,i+100)
if type==1:tri.localizeByTrilateration(nSensor,nAnchor,locX,conX,estPosX,noisyDisX,cLevel)
[nLocSensor,preLocSensor] = helper.analyseLocalizability(nSensor,nAnchor,locX)
locSensorNs.append(nLocSensor)
rmsd = helper.calcRMSD(nSensor,locX,posX,estPosX)
RMSDs.append(rmsd)
print nLocSensor
print rmsd
perLocSensor = sum(locSensorNs)/nSensor/iteN
perMinLocSensor = min(locSensorNs)/nSensor
perMaxLocSensor = max(locSensorNs)/nSensor
[aveRMSD,minRMSD,maxRMSD] = helper.analyseRMSDs(RMSDs)
RMSDsModified = [elem for elem in RMSDs if elem<1]
[aveRMSD_modified,a,b]= helper.analyseRMSDs(RMSDsModified)
return [perLocSensor,perMinLocSensor,perMaxLocSensor,aveRMSD,minRMSD,maxRMSD,aveRMSD_modified]
def clusteringByWheel(d,nSensor,radiorange):
[posX,locX,disX,conX,noisyDisX,trueDisX,estPosX,cLevel] = problems.createProblem_random(d,nSensor,0,0,radiorange)
nNode = len(conX)
isWheelHub = np.zeros(nNode)
for i in range(0,nNode):
c = conX[i]
indexes = np.where(c == 1)[0]
if len(indexes) >=3:
indexes = np.insert(indexes,0,i)
localConX = np.array([[conX[k][j] for k in indexes] for j in indexes])
if wheel.isWheelHub(0,localConX):
isWheelHub[i] = 1
for i in range(nNode):
if isWheelHub[i] == 1:
dot = "or"
else :
dot = "ob"
pylab.plot(posX[i][0],posX[i][1],dot)
for j in range(nNode):
if conX[i][j] == 1:
pylab.plot([posX[i][0],posX[j][0]],[posX[i][1],posX[j][1]],"g-")
pylab.show()
pylab.plot(posX[i][0],posX[i][1],"ob")
pylab.plot(estPosX[i][0],estPosX[i][1],"oc")
pylab.plot(estPosX2[i][0],estPosX2[i][1],"og")
else :
pylab.plot(posX[i][0],posX[i][1],"xb")
# アンカーをプロット
for i in range(nSensor,nSensor+nAnchor):
pylab.plot(posX[i][0],posX[i][1],"or")
if __name__ == "__main__":
d=2
nSensor=5
nAnchor=5
sigma=0.1
radiorange=20
[posX,locX,disX,conX,noisyDisX,trueDisX,estPosX,noiseLevel] = problems.createProblem_random(d,nSensor,nAnchor,sigma,radiorange)
localizeByTrilateration(nSensor,nAnchor,locX,conX,estPosX,noisyDisX,noiseLevel)
estPosX2 = copy.deepcopy(estPosX)
# func = function.DistanceFunction(0,3,estPosX,noisyDisX)
func = function.DistanceFunction1(nSensor,nAnchor,posX,noisyDisX,conX)
print func.getValue()
# print func.posX
for i in range(10):
if np.linalg.norm(func.dx)<0.001:break
func.nextStep()
print posX[0]
print estPosX2
print estPosX
drawGraph1(nSensor,nAnchor,locX,posX,estPosX)
pylab.show()
# print func.posX
