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 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 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, :]
