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