Ejemplo n.º 1
0
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()
Ejemplo n.º 2
0
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()
Ejemplo n.º 3
0
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()
Ejemplo n.º 4
0
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()