def HDFOptimizer(self, P, RN_RSS, RN_ToA, RN_TDoA, RN_TDoA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies LS approximation to get position P.
Return P
"""
RSSL=RSSLocation(RN_RSS)
TOAL=ToALocation(RN_ToA)
TDOAL=TDoALocation(RN_TDoA)
if RN_RSS==None:
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_ToA[:,0:1]*ones((1,RNnum_TDoA))
fopt=TOAL.ToAOptimizer(P, RN_ToA, ToA, ToAStd) + TDOAL.TDoAOptimizer(P, RN_TDoA, RN_TDoA2, TDoA, TDoAStd)
return fopt
elif RN_ToA==None:
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_RSS[:,0:1]*ones((1,RNnum_TDoA))
fopt=RSSL.DRSSOptimizer(P, RN_RSS, PL0, d0, RSS, RSSnp, RSSStd) + TDOAL.TDoAOptimizer(P, RN_TDoA, RN_TDoA2, TDoA, TDoAStd)
return fopt
elif RN_TDoA==None:
fopt=RSSL.DRSSOptimizer(P, RN_RSS, PL0, d0, RSS, RSSnp, RSSStd) + TOAL.ToAOptimizer(P, RN_ToA, ToA, ToAStd)
return fopt
else:
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_TDoA[:,0:1]*ones((1,RNnum_TDoA))
fopt=RSSL.DRSSOptimizer(P, RN_RSS, PL0, d0, RSS, RSSnp, RSSStd) + TOAL.ToAOptimizer(P, RN_ToA, ToA, ToAStd) + TDOAL.TDoAOptimizer(P, RN_TDoA, RN_TDoA2, TDoA, TDoAStd)
return fopt
def __init__(self,value=40,std=1.0,vcw=3,model={},p=np.array([])):
Constraint.__init__(self,'RSS',p)
self.Id = copy.copy(self.C_Id)
Constraint.C_Id = Constraint.C_Id+1 # constraint counter is incremented
self.value = value # attennation (dB)
if len(model)==0:
self.model['PL0'] =-34.7
self.model['d0'] = 1.0
self.model['RSSnp'] = 2.64
self.model['RSSStd'] = 4.34
self.model['Rest'] = 'mode'
else :
self.model = model
self.LOC = RSSLocation(p)
self.std = (self.LOC.getRangeStd(p, self.model['PL0'], self.model['d0'], self.value,self.model['RSSnp'], self.model['RSSStd'], self.model['Rest'])[0])/0.3
self.range = self.LOC.getRange(p, self.model['PL0'], self.model['d0'], self.value, self.model['RSSnp'], self.model['RSSStd'], self.model['Rest'])[0]
self.sstd = self.std*0.3
self.rescale(vcw)
self.runable = True
self.evaluated = False
self.annulus_bound()
def plot_cdf_ML_RSS_nTOA_crlb():
MLrss_vect=[]
MLrss1toa_vect=[]
MLrss1toacrlb_vect=[]
MLrss2toa_vect=[]
MLrss3toa_vect=[]
MLrss4toa_vect=[]
#Ntrial=10
for i in range(Ntrial):
print " ", Ntrial-i
P=L*rand(2,1)
shRN_TDOA = shape(RN_TDOA)
RNnumTDOA = shRN_TDOA[1]
RNTDOAmp=RN_TDOA-P
RNTDOA2mp=RN_TDOA2-P
RNTDOAmp2 = (sum(RNTDOAmp*RNTDOAmp,axis=0)).reshape(RNnumTDOA,1)
RNTDOA2mp2 = (sum(RNTDOA2mp*RNTDOA2mp,axis=0)).reshape(RNnumTDOA,1)
RDoA=sqrt(RNTDOAmp2)-sqrt(RNTDOA2mp2)
TDoAStd=(sig1/c)*rand(RNnumTDOA,1)
TDoA=RDoA/c+TDoAStd*randn(RNnumTDOA,1)
shRN_RSS = shape(RN_RSS)
RNnumRSS = shRN_RSS[1]
RSSStd =sh*ones((RNnumRSS,1))
RSSnp= np*ones((RNnumRSS,1))
d0=1.0
S2=RSSLocation(RN_RSS)
PL0=pl0*ones((RNnumRSS,1))
RSS=S2.getPL(RN_RSS, P, PL0, d0, RSSnp, RSSStd)
shRN_TOA = shape(RN_TOA)
RNnumTOA = shRN_TOA[1]
RNTOAmp=RN_TOA-P
RNTOAmp2 = (sum(RNTOAmp*RNTOAmp,axis=0)).reshape(RNnumTOA,1)
RoA=sqrt(RNTOAmp2)
ToAStd=(sig/c)*rand(RNnumTOA,1)
ToA=RoA/c+ToAStd*randn(RNnumTOA,1)
P0=L*rand(2,1)
S1=HDFLocation(RN_RSS, RN_TOA, RN_TDOA)
S2=RSSLocation(RN_RSS)
S3=CRBLocation(RN_RSS)
CRB_RSS_TOA_fim=[]
a=(4*rand(1)).astype(int)[0]
for b in range(4):
CRB_RSS_TOA_fim.append(sqrt(S3.CRB_RSS_TOA_fim(P, RN_RSS, RN_TOA[:,b:b+1], RSSnp, RSSStd, ToAStd[b:b+1,:])))
u=where(CRB_RSS_TOA_fim==min(CRB_RSS_TOA_fim))
v=where(CRB_RSS_TOA_fim==max(CRB_RSS_TOA_fim))
b=u[0][0]
a=v[0][0]
P4rss1toa=S1.MLHDFLocate(P, P0, RN_RSS, matrix(RN_TOA[:,a:a+1]), None, None, ToA[a:a+1,:], ToAStd[a:a+1,:], TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
MLrss1toa_vect.append(dist(P4rss1toa,P))
P4rss1toacrlb=S1.MLHDFLocate(P, P0, RN_RSS, matrix(RN_TOA[:,b:b+1]), None, None, ToA[b:b+1,:], ToAStd[b:b+1,:], TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
MLrss1toacrlb_vect.append(dist(P4rss1toacrlb,P))
'''P4rss2toa=S1.MLHDFLocate(P, P0, RN_RSS, matrix(RN_TOA[:,0:2]), None, None, ToA[:,0:2], ToAStd[:,0:2], TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
MLrss2toa_vect.append(dist(P4rss2toa,P))
P4rss3toa=S1.MLHDFLocate(P, P0, RN_RSS, matrix(RN_TOA[:,0:3]), None, None, ToA[:,0:3], ToAStd[:,0:3], TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
MLrss3toa_vect.append(dist(P4rss3toa,P))
P4rss4toa=S1.MLHDFLocate(P, P0, RN_RSS, matrix(RN_TOA[:,0:4]), None, None, ToA[:,0:4], ToAStd[:,0:4], TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
MLrss4toa_vect.append(dist(P4rss4toa,P))'''
cdf(MLrss1toa_vect,"b-","RSSI + 1 TOA (Randomly)",2)
cdf(MLrss1toacrlb_vect,"r-","RSSI + 1 TOA (CRLB Criteria)",2)
plt.legend(loc=4,numpoints=1)
#plt.axis([0,10,0,1])
plt.grid('on')
plt.xlabel("Positioning error (m)")
plt.ylabel("Cumulative probability")
plt.savefig("HDF_cdf_ML_RSS_nTOA_crlb.pdf", fromat="pdf")
plt.close()
def plot_cdf_ML_RSS_nTOA():
MLrss_vect=[]
MLrss1toa_vect=[]
MLrss2toa_vect=[]
MLrss3toa_vect=[]
MLrss4toa_vect=[]
#Ntrial=10
for i in range(Ntrial):
print " ", Ntrial-i
P=L*rand(2,1)
shRN_TDOA = shape(RN_TDOA)
RNnumTDOA = shRN_TDOA[1]
RNTDOAmp=RN_TDOA-P
RNTDOA2mp=RN_TDOA2-P
RNTDOAmp2 = (sum(RNTDOAmp*RNTDOAmp,axis=0)).reshape(RNnumTDOA,1)
RNTDOA2mp2 = (sum(RNTDOA2mp*RNTDOA2mp,axis=0)).reshape(RNnumTDOA,1)
RDoA=sqrt(RNTDOAmp2)-sqrt(RNTDOA2mp2)
TDoAStd=(sig1/c)*rand(RNnumTDOA,1)
TDoA=RDoA/c+TDoAStd*randn(RNnumTDOA,1)
shRN_RSS = shape(RN_RSS)
RNnumRSS = shRN_RSS[1]
RSSStd =sh*ones((RNnumRSS,1))
RSSnp= np*ones((RNnumRSS,1))
d0=1.0
S2=RSSLocation(RN_RSS)
PL0=pl0*ones((RNnumRSS,1))
RSS=S2.getPL(RN_RSS, P, PL0, d0, RSSnp, RSSStd)
shRN_TOA = shape(RN_TOA)
RNnumTOA = shRN_TOA[1]
RNTOAmp=RN_TOA-P
RNTOAmp2 = (sum(RNTOAmp*RNTOAmp,axis=0)).reshape(RNnumTOA,1)
RoA=sqrt(RNTOAmp2)
ToAStd=(sig/c)*rand(RNnumTOA,1)
ToA=RoA/c+ToAStd*randn(RNnumTOA,1)
P0=L*rand(2,1)
S1=HDFLocation(RN_RSS, RN_TOA, RN_TDOA)
S2=RSSLocation(RN_RSS)
P4rss=S2.MLDRSSLocate(P, P0, RN_RSS, PL0, d0, RSS, RSSnp, RSSStd)
MLrss_vect.append(dist(P4rss,P))
P4rss1toa=S1.MLHDFLocate(P, P0, RN_RSS, matrix(RN_TOA[:,0:1]), None, None, ToA[0:1,:], ToAStd[0:1,:], TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
MLrss1toa_vect.append(dist(P4rss1toa,P))
P4rss2toa=S1.MLHDFLocate(P, P0, RN_RSS, matrix(RN_TOA[:,0:2]), None, None, ToA[0:2,:], ToAStd[0:2,:], TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
MLrss2toa_vect.append(dist(P4rss2toa,P))
P4rss3toa=S1.MLHDFLocate(P, P0, RN_RSS, matrix(RN_TOA[:,0:3]), None, None, ToA[0:3,:], ToAStd[0:3,:], TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
MLrss3toa_vect.append(dist(P4rss3toa,P))
P4rss4toa=S1.MLHDFLocate(P, P0, RN_RSS, matrix(RN_TOA[:,0:4]), None, None, ToA[0:4,:], ToAStd[0:4,:], TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
MLrss4toa_vect.append(dist(P4rss4toa,P))
cdf(MLrss_vect,"k-","RSSI",2)
cdf(MLrss1toa_vect,"b-","RSSI + 1 TOA",2)
cdf(MLrss2toa_vect,"r-","RSSI + 2 TOA",2)
cdf(MLrss3toa_vect,"y-","RSSI + 3 TOA",2)
cdf(MLrss4toa_vect,"g-","RSSI + 4 TOA",2)
plt.legend(loc=4,numpoints=1)
#plt.axis([0,10,0,1])
plt.grid('on')
plt.xlabel("Positioning error (m)")
plt.ylabel("Cumulative probability")
plt.savefig("HDF_cdf_ML_RSS_nTOA.pdf", fromat="pdf")
plt.close()
ntoa=range(5)
err_moy=[mean(MLrss_vect),mean(MLrss1toa_vect),mean(MLrss2toa_vect),mean(MLrss3toa_vect),mean(MLrss4toa_vect)]
plt.plot(ntoa, err_moy, "ko-", linewidth=2)
plt.xlim=5
plt.grid('on')
plt.xlabel("Number of Added TOA")
plt.ylabel("Average Positioning Error")
plt.savefig("HDF_err_ML_RSS_nTOA.pdf", fromat="pdf")
plt.close()
def plot_cdf_SDP_All():
SDPrss_vect=[]
SDPrsstoa_vect=[]
SDPrsstdoa_vect=[]
SDPrsstoatdoa_vect=[]
for i in range(Ntrial):
print " ", Ntrial-i
P=L*rand(2,1)
shRN_TDOA = shape(RN_TDOA)
RNnumTDOA = shRN_TDOA[1]
RNTDOAmp=RN_TDOA-P
RNTDOA2mp=RN_TDOA2-P
RNTDOAmp2 = (sum(RNTDOAmp*RNTDOAmp,axis=0)).reshape(RNnumTDOA,1)
RNTDOA2mp2 = (sum(RNTDOA2mp*RNTDOA2mp,axis=0)).reshape(RNnumTDOA,1)
RDoA=sqrt(RNTDOAmp2)-sqrt(RNTDOA2mp2)
TDoAStd=(sig1/c)*rand(RNnumTDOA,1)
TDoA=RDoA/c+TDoAStd*randn(RNnumTDOA,1)
shRN_RSS = shape(RN_RSS)
RNnumRSS = shRN_RSS[1]
RSSStd =sh*ones((RNnumRSS,1))
RSSnp= np*ones((RNnumRSS,1))
d0=1.0
S2=RSSLocation(RN_RSS)
PL0=pl0*ones((RNnumRSS,1))
RSS=S2.getPL(RN_RSS, P, PL0, d0, RSSnp, RSSStd)
shRN_TOA = shape(RN_TOA)
RNnumTOA = shRN_TOA[1]
RNTOAmp=RN_TOA-P
RNTOAmp2 = (sum(RNTOAmp*RNTOAmp,axis=0)).reshape(RNnumTOA,1)
RoA=sqrt(RNTOAmp2)
ToAStd=(sig/c)*rand(RNnumTOA,1)
ToA=RoA/c+ToAStd*randn(RNnumTOA,1)
S1=HDFLocation(RN_RSS, RN_TOA, RN_TDOA)
S2=RSSLocation(RN_RSS)
P4rss=S2.SDPRSSLocate(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
SDPrss_vect.append(dist(P4rss,P))
P4rsstoa=S1.SDPHDFLocate(RN_RSS, RN_TOA, None, None, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
SDPrsstoa_vect.append(dist(P4rsstoa,P))
P4rsstdoa=S1.SDPHDFLocate(RN_RSS, None, RN_TDOA, RN_TDOA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
SDPrsstdoa_vect.append(dist(P4rsstdoa,P))
P4rsstoatdoa=S1.SDPHDFLocate(RN_RSS, RN_TOA, RN_TDOA, RN_TDOA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
SDPrsstoatdoa_vect.append(dist(P4rsstoatdoa,P))
cdf(SDPrss_vect,"k-","RSSI",2)
cdf(SDPrsstoa_vect,"b-","RSSI + TOA",2)
cdf(SDPrsstdoa_vect,"r-","RSSI + TDOA",2)
cdf(SDPrsstoatdoa_vect,"g-","RSSI + TOA + TDOA",2)
plt.legend(loc=4,numpoints=1)
plt.axis([0,10,0,1])
plt.grid('on')
plt.xlabel("Positioning error (m)")
plt.ylabel("Cumulative probability")
plt.savefig("HDF_cdf_SDP.pdf", fromat="pdf")
plt.close()
def plot_cdf_All():
for i in range(Ntrial):
print " ", Ntrial-i
P=L*rand(2,1)
shRN_TDOA = shape(RN_TDOA)
RNnumTDOA = shRN_TDOA[1]
RNTDOAmp=RN_TDOA-P
RNTDOA2mp=RN_TDOA2-P
RNTDOAmp2 = (sum(RNTDOAmp*RNTDOAmp,axis=0)).reshape(RNnumTDOA,1)
RNTDOA2mp2 = (sum(RNTDOA2mp*RNTDOA2mp,axis=0)).reshape(RNnumTDOA,1)
RDoA=sqrt(RNTDOAmp2)-sqrt(RNTDOA2mp2)
# TDoAStd=(sig1/c)*rand(RNnumTDOA,1)
TDoAStd=(sig1/c)*ones((RNnumTDOA,1))
TDoA=RDoA/c+TDoAStd*randn(RNnumTDOA,1)
shRN_RSS = shape(RN_RSS)
RNnumRSS = shRN_RSS[1]
RSSStd =sh*ones((RNnumRSS,1))
RSSnp= np*ones((RNnumRSS,1))
d0=1.0
S2=RSSLocation(RN_RSS)
PL0=pl0*ones((RNnumRSS,1))
print RN_RSS,'\n', P,'\n', PL0,'\n', d0,'\n', RSSnp,'\n', RSSStd
RSS=S2.getPL(RN_RSS, P, PL0, d0, RSSnp, RSSStd)
shRN_TOA = shape(RN_TOA)
RNnumTOA = shRN_TOA[1]
RNTOAmp=RN_TOA-P
RNTOAmp2 = (sum(RNTOAmp*RNTOAmp,axis=0)).reshape(RNnumTOA,1)
RoA=sqrt(RNTOAmp2)
# ToAStd=(sig/c)*rand(RNnumTOA,1)
ToAStd=(sig/c)*ones((RNnumTOA,1))
ToA=RoA/c+ToAStd*randn(RNnumTOA,1)
print '############### RSS ##################'
print 'RNRSS\n',RN_RSS
print 'PL0\n',PL0
print 'd0\n', d0
print 'RSS\n', RSS
print 'RSSnp\n', RSSnp
print 'RSSSTD\n',RSSStd
print '############### TOA ##################'
print 'RNTOA\n', RN_TOA
print 'ToA\n',ToA
print 'ToAStd\n', ToAStd
# print '############### TDOA ##################'
# print 'RNTDOA\n', RN_TDOA
# print 'RNTDOA_ref\n', RN_TDOA2
# print 'TDOA\n', TDoA*1e-9
# print 'TDOASTD\n', TDoAStd*1e-9
S1=HDFLocation(RN_RSS, RN_TOA, RN_TDOA)
'''P1=S1.LSHDFLocate(RN_RSS, RN_TOA, RN_TDOA, RN_TDOA2, ToA, TDoA, PL0, d0, RSS, RSSnp, RSSStd, Rest)
LS_vect.append(dist(P1,P))
P1tls=S1.TLSHDFLocate(RN_RSS, RN_TOA, RN_TDOA, RN_TDOA2, ToA, TDoA, PL0, d0, RSS, RSSnp, RSSStd, Rest)
TLS_vect.append(dist(P1tls,P))
P2=S1.WLSHDFLocate(RN_RSS, RN_TOA, RN_TDOA, RN_TDOA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
WLS_vect.append(dist(P2,P))'''
P2twls=S1.TWLSHDFLocate(RN_RSS, RN_TOA, RN_TDOA, RN_TDOA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
TWLS_vect.append(dist(P2twls,P))
P0=L*rand(2,1)
P4=S1.MLHDFLocate(P, P0, RN_RSS, RN_TOA, RN_TDOA, RN_TDOA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
ML_vect.append(dist(P4,P))
P5=S1.SDPHDFLocate(RN_RSS, RN_TOA, RN_TDOA, RN_TDOA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest)
SDP_vect.append(dist(P5,P))
'''crb=S1.CRBHDFLocate(P, RN_RSS, RN_TOA, RN_TDOA, RN_TDOA2, ToAStd, TDoAStd, PL0, d0, RSSnp, RSSStd, Rest)
crb_vect.append(sqrt(crb))'''
#cdf(LS_vect,"k-","LS",2)
#cdf(TLS_vect,"k-","TLS",2)
#cdf(WLS_vect,"r-","WLS",2)
cdf(ML_vect,"g-","ML",2)
cdf(SDP_vect,"b--","SDP",2)
cdf(TWLS_vect,"r-.","TWLS",2)
#cdf(crb_vect,"g-.","CRLB",2)
plt.legend(loc=4,numpoints=1)
plt.axis([0,10,0,1])
plt.grid('on')
plt.xlabel("Positioning error (m)")
plt.ylabel("Cumulative probability")
plt.savefig("HDF_cdf_RSS_TOA_TDOA.pdf", fromat="pdf")
plt.close()
def run(self):
"""
run : run simulation of the scenario
"""
self.CRB=[]
Nbn = len(self.bn)
Nan = len(self.an)
if self.parmsc['save_pe']:
self.pe = []
self.p_LS = []
self.p_LSci = []
self.p_LScr = []
if self.parmsc['save_CLA']:
self.CLA = []
self.err1 = np.array([])
#self.err2 = np.array([])
self.errx = np.array([])
self.erry = np.array([])
self.errz = np.array([])
self.errLS = np.array([])
self.errLSci = np.array([])
self.errLScr = np.array([])
self.errxLS = np.array([])
self.erryLS = np.array([])
self.errzLS = np.array([])
# list for algebraic
atoabn = []
astdbn = []
P = []
if self.parmsc['Constrain_Type']=='TDOA':
lbcl=Nan-1
else :
lbcl=Nan
tdoa_idx = nonzero(np.array(l_connect)=='TDOA')[0]
if self.parmsc['Constrain_Type']=='hybrid' or self.parmsc['Constrain_Type']=='test':
algehyb=1
else :
algehyb=0
if len(tdoa_idx) != 0:
lbcl=Nan-1
self.rss_idx = nonzero(np.array(l_connect)=='RSS')[0]
self.toa_idx = nonzero(np.array(l_connect)=='TOA')[0]
self.tdoa_idx = nonzero(np.array(l_connect)=='TDOA')[0]
self.ERRSAVE=[]
self.trgpa=[]
self.talg=[]
for ibn in range(Nbn): # for each blind node
self.errRSS=zeros((1))
self.errTOA=zeros((1))
self.errTDOA=zeros((1))
self.ibn=ibn
errli = []
Constraint.C_Id = 0 # reset constraint Id for each BN
print " Blind Node N°",ibn+1,"/",Nbn
atv=[]
pbn = self.bn[ibn]
#print "Blind Node N° ",ibn,pbn
self.tic_ensemblist=time.time()
cla = CLA(self.parmsh)
cla.bn = self.bn[ibn]
clarss = CLA(self.parmsh)
clatoa = CLA(self.parmsh)
clatdoa = CLA(self.parmsh)
if parmsc['exclude_out'] != None :
E = Exclude(nodes=parmsc['exclude_out'])
cla.append(E)
for ian in range(lbcl): # for each AN or couple of AN (TDOA)
TC=0
try :
self.parmsc['Constrain_Type'] = self.parmsc['l_connect'][ian]
except :
pass
####################### TOA : Geometric description
if self.parmsc['Constrain_Type']=='TOA':
pan = self.an[ian]
# tvalue : true value (ns)
tvalue = np.sqrt(np.dot(pan-pbn,pan-pbn))/3e8
# err (ns)
err = (self.std_v[ibn,ian]*sp.randn())
while err + tvalue < 0:
err = (self.std_v[ibn,ian]*sp.randn())
self.ERRSAVE.append(err)
self.errTOA=np.vstack((self.errTOA,err))
# value (toa : ns )
value = max(0,tvalue+err)
TC1=time.time()
C = TOA(value=value*1e9,
std=self.std_v[ibn,ian]*1e9,
vcw=self.parmsc['vcw'],
p=pan)
cla.append(C)
TC2=time.time()
clatoa.append(C)
####################### TDOA : Geometric description
if self.parmsc['Constrain_Type']=='TDOA':
pan = vstack((self.an[tdoa_idx[0]],self.an[ian+1]))
# dan : delay between 2 AN (ns)
dan = np.sqrt(dot(pan[0]-pan[1],pan[0]-pan[1]))/3e8
minvalue = -dan
maxvalue = dan
toa1 = np.sqrt(np.dot(pan[0]-pbn,pan[0]-pbn))/3e8
toa2 = np.sqrt(np.dot(pan[1]-pbn,pan[1]-pbn))/3e8
tvalue = toa2-toa1
err = self.std_v[ibn,ian]*sp.randn()
while ((tvalue + err) < minvalue) or ((tvalue + err) > maxvalue):
err = self.std_v[ibn,ian]*sp.randn()
self.ERRSAVE.append(err)
self.errTDOA=np.vstack((self.errTDOA,err))
# tvalue : true value (ns)
# value = max(minvalue,tvalue + err)
# value = min(maxvalue,value)
# value (ns)
value = tvalue+err
TC1=time.time()
C = TDOA(value=-value*1e9,
std=(self.std_v[ibn,ian]*1e9),
vcw=self.parmsc['vcw'],
p = pan)
cla.append(C)
TC2=time.time()
#C = TDOA(p=pan,value=value,std=2)
clatdoa.append(C)
####################### RSS : Geometric description
if self.parmsc['Constrain_Type']=='RSS':
pan = self.an[ian]
d0 = self.parmsc['d0']
RSSnp = vstack((self.parmsc['pn'],self.parmsc['pn']))
PL0= vstack((self.parmsc['rss0'],self.parmsc['rss0']))
RSSStd= vstack((self.parmsc['sigma_max_RSS'],self.parmsc['sigma_max_RSS']))
PP= vstack((self.bn[ibn],self.bn[ibn]))
PA= vstack((pan,pan))
rssloc = RSSLocation(PP)
value = (rssloc.getPLmean(PA.T, PP.T, PL0, d0, RSSnp))
err = (RSSStd*randn(shape(value)[0],shape(value)[1]))[0][0]
self.ERRSAVE.append(err)
self.errRSS=np.vstack((self.errRSS,err))
value = value[0] + err
# value = rssloc.getPL(PA.T, PP.T, PL0, d0, RSSnp, RSSStd)
value = value
# value = self.parmsc['rss0']-10*self.parmsc['pn']*log10(dr/self.parmsc['d0'])+self.err_v[ibn,ian]
self.Model = {}
self.Model['PL0'] =self.parmsc['rss0']
self.Model['d0'] = self.parmsc['d0']
self.Model['RSSnp'] = self.parmsc['pn']
self.Model['RSSStd'] = self.parmsc['sigma_max_RSS']
self.Model['Rest'] = 'mode'
TC1=time.time()
C = RSS(value=value,
std=self.std_v[ibn,ian],
vcw=self.parmsc['vcw'],
model=self.Model,
p=pan )
cla.append(C)
TC2=time.time()
clarss.append(C)
TC=TC+(TC2-TC1)
if self.parmsc['algebraic']:
atv.append(value)
if len(self.rss_idx) != 0:
self.errRSS = delete(self.errRSS,0,0)
if len(self.toa_idx) != 0:
self.errTOA = delete(self.errTOA,0,0)
if len(self.tdoa_idx) != 0:
self.errTDOA = delete(self.errTDOA,0,0)
# version boite recursive
#
######################### CLA TOTALE
TR1=time.time()
cla.merge2()
cla.refine(cla.Nc)
self.cla = cla
### DoubleListRefine version
cla.estpos2()
pe1=cla.pe
TR2=time.time()
self.trgpa.append((TR2-TR1)+TC)
self.pe.append(pe1)
errli.append(np.sqrt(np.dot(pe1[:2]-pbn[:2],pe1[:2]-pbn[:2])))
err1=min(errli)
self.err1 = np.hstack((self.err1,err1))
if self.parmsc['algebraic']:
if algehyb==1:
self.parmsc['Constrain_Type']='hybrid'
p_LS = self.algebraic_compute(atv,None)
# p_LSci = self.algebraic_compute(atv,self.pe[-1])
# p_LScr = self.algebraic_compute(atv,self.bn[ibn])
if self.parmsc['save_pe']:
self.p_LS.append(p_LS)
# self.p_LSci.append(p_LSci)
# self.p_LScr.append(p_LScr)
if self.parmsc['Constrain_Type'] != 'hybrid':
errLS = np.sqrt(np.dot(p_LS[:2]-pbn[:2],p_LS[:2]-pbn[:2]))
# errLSci = np.sqrt(np.dot(p_LSci[:2]-pbn[:2],p_LSci[:2]-pbn[:2]))
# errLScr = np.sqrt(np.dot(p_LScr[:2]-pbn[:2],p_LScr[:2]-pbn[:2]))
else :
errLS = np.sqrt(np.dot(p_LS[:2]-pbn[:2],p_LS[:2]-pbn[:2]))
# errLSci = np.sqrt(np.dot(p_LSci[:2]-pbn[:2],p_LSci[:2]-pbn[:2]))
# errLScr = np.sqrt(np.dot(p_LScr[:2]-pbn[:2],p_LScr[:2]-pbn[:2]))
self.errLS = np.hstack((self.errLS,errLS))
# self.errLSci = np.hstack((self.errLSci,errLSci))
# self.errLScr = np.hstack((self.errLSci,errLScr))
if errli > errLS:
print 'NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNnn\n',errli,'\n',errLS
def algebraic_compute(self, atv):
rss_idx = self.rss_idx
toa_idx = self.toa_idx
tdoa_idx = self.tdoa_idx
P = []
#
# print 'RSS_idx',rss_idx
# print 'TOA_idx',toa_idx
# print 'TDOA_idx',tdoa_idx
############### RSS ##############
RN_RSS = zeros(3)
Rss = zeros(1)
RSSStd = zeros(1)
d0 = self.parmsc['d0']
RSSnp = self.parmsc['pn'] * ones((len(self.an[rss_idx]), 1))
PL0 = self.parmsc['rss0'] * ones((len(self.an[rss_idx]), 1))
RSSStd = self.parmsc['sigma_max_RSS'] * ones(
(len(self.an[rss_idx]), 1))
if len(rss_idx) != 0:
for i in rss_idx:
aa = np.array(self.an[i])
ss = np.array(atv[i])
RN_RSS = np.vstack((RN_RSS, aa))
Rss = np.vstack((Rss, ss))
RN_RSS = np.delete(RN_RSS, 0, 0).T
RN_RSS = RN_RSS[:2]
Rss = np.delete(Rss, 0, 0)
else:
RN_RSS = None
############### TOA ##############
RN_TOA = zeros(3)
ToA = zeros(1)
ToAStd = zeros(1)
if len(toa_idx) != 0:
for i in toa_idx:
aa = np.array(self.an[i])
ss = np.array(atv[i])
tt = np.array(self.std_v[self.ibn, i])
RN_TOA = np.vstack((RN_TOA, aa))
ToA = np.vstack((ToA, ss))
ToAStd = np.vstack((ToAStd, tt))
RN_TOA = np.delete(RN_TOA, 0, 0).T
RN_TOA = RN_TOA[:2]
ToA = np.delete(ToA, 0, 0)
ToAStd = np.delete(ToAStd, 0, 0)
else:
RN_TOA = None
############### TDOA ##############
RN_TDOA = zeros(3)
RN_TDOA_ref = zeros(3)
TDoA = zeros(1)
TDoAStd = zeros(1)
if len(tdoa_idx) != 0:
#RN_TDOA=zeros(3)
#for i in tdoa_idx:
# aa=np.array(self.an[i])
# RN_TDOA=np.vstack((RN_TDOA,aa))
RN_TDOA = (self.an[tdoa_idx[1:]]).T
RN_TDOA_ref = (self.an[tdoa_idx[0]] * ones(np.shape(RN_TDOA))).T
for i in tdoa_idx[0:-1]:
ss = np.array(atv[i])
tt = np.array(self.std_v[self.ibn, i])
TDoA = np.vstack((TDoA, ss))
TDoAStd = np.vstack((TDoAStd, tt))
TDoA = np.delete(TDoA, 0, 0)
TDoAStd = np.delete(TDoAStd, 0, 0)
RN_TDOA = RN_TDOA[:2]
RN_TDOA_ref = RN_TDOA_ref[:2]
else:
RN_TDOA = None
RN_TDOA_ref = None
# if RN_RSS != None :
# print '############### RSS ##################'
# print 'RNRSS\n',RN_RSS
# print 'PL0\n',PL0
# print 'd0\n', d0
# print 'RSS\n', Rss
# print 'RSSnp\n', RSSnp
# print 'RSSSTD\n',RSSStd
# if RN_TOA != None :
# print '############## TOA ##################'
# print 'RNTOA\n', RN_TOA
# print 'ToA\n',ToA
# print 'ToAStd\n', ToAStd
# if RN_TDOA != None :
# print '############### TDOA ##################'
# print 'RNTDOA\n', RN_TDOA
# print 'RNTDOA_ref\n', RN_TDOA_ref
# print 'TDOA\n', TDoA
# print 'TDOASTD\n', TDoAStd
#
self.tic_algebric = time.time()
S1 = HDFLocation(RN_RSS, RN_TOA, RN_TDOA)
S2 = RSSLocation(RN_RSS)
S3 = ToALocation(RN_TOA)
S4 = TDoALocation(RN_TDOA)
if self.parmsc['Algebraic_method'] == 'LS':
print 'to be implemented'
elif self.parmsc['Algebraic_method'] == 'TLS':
print 'to be implemented'
elif self.parmsc['Algebraic_method'] == 'WLS':
print 'to be implemented'
elif self.parmsc['Algebraic_method'] == 'TWLS':
if RN_RSS == None and RN_TOA == None:
P = S4.TWLSTDoALocate(RN_TDOA, RN_TDOA_ref, TDoA, TDoAStd)
elif RN_RSS == None and RN_TDOA == None:
P = S3.TWLSToALocate(RN_TOA, ToA, ToAStd)
elif RN_TOA == None and RN_TDOA == None:
P = S2.TWLSRSSLocate(RN_RSS, PL0, d0, Rss, RSSnp, RSSStd,
'mode')
else:
P = S1.TWLSHDFLocate(RN_RSS, RN_TOA, RN_TDOA, RN_TDOA_ref, ToA,
ToAStd, TDoA, TDoAStd, PL0, d0, Rss,
RSSnp, RSSStd, 'mode')
elif self.parmsc['Algebraic_method'] == 'ML':
PP = L * rand(2, 1) # for 3D replace by L*rand(3,1)
P0 = L * rand(2, 1) # for 3D replace by L*rand(3,1)
# P0[2]=0.0 # for 3D uncomment
if RN_RSS == None and RN_TOA == None:
P = S4.MLTDoALocate(PP, P0, RN_TDOA, RN_TDOA_ref, TDoA,
TDoAStd)
elif RN_RSS == None and RN_TDOA == None:
P = S3.MLToALocate(PP, P0, RN_TOA, ToA, ToAStd)
elif RN_TOA == None and RN_TDOA == None:
P = S2.MLDRSSLocate(PP, P0, RN_RSS, PL0, d0, Rss, RSSnp,
RSSStd)
else:
P = S1.MLHDFLocate(PP, P0, RN_RSS, RN_TOA, RN_TDOA,
RN_TDOA_ref, ToA, ToAStd, TDoA, TDoAStd,
PL0, d0, Rss, RSSnp, RSSStd, 'mode')
elif self.parmsc['Algebraic_method'] == 'SDP':
if RN_RSS == None and RN_TOA == None:
P = S4.SDPTDoALocate(RN_TDOA, RN_TDOA_ref, TDoA, TDoAStd)
elif RN_RSS == None and RN_TDOA == None:
P = S3.SDPToALocate(RN_TOA, ToA, ToAStd)
elif RN_TOA == None and RN_TDOA == None:
P = S2.SDPRSSLocate(RN_RSS, PL0, d0, -Rss, RSSnp, RSSStd,
'mode')
else:
P = S1.SDPHDFLocate(RN_RSS, RN_TOA, RN_TDOA, RN_TDOA_ref, ToA,
ToAStd, TDoA, TDoAStd, PL0, d0, Rss, RSSnp,
RSSStd, 'mode')
else:
print "You have to chose the self.parmsc['Algebraic_method'] between those choices :\n LS,TLS,WLS,TWLS,ML,SDP "
if self.parmsc['CRB']:
CRBL = CRBLocation(None)
if len(rss_idx) != 0:
RSSStdX = self.errRSS #[rss_idx]#*ones(len(self.an[rss_idx]))
if len(toa_idx) != 0:
TOAStdX = self.errTOA #[toa_idx]#*ones((len(self.an[toa_idx]),1))
if len(tdoa_idx) != 0:
TDOAStdX = self.errTDOA #[tdoa_idx]#*ones((len(self.an[tdoa_idx])-1,1))
PP = self.bn[self.ibn, :2]
###################################### RSS PUR
if RN_TOA == None and RN_TDOA == None:
print 'RSS'
self.CRB.append(
sqrt(CRBL.CRB_RSS_fim(PP, RN_RSS, RSSnp, RSSStdX)))
###################################### TOA PUR
elif RN_RSS == None and RN_TDOA == None: # TOA
print 'TOA CRB'
self.CRB.append(sqrt(CRBL.CRB_TOA_fim(PP, RN_TOA, TOAStdX)))
###################################### TDOA PUR
elif RN_RSS == None and RN_TOA == None: # TDOA
print 'TDOA'
self.CRB.append(
sqrt(CRBL.CRB_TDOA_fim(PP, RN_TDOA, RN_TDOA_ref,
TDOAStdX)))
elif RN_TOA == None and RN_TDOA != None:
###################################### TDOA
if RN_RSS == None:
print 'TDOA2'
self.CRB.append(
sqrt(CRBL.CRB_TDOA_fim(PP, RN_RSS, PL0, RSSStdX)))
###################################### RSS + TDOA
else:
print 'RSS+TDOA'
self.CRB.append(
sqrt(
CRBL.CRB_RSS_TDOA_fim(PP, RN_RSS, RN_TDOA,
RN_TDOA_ref, RSSnp, RSSStdX,
TDOAStdX)))
elif RN_TOA != None and RN_TDOA == None:
##################################### TOA
if RN_RSS == None:
print 'TOA'
self.CRB.append(sqrt(CRBL.CRB_TOA_fim(PP, RN_TOA,
TOAStdX)))
##################################### RSS + TOA
else:
print 'RSS + TOA'
self.CRB.append(
sqrt(
CRBL.CRB_RSS_TOA_fim(PP, RN_RSS, RN_TOA, RSSnp,
RSSStdX, TOAStdX)))
elif RN_TOA != None and RN_TDOA != None:
##################################### TOA+TDOA
if RN_RSS == None:
print 'TOA + TDOA'
self.CRB.append(
sqrt(
CRBL.CRB_TOA_TDOA_fim(PP, RN_TOA, RN_TDOA,
RN_TDOA_ref, TOAStdX,
TDOAStdX)))
##################################### RSS+TOA+TDOA
else:
print 'RSS + TOA + TDOA'
self.CRB.append(
sqrt(
CRBL.CRB_RSS_TOA_TDOA_fim(PP, RN_RSS, RN_TOA,
RN_TDOA, RN_TDOA_ref,
RSSnp, RSSStdX, TOAStdX,
TDOAStdX)))
P = array(P)
return P[:, 0]
def run(self):
"""
run : run simulation of the scenario
"""
self.time_compute = {}
self.time_compute['RGPA'] = []
self.time_compute['algebraic'] = []
self.CRB = []
Nbn = len(self.bn)
Nan = len(self.an)
if self.parmsc['save_pe']:
self.pe = []
self.p_LS = []
if self.parmsc['save_CLA']:
self.CLA = []
self.err1 = np.array([])
#self.err2 = np.array([])
self.errx = np.array([])
self.erry = np.array([])
self.errz = np.array([])
self.errLS = np.array([])
self.errxLS = np.array([])
self.erryLS = np.array([])
self.errzLS = np.array([])
# list for algebraic
atoabn = []
astdbn = []
P = []
if self.parmsc['Constrain_Type'] == 'TDOA':
lbcl = Nan - 1
else:
lbcl = Nan
tdoa_idx = nonzero(np.array(l_connect) == 'TDOA')[0]
if self.parmsc['Constrain_Type'] == 'hybrid' or self.parmsc[
'Constrain_Type'] == 'test':
algehyb = 1
else:
algehyb = 0
if len(tdoa_idx) != 0:
lbcl = Nan - 1
#self.dan=[]
#for i in range(len(tdoa_idx)-1):
# self.dan.append(vstack((self.an[tdoa_idx[0]],self.an[i+1])))
self.rss_idx = nonzero(np.array(l_connect) == 'RSS')[0]
self.toa_idx = nonzero(np.array(l_connect) == 'TOA')[0]
self.tdoa_idx = nonzero(np.array(l_connect) == 'TDOA')[0]
self.ERRSAVE = []
for ibn in range(Nbn): # for each blind node
self.errRSS = zeros((1))
self.errTOA = zeros((1))
self.errTDOA = zeros((1))
self.ibn = ibn
errli = []
Constraint.C_Id = 0 # reset constraint Id for each BN
print " Blind Node N°", ibn + 1, "/", Nbn
atv = []
pbn = self.bn[ibn]
#print "Blind Node N° ",ibn,pbn
self.tic_ensemblist = time.time()
cla = CLA(self.parmsh)
cla.bn = self.bn[ibn]
clarss = CLA(self.parmsh)
clatoa = CLA(self.parmsh)
clatdoa = CLA(self.parmsh)
#cla.C_Id=0
if parmsc['exclude_out'] != None:
E = Exclude(nodes=parmsc['exclude_out'])
cla.append(E)
for ian in range(lbcl): # for each AN or couple of AN (TDOA)
#print "Anchor Node N° ",ian
try:
self.parmsc['Constrain_Type'] = self.parmsc['l_connect'][
ian]
except:
pass
# pdb.set_trace()
rgpatimea = time.time()
if self.parmsc['Constrain_Type'] == 'TOA':
pan = self.an[ian]
# tvalue : true value (ns)
tvalue = np.sqrt(np.dot(pan - pbn, pan - pbn)) / 3e8
# err (ns)
err = (self.std_v[ibn, ian] * sp.randn())
while err + tvalue < 0:
err = (self.std_v[ibn, ian] * sp.randn())
self.errTOA = np.vstack((self.errTOA, err))
# value (toa : ns )
value = max(0, tvalue + err)
C = TOA(value=value * 1e9,
std=self.std_v[ibn, ian] * 1e9,
vcw=self.parmsc['vcw'],
p=pan)
cla.append(C)
clatoa.append(C)
if self.parmsc['Constrain_Type'] == 'TDOA':
pan = vstack((self.an[tdoa_idx[0]], self.an[ian + 1]))
# dan : delay between 2 AN (ns)
dan = np.sqrt(dot(pan[0] - pan[1], pan[0] - pan[1])) / 3e8
minvalue = -dan
maxvalue = dan
toa1 = np.sqrt(np.dot(pan[0] - pbn, pan[0] - pbn)) / 3e8
toa2 = np.sqrt(np.dot(pan[1] - pbn, pan[1] - pbn)) / 3e8
tvalue = toa2 - toa1
err = self.std_v[ibn, ian] * sp.randn()
while ((tvalue + err) < minvalue) or (
(tvalue + err) > maxvalue):
err = self.std_v[ibn, ian] * sp.randn()
self.errTDOA = np.vstack((self.errTDOA, err))
# tvalue : true value (ns)
# value = max(minvalue,tvalue + err)
# value = min(maxvalue,value)
# value (ns)
value = tvalue + err
C = TDOA(value=-value * 1e9,
std=(self.std_v[ibn, ian] * 1e9),
vcw=self.parmsc['vcw'],
p=pan)
cla.append(C)
#C = TDOA(p=pan,value=value,std=2)
clatdoa.append(C)
if self.parmsc['Constrain_Type'] == 'RSS':
pan = self.an[ian]
######################################## MODEL RSS NICO
# dr = max(0, (np.sqrt(np.dot(pan-pbn,pan-pbn))))
# M = Model()
# err = (self.std_v[ibn,ian]*1e-9*sp.randn())
# value = M.OneSlope(max(0,dr + err))
# value = min(500,value)
# value = max(-500,value)
######################################## MOHAMED
d0 = self.parmsc['d0']
RSSnp = vstack((self.parmsc['pn'], self.parmsc['pn']))
PL0 = vstack((self.parmsc['rss0'], self.parmsc['rss0']))
RSSStd = vstack((self.parmsc['sigma_max_RSS'],
self.parmsc['sigma_max_RSS']))
PP = vstack((self.bn[ibn], self.bn[ibn]))
PA = vstack((pan, pan))
rssloc = RSSLocation(PP)
value = (rssloc.getPLmean(PA.T, PP.T, PL0, d0, RSSnp))
err = (RSSStd * randn(shape(value)[0],
shape(value)[1]))[0][0]
self.errRSS = np.vstack((self.errRSS, err))
value = value[0] + err
# value = rssloc.getPL(PA.T, PP.T, PL0, d0, RSSnp, RSSStd)
value = value
# value = self.parmsc['rss0']-10*self.parmsc['pn']*log10(dr/self.parmsc['d0'])+self.err_v[ibn,ian]
self.Model = {}
self.Model['PL0'] = self.parmsc['rss0']
self.Model['d0'] = self.parmsc['d0']
self.Model['RSSnp'] = self.parmsc['pn']
self.Model['RSSStd'] = self.parmsc['sigma_max_RSS']
self.Model['Rest'] = 'mode'
C = RSS(value=value,
std=self.std_v[ibn, ian],
vcw=self.parmsc['vcw'],
model=self.Model,
p=pan)
cla.append(C)
clarss.append(C)
if self.parmsc['algebraic']:
atv.append(value)
if len(self.rss_idx) != 0:
self.errRSS = delete(self.errRSS, 0, 0)
if len(self.toa_idx) != 0:
self.errTOA = delete(self.errTOA, 0, 0)
if len(self.tdoa_idx) != 0:
self.errTDOA = delete(self.errTDOA, 0, 0)
# version boite recursive
#
######################### CLA TOTALE
cla.merge2()
cla.refine(cla.Nc)
self.cla = cla
### DoubleListRefine version
cla.estpos2()
pe1 = cla.pe
rgpatimeb = time.time()
self.time_compute['RGPA'].append(rgpatimeb - rgpatimea)
self.pe.append(pe1)
errli.append(np.sqrt(np.dot(pe1[:2] - pbn[:2], pe1[:2] - pbn[:2])))
#print len(parmsc['l_connect'])
if len(parmsc['l_connect']
) > 4: # pour ne pas calculer 2fois les cas non hybrides
if len(nonzero(
np.array(parmsc['l_connect']) == 'RSS')[0]) != 0:
for i in range(4):
clarss.c[i].Id = i
clarss.merge2()
clarss.refine(clarss.Nc)
clarss.estpos2()
clarss.bn = bn[ibn]
self.clarss = clarss
self.perss = clarss.pe
errli.append(
np.sqrt(
np.dot(self.perss[:2] - pbn[:2],
self.perss[:2] - pbn[:2])))
if len(nonzero(
np.array(parmsc['l_connect']) == 'TOA')[0]) != 0:
for i in range(4):
clatoa.c[i].Id = i
clatoa.merge2()
clatoa.refine(clatoa.Nc)
clatoa.estpos2()
clatoa.bn = bn[ibn]
self.clatoa = clatoa
self.petoa = clatoa.pe
errli.append(
np.sqrt(
np.dot(self.petoa[:2] - pbn[:2],
self.petoa[:2] - pbn[:2])))
if len(nonzero(
np.array(parmsc['l_connect']) == 'TDOA')[0]) != 0:
for i in range(3):
clatdoa.c[i].Id = i
clatdoa.merge2()
clatdoa.refine(clatdoa.Nc)
clatdoa.estpos2()
clatdoa.bn = bn[ibn]
self.clatdoa = clatdoa
self.petdoa = clatdoa.pe
errli.append(
np.sqrt(
np.dot(self.petdoa[:2] - pbn[:2],
self.petdoa[:2] - pbn[:2])))
#print errli
err1 = min(errli)
#print err1
self.err1 = np.hstack((self.err1, err1))
#self.err2 = np.hstack((self.err2,err2))
#if err >3:
# pdb.set_trace()
if self.parmsc['algebraic']:
if algehyb == 1:
self.parmsc['Constrain_Type'] = 'hybrid'
algetimea = time.time()
p_LS = self.algebraic_compute(atv)
algetimeb = time.time()
self.time_compute['algebraic'].append(algetimeb - algetimea)
if self.parmsc['save_pe']:
self.p_LS.append(p_LS)
if self.parmsc['Constrain_Type'] != 'hybrid':
errLS = np.sqrt(
np.dot(p_LS[:2] - pbn[:2], p_LS[:2] - pbn[:2]))
#elif self.parmsc['Algebraic_method'] == 'CRB':
# errLS = np.sqrt(self.CRB)
else:
errLS = np.sqrt(
np.dot(p_LS[:2] - pbn[:2], p_LS[:2] - pbn[:2]))
self.errLS = np.hstack((self.errLS, errLS))
if errli > errLS:
print 'NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNnn\n', errli, '\n', errLS
def run(self):
"""
run : run simulation of the scenario
"""
self.time_compute={}
self.time_compute['RGPA']=[]
self.time_compute['algebraic']=[]
self.CRB=[]
Nbn = len(self.bn)
Nan = len(self.an)
if self.parmsc['save_pe']:
self.pe = []
self.p_LS = []
if self.parmsc['save_CLA']:
self.CLA = []
self.err1 = np.array([])
#self.err2 = np.array([])
self.errx = np.array([])
self.erry = np.array([])
self.errz = np.array([])
self.errLS = np.array([])
self.errxLS = np.array([])
self.erryLS = np.array([])
self.errzLS = np.array([])
# list for algebraic
atoabn = []
astdbn = []
P = []
if self.parmsc['Constrain_Type']=='TDOA':
lbcl=Nan-1
else :
lbcl=Nan
tdoa_idx = nonzero(np.array(l_connect)=='TDOA')[0]
if self.parmsc['Constrain_Type']=='hybrid' or self.parmsc['Constrain_Type']=='test':
algehyb=1
else :
algehyb=0
if len(tdoa_idx) != 0:
lbcl=Nan-1
#self.dan=[]
#for i in range(len(tdoa_idx)-1):
# self.dan.append(vstack((self.an[tdoa_idx[0]],self.an[i+1])))
self.rss_idx = nonzero(np.array(l_connect)=='RSS')[0]
self.toa_idx = nonzero(np.array(l_connect)=='TOA')[0]
self.tdoa_idx = nonzero(np.array(l_connect)=='TDOA')[0]
self.ERRSAVE=[]
for ibn in range(Nbn): # for each blind node
self.errRSS=zeros((1))
self.errTOA=zeros((1))
self.errTDOA=zeros((1))
self.ibn=ibn
errli = []
Constraint.C_Id = 0 # reset constraint Id for each BN
print " Blind Node N°",ibn+1,"/",Nbn
atv=[]
pbn = self.bn[ibn]
#print "Blind Node N° ",ibn,pbn
self.tic_ensemblist=time.time()
cla = CLA(self.parmsh)
cla.bn = self.bn[ibn]
clarss = CLA(self.parmsh)
clatoa = CLA(self.parmsh)
clatdoa = CLA(self.parmsh)
#cla.C_Id=0
if parmsc['exclude_out'] != None :
E = Exclude(nodes=parmsc['exclude_out'])
cla.append(E)
for ian in range(lbcl): # for each AN or couple of AN (TDOA)
#print "Anchor Node N° ",ian
try :
self.parmsc['Constrain_Type'] = self.parmsc['l_connect'][ian]
except :
pass
# pdb.set_trace()
rgpatimea=time.time()
if self.parmsc['Constrain_Type']=='TOA':
pan = self.an[ian]
# tvalue : true value (ns)
tvalue = np.sqrt(np.dot(pan-pbn,pan-pbn))/3e8
# err (ns)
err = (self.std_v[ibn,ian]*sp.randn())
while err + tvalue < 0:
err = (self.std_v[ibn,ian]*sp.randn())
self.errTOA=np.vstack((self.errTOA,err))
# value (toa : ns )
value = max(0,tvalue+err)
C = TOA(value=value*1e9,
std=self.std_v[ibn,ian]*1e9,
vcw=self.parmsc['vcw'],
p=pan)
cla.append(C)
clatoa.append(C)
if self.parmsc['Constrain_Type']=='TDOA':
pan = vstack((self.an[tdoa_idx[0]],self.an[ian+1]))
# dan : delay between 2 AN (ns)
dan = np.sqrt(dot(pan[0]-pan[1],pan[0]-pan[1]))/3e8
minvalue = -dan
maxvalue = dan
toa1 = np.sqrt(np.dot(pan[0]-pbn,pan[0]-pbn))/3e8
toa2 = np.sqrt(np.dot(pan[1]-pbn,pan[1]-pbn))/3e8
tvalue = toa2-toa1
err = self.std_v[ibn,ian]*sp.randn()
while ((tvalue + err) < minvalue) or ((tvalue + err) > maxvalue):
err = self.std_v[ibn,ian]*sp.randn()
self.errTDOA=np.vstack((self.errTDOA,err))
# tvalue : true value (ns)
# value = max(minvalue,tvalue + err)
# value = min(maxvalue,value)
# value (ns)
value = tvalue+err
C = TDOA(value=-value*1e9,
std=(self.std_v[ibn,ian]*1e9),
vcw=self.parmsc['vcw'],
p = pan)
cla.append(C)
#C = TDOA(p=pan,value=value,std=2)
clatdoa.append(C)
if self.parmsc['Constrain_Type']=='RSS':
pan = self.an[ian]
######################################## MODEL RSS NICO
# dr = max(0, (np.sqrt(np.dot(pan-pbn,pan-pbn))))
# M = Model()
# err = (self.std_v[ibn,ian]*1e-9*sp.randn())
# value = M.OneSlope(max(0,dr + err))
# value = min(500,value)
# value = max(-500,value)
######################################## MOHAMED
d0 = self.parmsc['d0']
RSSnp = vstack((self.parmsc['pn'],self.parmsc['pn']))
PL0= vstack((self.parmsc['rss0'],self.parmsc['rss0']))
RSSStd= vstack((self.parmsc['sigma_max_RSS'],self.parmsc['sigma_max_RSS']))
PP= vstack((self.bn[ibn],self.bn[ibn]))
PA= vstack((pan,pan))
rssloc = RSSLocation(PP)
value = (rssloc.getPLmean(PA.T, PP.T, PL0, d0, RSSnp))
err = (RSSStd*randn(shape(value)[0],shape(value)[1]))[0][0]
self.errRSS=np.vstack((self.errRSS,err))
value = value[0] + err
# value = rssloc.getPL(PA.T, PP.T, PL0, d0, RSSnp, RSSStd)
value = value
# value = self.parmsc['rss0']-10*self.parmsc['pn']*log10(dr/self.parmsc['d0'])+self.err_v[ibn,ian]
self.Model = {}
self.Model['PL0'] =self.parmsc['rss0']
self.Model['d0'] = self.parmsc['d0']
self.Model['RSSnp'] = self.parmsc['pn']
self.Model['RSSStd'] = self.parmsc['sigma_max_RSS']
self.Model['Rest'] = 'mode'
C = RSS(value=value,
std=self.std_v[ibn,ian],
vcw=self.parmsc['vcw'],
model=self.Model,
p=pan )
cla.append(C)
clarss.append(C)
if self.parmsc['algebraic']:
atv.append(value)
if len(self.rss_idx) != 0:
self.errRSS = delete(self.errRSS,0,0)
if len(self.toa_idx) != 0:
self.errTOA = delete(self.errTOA,0,0)
if len(self.tdoa_idx) != 0:
self.errTDOA = delete(self.errTDOA,0,0)
# version boite recursive
#
######################### CLA TOTALE
cla.merge2()
cla.refine(cla.Nc)
self.cla = cla
### DoubleListRefine version
cla.estpos2()
pe1=cla.pe
rgpatimeb=time.time()
self.time_compute['RGPA'].append(rgpatimeb-rgpatimea)
self.pe.append(pe1)
errli.append(np.sqrt(np.dot(pe1[:2]-pbn[:2],pe1[:2]-pbn[:2])))
#print len(parmsc['l_connect'])
if len(parmsc['l_connect']) > 4 : # pour ne pas calculer 2fois les cas non hybrides
if len(nonzero(np.array(parmsc['l_connect'])=='RSS')[0]) != 0:
for i in range(4):
clarss.c[i].Id=i
clarss.merge2()
clarss.refine(clarss.Nc)
clarss.estpos2()
clarss.bn=bn[ibn]
self.clarss=clarss
self.perss=clarss.pe
errli.append(np.sqrt(np.dot(self.perss[:2]-pbn[:2],self.perss[:2]-pbn[:2])))
if len(nonzero(np.array(parmsc['l_connect'])=='TOA')[0]) != 0:
for i in range(4):
clatoa.c[i].Id=i
clatoa.merge2()
clatoa.refine(clatoa.Nc)
clatoa.estpos2()
clatoa .bn=bn[ibn]
self.clatoa=clatoa
self.petoa=clatoa.pe
errli.append(np.sqrt(np.dot(self.petoa[:2]-pbn[:2],self.petoa[:2]-pbn[:2])))
if len(nonzero(np.array(parmsc['l_connect'])=='TDOA')[0]) != 0:
for i in range(3):
clatdoa.c[i].Id=i
clatdoa.merge2()
clatdoa.refine(clatdoa.Nc)
clatdoa.estpos2()
clatdoa.bn=bn[ibn]
self.clatdoa=clatdoa
self.petdoa=clatdoa.pe
errli.append(np.sqrt(np.dot(self.petdoa[:2]-pbn[:2],self.petdoa[:2]-pbn[:2])))
#print errli
err1=min(errli)
#print err1
self.err1 = np.hstack((self.err1,err1))
#self.err2 = np.hstack((self.err2,err2))
#if err >3:
# pdb.set_trace()
if self.parmsc['algebraic']:
if algehyb==1:
self.parmsc['Constrain_Type']='hybrid'
algetimea=time.time()
p_LS = self.algebraic_compute(atv)
algetimeb=time.time()
self.time_compute['algebraic'].append(algetimeb-algetimea)
if self.parmsc['save_pe']:
self.p_LS.append(p_LS)
if self.parmsc['Constrain_Type'] != 'hybrid':
errLS = np.sqrt(np.dot(p_LS[:2]-pbn[:2],p_LS[:2]-pbn[:2]))
#elif self.parmsc['Algebraic_method'] == 'CRB':
# errLS = np.sqrt(self.CRB)
else :
errLS = np.sqrt(np.dot(p_LS[:2]-pbn[:2],p_LS[:2]-pbn[:2]))
self.errLS = np.hstack((self.errLS,errLS))
if errli > errLS:
print 'NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNnn\n',errli,'\n',errLS
def TWLSHDFLocate(self, RN_RSS, RN_ToA, RN_TDoA, RN_TDoA2, ToA, ToAStd,
TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies LS approximation to get position P.
Return P
"""
c = 3e08
RSSL = RSSLocation(RN_RSS)
if RN_RSS == None:
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_ToA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((K_ToA, K_TDoA))
A = vstack((A_ToA, A_TDoA))
C = diag(hstack((C_ToA, C_TDoA)))
A2 = dot(A.T, dot(linalg.inv(C), A))
[U, S, V] = svd(A2)
J = 1 / S
rA = rank(A)
m, n = shape(A)
f = 0
if log10(cond(A2)) >= max(
RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)):
f = f + 1
for i in range(n - rA):
u = where(J == max(J))
J[u] = 0
A2i = dot(dot(V.T, diag(J)), U.T)
P = 0.5 * dot(A2i, dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
elif RN_ToA == None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_RSS[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((K_RSS, K_TDoA))
A = vstack((A_RSS, A_TDoA))
C = diag(hstack((C_RSS, C_TDoA)))
A2 = dot(A.T, dot(linalg.inv(C), A))
[U, S, V] = svd(A2)
J = 1 / S
rA = rank(A)
m, n = shape(A)
f = 0
if log10(cond(A2)) >= max(
RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)):
f = f + 1
for i in range(n - rA):
u = where(J == max(J))
J[u] = 0
A2i = dot(dot(V.T, diag(J)), U.T)
P = 0.5 * dot(A2i, dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
elif RN_TDoA == None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# solution
K = vstack((K_RSS, K_ToA))
A = vstack((A_RSS, A_ToA))
C = diag(hstack((C_RSS, C_ToA)))
A2 = dot(A.T, dot(linalg.inv(C), A))
[U, S, V] = svd(A2)
J = 1 / S
rA = rank(A)
m, n = shape(A)
f = 0
if log10(cond(A2)) >= max(
RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)):
f = f + 1
for i in range(n - rA):
u = where(J == max(J))
J[u] = 0
A2i = dot(dot(V.T, diag(J)), U.T)
P = 0.5 * dot(A2i, dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
else:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_TDoA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((vstack((K_RSS, K_ToA)), K_TDoA))
A = vstack((vstack((A_RSS, A_ToA)), A_TDoA))
C = diag(hstack((hstack((C_RSS, C_ToA)), C_TDoA)))
A2 = dot(A.T, dot(linalg.inv(C), A))
[U, S, V] = svd(A2)
J = 1 / S
rA = rank(A)
m, n = shape(A)
f = 0
if log10(cond(A2)) >= max(
RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)):
f = f + 1
for i in range(n - rA):
u = where(J == max(J))
J[u] = 0
A2i = dot(dot(V.T, diag(J)), U.T)
P = 0.5 * dot(A2i, dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
def WLSHDFLocate(self, RN_RSS, RN_ToA, RN_TDoA, RN_TDoA2, ToA, ToAStd,
TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies LS approximation to get position P.
Return P
"""
c = 3e08
RSSL = RSSLocation(RN_RSS)
if RN_RSS == None:
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_ToA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((K_ToA, K_TDoA))
A = vstack((A_ToA, A_TDoA))
C = diag(hstack((C_ToA, C_TDoA)))
P = 0.5 * dot(linalg.inv(dot(A.T, dot(linalg.inv(C), A))),
dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
elif RN_ToA == None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_RSS[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((K_RSS, K_TDoA))
A = vstack((A_RSS, A_TDoA))
C = diag(hstack((C_RSS, C_TDoA)))
P = 0.5 * dot(linalg.inv(dot(A.T, dot(linalg.inv(C), A))),
dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
elif RN_TDoA == None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# solution
K = vstack((K_RSS, K_ToA))
A = vstack((A_RSS, A_ToA))
C = diag(hstack((C_RSS, C_ToA)))
P = 0.5 * dot(linalg.inv(dot(A.T, dot(linalg.inv(C), A))),
dot(dot(A.T, linalg.inv(C)), K))
return P
else:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS * RN_RSS, axis=0)).reshape(RNnum_RSS, 1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd,
Rest)
RoA2 = (RoA * RoA).reshape(RNnum_RSS, 1)
K_RSS = RN_RSS2[1:RNnum_RSS, :] - RN_RSS2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_RSS, :]
A_RSS = hstack(
(RN_RSS[:, 1:RNnum_RSS].T -
RN_RSS[:, 0].reshape(1, shRN_RSS[0]), zeros(
(RNnum_RSS - 1, 1))))
C_RSS = RoAStd[1:RNnum_RSS, 0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA * RN_ToA, axis=0)).reshape(RNnum_ToA, 1)
RoA = c * ToA
RoAStd = c * ToAStd
RoA2 = (RoA * RoA).reshape(RNnum_ToA, 1)
K_ToA = RN_ToA2[1:RNnum_ToA, :] - RN_ToA2[0, 0] + RoA2[
0, 0] - RoA2[1:RNnum_ToA, :]
A_ToA = hstack(
(RN_ToA[:, 1:RNnum_ToA].T -
RN_ToA[:, 0].reshape(1, shRN_ToA[0]), zeros(
(RNnum_ToA - 1, 1))))
C_ToA = RoAStd[1:RNnum_ToA, 0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_TDoA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c * TDoA
RDoAStd = c * TDoAStd
RDoA2 = (RDoA * RDoA).reshape(RNnum_TDoA, 1)
K_TDoA = (sum(
(RN_TDoA - RN_TDoA2) *
(RN_TDoA - RN_TDoA2), axis=0)).reshape(RNnum_TDoA, 1) - RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T, 0.5 * RoA[0, 0] * RDoA))
C_TDoA = RDoAStd[:, 0]**2
# solution
K = vstack((vstack((K_RSS, K_ToA)), K_TDoA))
A = vstack((vstack((A_RSS, A_ToA)), A_TDoA))
C = diag(hstack((hstack((C_RSS, C_ToA)), C_TDoA)))
P = 0.5 * dot(linalg.inv(dot(A.T, dot(linalg.inv(C), A))),
dot(dot(A.T, linalg.inv(C)), K))
return P[:2, :]
class RSS(Constraint):
""" RSS Constraint
Description and evaluation of TOA constraints
:Parameters:
value : float
Constraint value in dB. Default = 40
std : float
Value standard deviation in dB. default = 1.0
vcw : float
scale factor. Default = 1.0
model : dictionnary
keys: self.model['PL0'] =-34.7
self.model['d0'] = 1.0
self.model['RSSnp'] = 2.64
self.model['RSSStd'] = 4.34
self.model['Rest'] = 'mode'
p : np.array 1 x ndim
constraint center
:Attributes:
range : distance conversion from time self.value.
sstd : distance conversion from time self.std
runable : True NOT USED
evaluated :False NOT USED
self.Id : Constraint ID
from annulus bound:
min : minimum value of observable
max : maximum value of observable
mean : mean value of observable
:Methods:
annulus_bound(self) : Compute the minimum and maximum distance of the enclosing annulus of the constraint
rescale(self,vcw) : rescale contraint boundary with a given scale factor 'vcw'
inclusive(self,b) : Is constraint center is inside a box ?
valid(self,b) : Test if Lbox is compatible with the constraint
valid_v(self,lv) : Test if a liste of a vertexes from a box is compatible with the constraint. vertexes are obtained thanks to LBoxN.bd2coordinates()
estvol(self) : Constraint Volume estimation
"""
def __init__(self,value=40,std=1.0,vcw=3,model={},p=np.array([])):
Constraint.__init__(self,'RSS',p)
self.Id = copy.copy(self.C_Id)
Constraint.C_Id = Constraint.C_Id+1 # constraint counter is incremented
self.value = value # attennation (dB)
if len(model)==0:
self.model['PL0'] =-34.7
self.model['d0'] = 1.0
self.model['RSSnp'] = 2.64
self.model['RSSStd'] = 4.34
self.model['Rest'] = 'mode'
else :
self.model = model
self.LOC = RSSLocation(p)
self.std = (self.LOC.getRangeStd(p, self.model['PL0'], self.model['d0'], self.value,self.model['RSSnp'], self.model['RSSStd'], self.model['Rest'])[0])/0.3
self.range = self.LOC.getRange(p, self.model['PL0'], self.model['d0'], self.value, self.model['RSSnp'], self.model['RSSStd'], self.model['Rest'])[0]
self.sstd = self.std*0.3
self.rescale(vcw)
self.runable = True
self.evaluated = False
self.annulus_bound()
def annulus_bound(self):
"""
annulus_bound():
Compute the minimum and maximum distance of the enclosing annulus of the constraint
:Returns:
Nothing but update cmin, cmax
"""
self.cmin = max(0,self.range - self.vcw*self.sstd )
self.cmax = self.range + self.vcw*self.sstd
def rescale(self,vcw):
"""rescale bounds of the constraint with vcw factor
:Parameters:
vcw : float
scale factor
"""
self.vcw = vcw
dx = self.range+(self.vcw*self.std)*0.3*np.ones(len(self.p))
box = BoxN(np.array([self.p-dx,self.p+dx]),ndim=len(self.p))
self.lbox = LBoxN([box])
self.estvol()
def inclusive(self,b):
""" Is constraint center is inside a box ?
:Parameters:
b : LBoxN
:Returns:
Boolean
"""
if b.inbox(self.p):
return True
else:
return False
def valid(self,b):
"""check if a box is valid for the given constraint
:Parameters:
b : LBoxN
:Return:
True : if box is enclosed
False : if the box is ambiguous
'Out' : if the box is out
"""
p0 = self.p
v = self.bd2coord(b)
P = np.outer(np.ones(len(v)),p0)
D = P-v
DD = np.sqrt(np.sum(D*D,axis=1))
DDcmin = sum(DD>=cmin)
DDcmax = sum(DD<=cmax)
if DDcmin+DDcmax > 15:
return(True)
elif (DDcmin <1) | (DDcmax <1) :#(bmax<cmin)|(bmin>cmax):
return('out')
else :
return(False)
def valid_v(self,v):
"""
.. todo:
update as TOA valid_v !!
valid_v(v) : check if vertex are valid for the given constraint
A box is valid if it not not valid
A box is not valid if all distances are greater than rangemax
or all distances are less than rangemin
"""
p0 = self.p
P = np.outer(np.ones(len(v)),p0)
D = P-v
DD = np.sqrt(np.sum(D*D,axis=1))
DD2 = (DD>=self.cmin) & (DD<=self.cmax)
return DD2
def estvol(self):
""" Constraint Volume estimation
"""
R2 = self.range+self.vcw*self.sstd
R1 = self.range-self.vcw*self.sstd
V2 = (4.*np.pi*R2**3)/3
V1 = (4.*np.pi*R1**3)/3
self.estvlm = V2-V1
def TWLSHDFLocate(self, RN_RSS, RN_ToA, RN_TDoA, RN_TDoA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies LS approximation to get position P.
Return P
"""
c = 3e08
RSSL=RSSLocation(RN_RSS)
if RN_RSS==None:
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_ToA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((K_ToA, K_TDoA))
A=vstack((A_ToA, A_TDoA))
C=diag(hstack((C_ToA, C_TDoA)))
A2 = dot(A.T,dot(linalg.inv(C),A))
[U,S,V]=svd(A2)
J = 1/S
rA=rank(A)
m,n=shape(A)
f=0
if log10(cond(A2))>=max(RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = where(J==max(J))
J[u] = 0
A2i = dot(dot(V.T,diag(J)),U.T)
P = 0.5*dot(A2i,dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
elif RN_ToA==None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_RSS[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((K_RSS, K_TDoA))
A=vstack((A_RSS, A_TDoA))
C=diag(hstack((C_RSS, C_TDoA)))
A2 = dot(A.T,dot(linalg.inv(C),A))
[U,S,V]=svd(A2)
J = 1/S
rA=rank(A)
m,n=shape(A)
f=0
if log10(cond(A2))>=max(RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = where(J==max(J))
J[u] = 0
A2i = dot(dot(V.T,diag(J)),U.T)
P = 0.5*dot(A2i,dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
elif RN_TDoA==None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# solution
K=vstack((K_RSS, K_ToA))
A=vstack((A_RSS, A_ToA))
C=diag(hstack((C_RSS, C_ToA)))
A2 = dot(A.T,dot(linalg.inv(C),A))
[U,S,V]=svd(A2)
J = 1/S
rA=rank(A)
m,n=shape(A)
f=0
if log10(cond(A2))>=max(RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = where(J==max(J))
J[u] = 0
A2i = dot(dot(V.T,diag(J)),U.T)
P = 0.5*dot(A2i,dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
else:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_TDoA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((vstack((K_RSS,K_ToA)), K_TDoA))
A=vstack((vstack((A_RSS,A_ToA)), A_TDoA))
C=diag(hstack((hstack((C_RSS,C_ToA)), C_TDoA)))
A2 = dot(A.T,dot(linalg.inv(C),A))
[U,S,V]=svd(A2)
J = 1/S
rA=rank(A)
m,n=shape(A)
f=0
if log10(cond(A2))>=max(RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = where(J==max(J))
J[u] = 0
A2i = dot(dot(V.T,diag(J)),U.T)
P = 0.5*dot(A2i,dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
def WLSHDFLocate(self, RN_RSS, RN_ToA, RN_TDoA, RN_TDoA2, ToA, ToAStd, TDoA, TDoAStd, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies LS approximation to get position P.
Return P
"""
c = 3e08
RSSL=RSSLocation(RN_RSS)
if RN_RSS==None:
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_ToA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((K_ToA, K_TDoA))
A=vstack((A_ToA, A_TDoA))
C=diag(hstack((C_ToA, C_TDoA)))
P = 0.5*dot(linalg.inv(dot(A.T,dot(linalg.inv(C),A))),dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
elif RN_ToA==None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_RSS[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((K_RSS, K_TDoA))
A=vstack((A_RSS, A_TDoA))
C=diag(hstack((C_RSS, C_TDoA)))
P = 0.5*dot(linalg.inv(dot(A.T,dot(linalg.inv(C),A))),dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
elif RN_TDoA==None:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# solution
K=vstack((K_RSS, K_ToA))
A=vstack((A_RSS, A_ToA))
C=diag(hstack((C_RSS, C_ToA)))
P = 0.5*dot(linalg.inv(dot(A.T,dot(linalg.inv(C),A))),dot(dot(A.T,linalg.inv(C)),K))
return P
else:
# for RSS
shRN_RSS = shape(RN_RSS)
RNnum_RSS = shRN_RSS[1]
RN_RSS2 = (sum(RN_RSS*RN_RSS,axis=0)).reshape(RNnum_RSS,1)
RoA = RSSL.getRange(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = RSSL.getRangeStd(RN_RSS, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum_RSS,1)
K_RSS = RN_RSS2[1:RNnum_RSS,:]-RN_RSS2[0,0] + RoA2[0,0]-RoA2[1:RNnum_RSS,:]
A_RSS = hstack((RN_RSS[:,1:RNnum_RSS].T - RN_RSS[:,0].reshape(1,shRN_RSS[0]), zeros((RNnum_RSS-1,1))))
C_RSS = RoAStd[1:RNnum_RSS,0]**2
# for ToA
shRN_ToA = shape(RN_ToA)
RNnum_ToA = shRN_ToA[1]
RN_ToA2 = (sum(RN_ToA*RN_ToA,axis=0)).reshape(RNnum_ToA,1)
RoA = c*ToA
RoAStd = c*ToAStd
RoA2 = (RoA*RoA).reshape(RNnum_ToA,1)
K_ToA = RN_ToA2[1:RNnum_ToA,:]-RN_ToA2[0,0] + RoA2[0,0]-RoA2[1:RNnum_ToA,:]
A_ToA = hstack((RN_ToA[:,1:RNnum_ToA].T - RN_ToA[:,0].reshape(1,shRN_ToA[0]), zeros((RNnum_ToA-1,1))))
C_ToA = RoAStd[1:RNnum_ToA,0]**2
# for TDoA
shRN_TDoA = shape(RN_TDoA)
RNnum_TDoA = shRN_TDoA[1]
#RN_TDoA2= RN_TDoA[:,0:1]*ones((1,RNnum_TDoA))
RDoA = c*TDoA
RDoAStd = c*TDoAStd
RDoA2 = (RDoA*RDoA).reshape(RNnum_TDoA,1)
K_TDoA = (sum((RN_TDoA-RN_TDoA2)*(RN_TDoA-RN_TDoA2),axis=0)).reshape(RNnum_TDoA,1)-RDoA2
A_TDoA = hstack((RN_TDoA.T - RN_TDoA2.T,0.5*RoA[0,0]*RDoA))
C_TDoA = RDoAStd[:,0]**2
# solution
K=vstack((vstack((K_RSS,K_ToA)), K_TDoA))
A=vstack((vstack((A_RSS,A_ToA)), A_TDoA))
C=diag(hstack((hstack((C_RSS,C_ToA)), C_TDoA)))
P = 0.5*dot(linalg.inv(dot(A.T,dot(linalg.inv(C),A))),dot(dot(A.T,linalg.inv(C)),K))
return P[:2,:]
