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]