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]