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_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_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_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()