def GNSSNavPropCall(self,data): # llh from proposed method # about 20 Hz self.navfixprop_ = NavSatFix() self.navfixprop_ = data self.latProp_.append(self.navfixprop_.latitude) self.lonProp_.append(self.navfixprop_.longitude) self.navfixpropAvailable = 1 # ---sometimes, self.GNSSNRAWlosDel will be initialized by function CallGNSSRAWNlosDel_1 # ---In this case, save this value for further processing self.GNSSNRAWlosDelSave = GNSS_Raw_Array() self.GNSSNRAWlosDelSave = self.GNSSNRAWlosDel if((self.navfixprop_.header.stamp.secs == self.navfixTruth_.header.stamp.secs) and (len(self.GNSSNRAWlosDel.GNSS_Raws))): # print 'self.navfixprop_.header.stamp.secs',self.navfixprop_.header.stamp.secs if(self.firstTimeProp_ == 0): self.firstTimeProp_ = float(self.navfixprop_.header.stamp.secs) self.GNSSTimeProp.append((float(self.navfixprop_.header.stamp.secs)- float(self.firstTimeProp_)) / 1000) # get GNSS Time self.SatNumProp.append(float(len(self.GNSSNRAWlosDel.GNSS_Raws))) # get SatNum puGNSSPosCalProp_ = puGNSSPosCal.GNSSPosCal() self.HDOPProp.append(float(puGNSSPosCalProp_.DopCalculation(self.GNSSNRAWlosDelSave))) self.errorProp.append(self.PosErrorCal(self.navfixTruth_, self.navfixprop_)) errorxyz_ = Error() errorxyz_.header.stamp.secs = self.navfixprop_.header.stamp.secs errorxyz_.errorxyz = float(self.errorProp[-1]) self.ErrorProp_Pub.publish(errorxyz_) Dop_ = DOP() Dop_.header.stamp.secs = self.navfixprop_.header.stamp.secs Dop_.HDOP = float(self.HDOPProp[-1]) self.DOPProp_Pub.publish(Dop_)
def callcptBestPos_(self, data): self.bestPos_ = BESTPOS() self.bestPos_ = data self.GNSSArr1 = GNSS_Raw_Array() pos_index = 0 if (self.endFlag == 1): if ((self.bestPos_.header.gps_week_seconds) in self.GNSS_time): print 'GNSS_time.index(self.bestPos_.header.gps_week_seconds)', self.bestPos_.header.gps_week_seconds print self.GNSS_time.index( self.bestPos_.header.gps_week_seconds) print '-------------------------------------------' pos_index = self.GNSS_time.index( self.bestPos_.header.gps_week_seconds) totalSv_ = self.total_sv[pos_index] self.GNSS_1 = GNSS_Raw() for index_ in range(int(totalSv_)): index_ = index_ + pos_index print 'index_', index_ self.GNSS_1.GNSS_time = float( self.GNSS_time[index_] ) # GNSS time contained in csv file self.GNSS_1.total_sv = float( self.total_sv[index_] ) # total sitellites in one epoch (epoch: each time point) self.GNSS_1.prn_satellites_index = float( self.prn_satellites_index[index_]) self.GNSS_1.pseudorange = float(self.pseudorange[index_]) self.GNSS_1.snr = float( self.snr[index_] ) # signal noise ratio for this satellite self.GNSS_1.elevation = float( self.elevation[index_] ) # elevation for this satellite and receiver self.GNSS_1.azimuth = float( self.azimuth[index_]) # azimuth for this satellite self.GNSS_1.err_tropo = float( self.err_tropo[index_]) # troposphere error in meters self.GNSS_1.err_iono = float( self.err_iono[index_]) # ionophere error in meters self.GNSS_1.sat_clk_err = float( self.sat_clk_err[index_] ) # satellite clock bias caused error self.GNSS_1.sat_pos_x = float( self.sat_pos_x[index_] ) # satellite positioning in x direction self.GNSS_1.sat_pos_y = float( self.sat_pos_y[index_] ) # satellite positioning in y direction self.GNSS_1.sat_pos_z = float( self.sat_pos_z[index_] ) # satellite positioning in Z direction if (self.GNSS_1.elevation > 18.0): # mask angle = 18 self.GNSSArr1.GNSS_Raws.append(self.GNSS_1) self.GNSS_1 = GNSS_Raw() self.GNSSArr1.header.seq = self.seq self.GNSSArr1.header.stamp.secs = self.stampSecs self.GNSSArr1.header.stamp.nsecs = self.stampnSecs self.GNSSArr1.header.frame_id = 'GNSS_' self.GNSS_Raw_pub.publish(self.GNSSArr1)
def DopCalculation(self,GNSS_one_epoch_): # get Dop in one epoch GNSS_one_epoch = GNSS_Raw_Array() GNSS_one_epoch = GNSS_one_epoch_ H_matrix_ = array([2, 2, 2, 1], dtype='float') # creat H matrix to save transform parameters Hdop_ = 0.0 # create an variable to save Hdop elemin = 15.0 # minimun elevation angle for index_1 in range(len(GNSS_one_epoch.GNSS_Raws)): # index all the satellite information in one epoch if (GNSS_one_epoch.GNSS_Raws[index_1].elevation <= elemin): # print 'satellite elevation less than 15 degree=',GNSS_one_epoch.GNSS_Raws[index_1].elevation continue cosel = float(cos(GNSS_one_epoch.GNSS_Raws[index_1].elevation)) sinel = float(sin(GNSS_one_epoch.GNSS_Raws[index_1].elevation)) H_row = [] # H matrix row H_row.append(float(cosel * sin(GNSS_one_epoch.GNSS_Raws[index_1].azimuth))) H_row.append(float(cosel * cos(GNSS_one_epoch.GNSS_Raws[index_1].azimuth))) H_row.append(float(sinel)) H_row.append(1.0) H_matrix_ = np.row_stack((H_matrix_, H_row)) # add each row to H_matrix del H_row[:] # relief H_row # get H matrix H_matrix_ = np.delete(H_matrix_, [0], axis=0) # delete the first row of H matrix # print 'H_matrix_',H_matrix_ Q_matrix_ = np.dot((H_matrix_.transpose()), H_matrix_) # H(T) * G Q_matrix_ = np.linalg.inv(Q_matrix_) # inverse matrix of H(T) * G Hdop = float(sqrt(Q_matrix_[0, 0] + Q_matrix_[1, 1])) # print 'Q_matrix_', Q_matrix_, 'Hdop', Hdop return float(Hdop) # return result
def CallGNSS_1(self, data): # GNSS data self.GNSS_1 = GNSS_Raw_Array() self.GNSS_1 = data self.satecirclRad = 3 for index_1 in range(len(self.GNSS_1.GNSS_Raws)): # index all the satellite information in one epoch self.azim_.append( (self.GNSS_1.GNSS_Raws[index_1].elevation*(-0.55556)+50.0)* np.cos(-1*(self.GNSS_1.GNSS_Raws[index_1].azimuth-90.0)*3.14159/180.0)) self.elev_.append( (self.GNSS_1.GNSS_Raws[index_1].elevation*(-0.55556)+50.0)* np.sin(-1*(self.GNSS_1.GNSS_Raws[index_1].azimuth-90.0)*3.14159/180.0)) self.satIdx.append(self.GNSS_1.GNSS_Raws[index_1].prn_satellites_index) self.drawAll()
def __init__(self): # rospy.init_node('csv2Topi_', anonymous=True) # initialize node with specific node name self.GNSS_Raw_pub = rospy.Publisher( 'GNSS_', GNSS_Raw_Array, queue_size=100) # customerized GNSS raw data ros message rospy.Subscriber('/novatel_data/bestpos', BESTPOS, self.callcptBestPos_) rospy.Subscriber('/imu_c', Imu, self.callcptImu) self.lowEleSatLis_ = [3, 91, 22, 32, 94] self.lowEleSatLis_ = [] self.eleThres_ = 18.0 # 18 self.snrThres_ = 20.0 # 20 self.GNSSArr_ = GNSS_Raw_Array() self.initVariable()
def __init__(self, csvPath, tophz_): # rospy.init_node('csv2Topi_', anonymous=True) # initialize node with specific node name self.GNSS_Raw_pub = rospy.Publisher( 'GNSS_', GNSS_Raw_Array, queue_size=10) # customerized GNSS raw data ros message self.topHz = tophz_ self.lowEleSatLis_ = [3, 91, 22, 32, 94] self.lowEleSatLis_ = [] self.eleThres_ = 18.0 # 18 self.snrThres_ = 20.0 # 20 self.csvPath = csvPath # /home/wenws/3_nlosMovobj/src/nlosExclusion/src/data5/exper1/sv_data.csv if (tophz_ > 0): self.readCSV() self.GNSSArr_ = GNSS_Raw_Array()
def __init__(self, calAngle): self.nlExcMode = 'statManu' # 'statAuto' 'dynaAuto' 'statManu' 'dynaManu' self.GNSSNlos_pub = rospy.Publisher( 'GNSSRAWNNlos_', GNSS_Raw_Array, queue_size=10) # customerized GNSS raw data ros message self.GNSSNlosDel_pub = rospy.Publisher( 'GNSSRAWNlosDel_', GNSS_Raw_Array, queue_size=10) # customerized GNSS raw data ros message rospy.Subscriber('novatel_data/inspvax', INSPVAX, self.callINSPVAX_) self.exclusionSatnum_Pub = rospy.Publisher('exclusionSatNum', exclusionSatNum, queue_size=100) # self.calAngN = calAngle self.bouRang = 160 self.GNSSTime = 0 self.azim_ = [] self.elev_ = [] self.azimIni_ = [] self.elevIni_ = [] self.satIdx = [] self.bouSide1X = [] self.bouSide1Y = [] self.bouSide2X = [] self.bouSide2Y = [] self.bouSide1IniX = [] self.bouSide2IniX = [] self.satExcl_ = [] self.mannuSat = [] self.GPSNum_ = 0 self.BeiNum_ = 0 self.GPSExclNum = 0 self.BeidExclNum = 0 self.IniGNSS_ = GNSS_Raw_Array() self.nlosGNSS_ = GNSS_Raw_Array() self.posArr = [ ] # create a list to save double-decker bus boundary information
def __init__(self, ): self.reserve = 0 self.GrouLat_ = 0.0 self.GrouLon_ = 0.0 self.GrouAlt_ = 0.0 self.calAngNNLOS = 0.0 self.SPANUpdate = 0.0 self.busUpdate = 0.0 self.GNSSUpdate = 0.0 # Ublox self.SPANYawUpdate = 0.0 self.GNSSNRAWlosDelTimeFlag = 0 self.posArr = [] self.initialLLH = [114.19289862,22.3181226456,0] self.HDOPProp = 0 # self.excluSatLis = [17,28,8,3,22,9,92] # Not blocked: 11,1,93,94,96,89,7 blocked: 17,28,8,3,22,9,92 self.excluSatLis = [9,14,31] # 9,14,31,94,90,87 self.GNSS_ = GNSS_Raw_Array() self.GNSSNRAWlosDel = GNSS_Raw_Array() self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal() self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal() self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS) rospy.Subscriber('/novatel_data/bestpos', BESTPOS, self.callcptBestPos_) # Ground truth rospy.Subscriber('double_decker_parameters', PoseArray, self.callDouDeckBus) # bus rospy.Subscriber('GNSS_', GNSS_Raw_Array, self.CallGNSS_) # Ublox rospy.Subscriber('novatel_data/inspvax', INSPVAX, self.callINSPVAXNLOS_) # yaw rospy.Subscriber('velodyne_points_0', PointCloud2, self.PointCloudCall) # velodyne rospy.Subscriber('GNSSRAWNlosDel_', GNSS_Raw_Array, self.CallGNSSRAWNlosDel_) # Ublox rospy.Subscriber('DOP_', DOP, self.dopsubscribe) # Ublox self.GNSS_Navfix_pub = rospy.Publisher('GNSS_NavFix', NavSatFix,queue_size=100) # self.propGNSS_Navfix_pub = rospy.Publisher('GNSS_NavFix_pro', NavSatFix, queue_size=100) # self.graphslam_Navfix_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=100) # self.graphslam_GeoPoint_pub = rospy.Publisher('/gps/geopoint', GeoPointStamped, queue_size=100) # self.graphslam_PointCloud_pub = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=100) #
def __init__(self, parent=None): QtCore.QThread.__init__(self, parent) self.posArr = [] self.GNSS_ = GNSS_Raw_Array() self.print_ = 8 self.iniSatLis_ = [] self.iniSnr = [] # self.excluSatLis = [29,23,92] # for small nlos, experiment 1 self.excluSatLis = [ 3, 91, 22, 32, 94, 23, 26, 93 ] # 23,26,93 for big nlos, experiment 2 (alway visible: 13 26 31 93 96 100 sometimes visible: 23) Low elevation:3,91,22,32,94, self.nlosExclusionF_ = puNlosExclusion.nlosExclusion() self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal() self.puGNSSPosCalF2_ = puGNSSPosCal.GNSSPosCal() rospy.Subscriber('double_decker_parameters', PoseArray, self.callDouDeckBus) rospy.Subscriber('GNSS_', GNSS_Raw_Array, self.CallGNSS_)
def callback_GNSS_Raw_data(data): # callback function to handle GNSS Raw data GNSS_Raw_array_list=[] # create a list to save PoseArray message, which contains GNSS Raw data GNSS_exclusion_Array_1 = GNSS_Raw_Array() # create an variable in GNSS_Raw_Array format to save GNSS data after exclusion GNSS_Raw_array_list=data.GNSS_Raws # save all the satellite azimuth information and elevation information in GNSS_Raw_array_list (one GNSS_Raw_array_list message contains several GNSS_Raws message) GNSS_exclusion_Array_1=data plot_total_sate.append(GNSS_exclusion_Array_1.GNSS_Raws[0].total_sv) # save the total satellites numbers (GPS+Beidou) plot_total_sateTim.append(float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time)) GPSNum = 0 # create a variable to save GPS number BeidouNum = 0 # create a variable to save Beidou number for i in range(len(GNSS_Raw_array_list)): # index all the Pose message in PoseArray message (http://docs.ros.org/api/geometry_msgs/html/msg/PoseArray.html) # x--azimuth y--elevation (Skyplot: https://github.com/feddischson/skyplotwidget) satellite_azimuth_list.append( (GNSS_Raw_array_list[i].elevation*(-0.55556)+50.0)* np.cos(-1*(GNSS_Raw_array_list[i].azimuth-90.0)*3.14159/180.0)) # function to transfer from azimuth into skyplot azimuth satellite_elevation_list.append( (GNSS_Raw_array_list[i].elevation*(-0.55556)+50.0)* np.sin(-1*(GNSS_Raw_array_list[i].azimuth-90.0)*3.14159/180.0)) # function to transfer from elevation into skyplot elevation satellite_azimuth_list_glob.append((GNSS_Raw_array_list[i].elevation*(-0.55556)+50.0)* np.cos(-1*(GNSS_Raw_array_list[i].azimuth-90.0)*3.14159/180.0)) satellite_elevation_list_glob.append((GNSS_Raw_array_list[i].elevation*(-0.55556)+50.0)* np.sin(-1*(GNSS_Raw_array_list[i].azimuth-90.0)*3.14159/180.0)) satellite_number_list.append(GNSS_Raw_array_list[i].prn_satellites_index) satellite_number_list_glob.append(GNSS_Raw_array_list[i].prn_satellites_index) satellite_initial_azimuth_list.append(GNSS_Raw_array_list[i].azimuth) # save initial azimuth data for plotting in callback_azi_eleva() satellite_initial_azimuth_list_glob.append(GNSS_Raw_array_list[i].azimuth) # save initial azimuth data for NLOS exclusion in callback_GNSS_Raw_data if ((GNSS_Raw_array_list[i].prn_satellites_index >= 1) and (GNSS_Raw_array_list[i].prn_satellites_index <= 32)): # GPS GPSNum=GPSNum+1 if ((GNSS_Raw_array_list[i].prn_satellites_index >= 88) and (GNSS_Raw_array_list[i].prn_satellites_index <= (87+37))): # Beidou BeidouNum=BeidouNum+1 if (GPSNum > 0): # make sure that GPSNum is not zero plot_GPS_sate.append(float(GPSNum)) # save GPS number to list plot_Beidou_sate.append(float(BeidouNum)) # save Beidou number to list plot_Beidou_sateTim.append(float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time)) GPSNum = 0 # clear BeidouNum = 0 # clear #NLOS Exclusion len_listx_2 = len(listx_2_ini_glob) if (len_listx_2 >= 3): # set threshold: maxinum double-decker bus for NLOS exclusion len_listx_2 = 3 if(exclusMode==0): # autonomatic exclusion for i_bou_num in range(len_listx_2): # index all the boundary line (usually only one double-decker bus, sometime 2) for index_sat in range(len(GNSS_Raw_array_list)): # index all the satellite information in one epoch point0_x = 0.0 # yuanxin x point0_y = 0.0 # yuanxin y point1_x=float(listx_2_glob[i_bou_num]) # point1 coordinate x point1_y=float(listy_2_glob[i_bou_num]) # point1 coordinate y point2_x=float(listx_3_glob[i_bou_num]) # point2 coordinate x point2_y=float(listy_3_glob[i_bou_num]) # point2 coordinate y point_x =float(satellite_azimuth_list_glob[index_sat]) # point_x (satellite azimuth in skyplot) point_y =float(satellite_elevation_list_glob[index_sat]) # point_y (satellite elevation in skyplot) area_error=math.fabs(IsInside(point1_x, point1_y, point2_x, point2_y, point0_x, point0_y, point_x, point_y)) # print 'area_error',area_error,'listx_2_ini_glob',listx_2_ini_glob # print 'len(listx_2)=',len(listx_2),'len(listx_2_ini_glob)=',len(listx_2_ini_glob) if(listx_2_ini_glob[i_bou_num]<0): # negtive to positive listx_2_ini_glob[i_bou_num]=listx_2_ini_glob[i_bou_num]+360 if (listx_3_ini_glob[i_bou_num] < 0): # negtive to positive listx_3_ini_glob[i_bou_num] = listx_3_ini_glob[i_bou_num] + 360 if(listx_2_ini_glob[i_bou_num]>listx_3_ini_glob[i_bou_num]) and len(listx_2_ini_glob)>0: # make sure that listx_3_initial[i_bou_num] is smaller azi_temp=listx_3_ini_glob[i_bou_num] listx_3_ini_glob[i_bou_num]=listx_2_ini_glob[i_bou_num] listx_2_ini_glob[i_bou_num]=azi_temp ang_ran_=float(listx_3_ini_glob[i_bou_num]-listx_2_ini_glob[i_bou_num]) print 'ang_ran_----------------------',ang_ran_,'from',listx_2_ini_glob[i_bou_num],'to',listx_3_ini_glob[i_bou_num] # if(len(satellite_visability)>0): # if satellite_visability is not empty, clear # del satellite_visability[:] # clear content of satellite_visability if(ang_ran_<160): if(satellite_initial_azimuth_list_glob[index_sat]>listx_2_ini_glob[i_bou_num]) and (satellite_initial_azimuth_list_glob[index_sat]<listx_3_ini_glob[i_bou_num]) and (area_error>5):# and (math.fabs(area_error)>3) : print 'detect one NLOS signals at satellite-',satellite_number_list_glob[index_sat],'area_error',area_error,'satellite_initial_azimuth_list',satellite_initial_azimuth_list_glob[index_sat] print 'point1_x point1_y point2_x point2_y point_x point_y',point1_x,point1_y,point2_x,point2_y,point_x,point_y GNSS_exclusion_Array_1.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later satellite_visability.append(float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index)) # save the visability elif(ang_ran_>160): if (satellite_initial_azimuth_list_glob[index_sat] > 0) and (satellite_initial_azimuth_list_glob[index_sat] < listx_2_ini_glob[i_bou_num]) and (area_error >5): # and (math.fabs(area_error)>3) : print 'detect one NLOS signals at satellite-', satellite_number_list_glob[index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list',satellite_initial_azimuth_list_glob[index_sat] print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y GNSS_exclusion_Array_1.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later satellite_visability.append(float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index)) # save the visability if (satellite_initial_azimuth_list_glob[index_sat] < 360) and (satellite_initial_azimuth_list_glob[index_sat] > listx_3_ini_glob[i_bou_num]) and (area_error >5): # and (math.fabs(area_error)>3) : print 'detect one NLOS signals at satellite-', satellite_number_list_glob[index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list',satellite_initial_azimuth_list_glob[index_sat] print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y GNSS_exclusion_Array_1.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later satellite_visability.append(float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index)) # save the visability GPSNumExcluded=0.0 BeidouNumExcluded=0.0 for del_index in range(len(GNSS_exclusion_Array_1.GNSS_Raws)): # index all the satellites' visability in one epoch if (del_index >= len(GNSS_exclusion_Array_1.GNSS_Raws)): break if(GNSS_exclusion_Array_1.GNSS_Raws[del_index].visable == 2) or(GNSS_exclusion_Array_1.GNSS_Raws[del_index].elevation<=20): # if satellite is NLOS reception, delete this satellite if ((GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index >= 1) and (GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index <= 32)): # GPS GPSNumExcluded=GPSNumExcluded+1 # one GPS exclusion if ((GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index >= 88) and (GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index <= (87+37))): # GPS BeidouNumExcluded=BeidouNumExcluded+1 # one Beidou exclusion del GNSS_exclusion_Array_1.GNSS_Raws[del_index] # delete the NLOS reception satellites if(exclusMode==1): # mannual exclusion: specify certain amount of satellites needed to be excluded for index_sat in range(len(GNSS_Raw_array_list)): # index all the satellite information in one epoch if(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index in satellite_visability_mannually):# and (math.fabs(area_error)>3) : GNSS_exclusion_Array_1.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later if float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index) not in satellite_visability: satellite_visability.append(float(GNSS_exclusion_Array_1.GNSS_Raws[index_sat].prn_satellites_index)) # save the visability GPSNumExcluded=0.0 BeidouNumExcluded=0.0 for del_index in range(len(GNSS_exclusion_Array_1.GNSS_Raws)): # index all the satellites' visability in one epoch if (del_index >= len(GNSS_exclusion_Array_1.GNSS_Raws)): break if(GNSS_exclusion_Array_1.GNSS_Raws[del_index].visable == 2) or(GNSS_exclusion_Array_1.GNSS_Raws[del_index].elevation<=20): # if satellite is NLOS reception, delete this satellite if ((GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index >= 1) and (GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index <= 32)): # GPS GPSNumExcluded=GPSNumExcluded+1 # one GPS exclusion if ((GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index >= 88) and (GNSS_exclusion_Array_1.GNSS_Raws[del_index].prn_satellites_index <= (87+37))): # GPS BeidouNumExcluded=BeidouNumExcluded+1 # one Beidou exclusion print 'GNSS_exclusion_Array_1.GNSS_Raws[del_index].elevation',GNSS_exclusion_Array_1.GNSS_Raws[del_index].elevation del GNSS_exclusion_Array_1.GNSS_Raws[del_index] # delete the NLOS reception satellites plot_GPS_sate_excluded.append(float(GPSNumExcluded)) # save GPS exclusion numbers plot_Beidou_sate_excluded.append(float(BeidouNumExcluded)) # save Beidou exclusion numbers plot_Total_sate_excluded.append(float(GPSNumExcluded)+float(BeidouNumExcluded)) # save all the satellites that have been excluded plot_Total_sate_excludedTim.append(float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time)) satelliteInfo=Satellite_Info() satelliteInfo.GNSS_time = int(float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time)) satelliteInfo.GPSExcluded = float(GPSNumExcluded) satelliteInfo.BeidouExcluded = float(BeidouNumExcluded) Satellite_Info_pub_.publish(satelliteInfo) GPSNumExcluded = 0.0 # clear numbers BeidouNumExcluded = 0.0 # clear numbers if(len(listx_2_ini_glob)>0): # make sure, publish GNSS_exclusion_Array_1 only when there is at least one double-decker bus GNSS_exclusion_pub.publish(GNSS_exclusion_Array_1) del listx_2_ini_glob[:] del listx_3_ini_glob[:] del listx_2_glob[:] del listy_2_glob[:] del listx_3_glob[:] del listy_3_glob[:] del satellite_initial_azimuth_list_glob[:] del satellite_azimuth_list_glob[:] del satellite_elevation_list_glob[:] del satellite_number_list_glob[:]
def readCSV(self): self.csv_GNSS = csv.reader(open( self.csvPath, 'r')) # read csv context to csv_reader variable self.GNSSArr = GNSS_Raw_Array() self.GNSSTim = 0 self.preGNSSTim = 0 self.linNum = 0 self.preTopiTim = time.time() self.curTopTim = time.time() for rowCsv in self.csv_GNSS: self.preTopiTim = float(round(time.time() * 1000)) self.GNSS_ = GNSS_Raw() self.rosNavSaFix = NavSatFix() self.GNSSTim = rowCsv[0] self.GNSS_.GNSS_time = float( rowCsv[0]) # GNSS time contained in csv file self.GNSS_.total_sv = float( rowCsv[1] ) # total sitellites in one epoch (epoch: each time point) self.GNSS_.prn_satellites_index = float( rowCsv[2] ) # satellite index in this epoch (epoch: each time point) self.GNSS_.pseudorange = float( rowCsv[3]) - float(rowCsv[7]) - float(rowCsv[8]) + float( rowCsv[9]) # pseudorange measurement self.GNSS_.snr = float( rowCsv[4]) # signal noise ratio for this satellite self.GNSS_.elevation = float( rowCsv[5]) # elevation for this satellite and receiver self.GNSS_.azimuth = float(rowCsv[6]) # azimuth for this satellite self.GNSS_.err_tropo = float( rowCsv[7]) # troposphere error in meters self.GNSS_.err_iono = float(rowCsv[8]) # ionophere error in meters self.GNSS_.sat_clk_err = float( rowCsv[9]) # satellite clock bias caused error self.GNSS_.sat_pos_x = float( rowCsv[10]) # satellite positioning in x direction self.GNSS_.sat_pos_y = float( rowCsv[11]) # satellite positioning in y direction self.GNSS_.sat_pos_z = float( rowCsv[12]) # satellite positioning in Z direction self.GNSS_.visable = 0 # satellite visability juded by NLOS exclusion if ((self.preGNSSTim == self.GNSSTim) or (self.linNum == 0)): if (self.GNSS_.elevation > self.eleThres_) and ( self.GNSS_.snr > self.snrThres_) and (self.GNSS_.prn_satellites_index not in self.lowEleSatLis_): self.GNSSArr.GNSS_Raws.append(self.GNSS_) else: self.GNSSArr.header.frame_id = 'GNSS_' self.GNSS_Raw_pub.publish(self.GNSSArr) # GNSSPosCal_ = puGNSSPosCal.GNSSPosCal() # GNSSPosCal_.iterPosCal(self.GNSSArr,'LSGNSS') # print 'llh',GNSSPosCal_.llh_,len(GNSSPosCal_.azimuth),GNSSPosCal_.azimuth self.GNSSArr_ = self.GNSSArr # keep for outside del self.GNSSArr.GNSS_Raws[:] if (self.GNSS_.elevation > self.eleThres_) and ( self.GNSS_.snr > self.snrThres_) and (self.GNSS_.prn_satellites_index not in self.lowEleSatLis_): self.GNSSArr.GNSS_Raws.append(self.GNSS_) self.curTopTim = float(round(time.time() * 1000)) time.sleep(1 / self.topHz - (self.curTopTim - self.preTopiTim) / 1000.0) # time.sleep(1 / self.topHz ) print 'rostopic time bias / ms ------------ ', self.curTopTim - self.preTopiTim self.preGNSSTim = self.GNSSTim self.linNum = self.linNum + 1
def __init__(self, parent=None): super(readCSV2TopiThread, self).__init__(parent) self.GNSS_ = GNSS_Raw_Array()
def LSGPSPosCalStep(self, GNSS_one_epoch_, init_x, init_y, init_z, init_b): # least square GNSS_one_epoch = GNSS_Raw_Array() GNSS_one_epoch = GNSS_one_epoch_ rec_ini_pos_x = float(init_x) # initial position x (m) rec_ini_pos_y = float(init_y) # initial position y (m) rec_ini_pos_z = float(init_z) # initial position z (m) rec_clo_bia_b = float( init_b) # initial distance bias caused by clock bias (m) devia_xyz = 1.0 # initial set receiver position estimation error (m) gues_pseud = [] # guessed pseudorange pseud_error = [] # pseudorange error G_matrix_ = array( [2, 2, 2, 1], dtype='float') # creat G matrix to save transform parameters self.dop = self.DopCalculation(GNSS_one_epoch) for index_1 in range( len(GNSS_one_epoch.GNSS_Raws )): # index all the satellite information in one epoch if ((GNSS_one_epoch.GNSS_Raws[index_1].prn_satellites_index >= 88) and (GNSS_one_epoch.GNSS_Raws[index_1].prn_satellites_index <= (87 + 37))): if (self.iterations_ < 1): # save one time only self.GNSSTim = GNSS_one_epoch.GNSS_Raws[index_1].GNSS_time self.toSv = GNSS_one_epoch.GNSS_Raws[index_1].total_sv self.prn.append( GNSS_one_epoch.GNSS_Raws[index_1].prn_satellites_index) self.snr.append(GNSS_one_epoch.GNSS_Raws[index_1].snr) self.visi.append(GNSS_one_epoch.GNSS_Raws[index_1].visable) self.azimuth.append( GNSS_one_epoch.GNSS_Raws[index_1].azimuth) self.elevation.append( GNSS_one_epoch.GNSS_Raws[index_1].elevation) sx_1 = float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_x ) # satellite position x sy_1 = float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_y ) # satellite position y sz_1 = float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_z ) # satellite position z sx_1 = (sx_1 - rec_ini_pos_x) * ( sx_1 - rec_ini_pos_x ) # satellite to receiver distance in x idrection sy_1 = (sy_1 - rec_ini_pos_y) * ( sy_1 - rec_ini_pos_y ) # satellite to receiver distance in y idrection sz_1 = (sz_1 - rec_ini_pos_z) * ( sz_1 - rec_ini_pos_z ) # satellite to receiver distance in z idrection sat2rec_dis = sqrt(sx_1 + sy_1 + sz_1) # guessed pseudorange gues_pseud.append(sat2rec_dis) # save guessed pseudorange pseud_error_element = float( GNSS_one_epoch.GNSS_Raws[index_1].pseudorange) - float( sat2rec_dis) + float( rec_clo_bia_b) # pseudorange error pseud_error.append( pseud_error_element) # save pseudorange error G_row = [] # G matrix row G_row.append( float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_x - rec_ini_pos_x) / float(sat2rec_dis) * -1) # x for G matrix row G_row.append( float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_y - rec_ini_pos_y) / float(sat2rec_dis) * -1) # y for G matrix row G_row.append( float(GNSS_one_epoch.GNSS_Raws[index_1].sat_pos_z - rec_ini_pos_z) / float(sat2rec_dis) * -1) # z for G matrix row element_float = 1.0 # last element for each row G_row.append(element_float) # save last element for each row G_matrix_ = np.row_stack( (G_matrix_, G_row)) # add each row to G_matrix del G_row[:] # relief G_row # get pseudorange error pseud_error_mat = np.array(pseud_error) # from list to array pseud_error_mat = pseud_error_mat.transpose() # transpose # get G matrix G_matrix_ = np.delete(G_matrix_, [0], axis=0) # delete the first row of G matrix delta_p = np.dot((G_matrix_.transpose()), G_matrix_) # G(T) * G delta_p_2 = np.linalg.inv(delta_p) # inverse matrix of G(T) * G delta_p = np.dot(delta_p_2, (G_matrix_.transpose() )) # multiply (inverse matrix of G(T) * G) and G(T) delta_p = np.dot(delta_p, pseud_error_mat) # multiply with pseud_error_mat rec_ini_pos_x = rec_ini_pos_x + float( delta_p[0]) # update receiver position in x direction rec_ini_pos_y = rec_ini_pos_y + float( delta_p[1]) # update receiver position in y idrection rec_ini_pos_z = rec_ini_pos_z + float( delta_p[2]) # update receiver position in z idrection rec_clo_bia_b = rec_clo_bia_b + float( delta_p[3]) # update receiver clock bias in meters devia_x = float(delta_p[0]) # save delta x devia_y = float(delta_p[1]) # save delta y devia_z = float(delta_p[2]) # save delta z devia_b = float(delta_p[3]) # save delta bias devia_xyz = sqrt(devia_x * devia_x + devia_y * devia_y + devia_z * devia_z) # get total bias # print 'delta_p',delta_p # print 'position estimation x=',rec_ini_pos_x # print 'position estimation y=', rec_ini_pos_y # print 'position estimation Z=', rec_ini_pos_z # print 'position estimation b=', rec_clo_bia_b # print 'position estimation devia_xyz=', devia_xyz del gues_pseud[:] # relief gues_pseud[] list del pseud_error[:] # relief pseud_error[] list return float(rec_ini_pos_x), float(rec_ini_pos_y), float( rec_ini_pos_z), float(rec_clo_bia_b), float(devia_xyz)
def nlosExclusion_(self, dataPB, dataNG, mode, manuSatLis): self.posArr = PoseArray() self.IniGNSS_ = GNSS_Raw_Array() self.nlosGNSS_ = GNSS_Raw_Array() self.nlosGNSSDel_ = GNSS_Raw_Array() self.mannuSat = manuSatLis self.nlExcMode = mode self.posArr = dataPB self.IniGNSS_ = dataNG self.nlosGNSS_ = dataNG self.lateralDis = 5.8 # for Transaction on Vehicle Technology self.GNSSCovariance = 0 self.getSatNum(self.IniGNSS_) lenPosArr_ = 0.0 lenPosArr_ = len( self.posArr) # print 'len(self.posArr)------',len(self.posArr) if (lenPosArr_ > 2): lenPosArr_ = 2 for i in range(lenPosArr_): # x--azimuth y--elevation self.bouSide1X.append( 1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) * np.cos((self.posArr[i].orientation.x - 90.0 + self.calAngN) * 3.14159 / 180.0)) # start point azimuth self.bouSide1Y.append( 1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) * np.sin((self.posArr[i].orientation.x - 90.0 + self.calAngN) * 3.14159 / 180.0)) # start point elevation azi_temp = 0.0 azi_temp = (math.atan2(self.bouSide1Y[-1], self.bouSide1X[-1])) * 180.0 / (math.pi) if (azi_temp < 0.0): azi_temp = azi_temp + 360 self.bouSide1IniX.append( float(azi_temp)) # initial azimuth start point # print 'start point azimuth IN PUNLOSEXCLUSION===', azi_temp,self.bouSide1IniX[-1],len(self.bouSide1IniX) self.bouSide2X.append( 1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) * np.cos((self.posArr[i].orientation.z - 90.0 + self.calAngN) * 3.14159 / 180.0)) # end point azimuth self.bouSide2Y.append( 1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) * np.sin((self.posArr[i].orientation.z - 90.0 + self.calAngN) * 3.14159 / 180.0)) # end point elevation azi_temp = (math.atan2(self.bouSide2Y[-1], self.bouSide2X[-1])) * 180.0 / (math.pi) if (azi_temp < 0.0): azi_temp = azi_temp + 360 self.bouSide2IniX.append( float(azi_temp)) # initial azimuth start point # print 'start point azimuth===', azi_temp for index_1 in range( len(self.IniGNSS_.GNSS_Raws )): # index all the satellite information in one epoch self.azim_.append( (self.IniGNSS_.GNSS_Raws[index_1].elevation * (-0.55556) + 50.0) * np.cos(-1 * (self.IniGNSS_.GNSS_Raws[index_1].azimuth - 90.0) * 3.14159 / 180.0)) self.elev_.append( (self.IniGNSS_.GNSS_Raws[index_1].elevation * (-0.55556) + 50.0) * np.sin(-1 * (self.IniGNSS_.GNSS_Raws[index_1].azimuth - 90.0) * 3.14159 / 180.0)) sateazi_ = ( 360 - (self.IniGNSS_.GNSS_Raws[index_1].azimuth) ) + 90 # change azimuth value into the coordinate that the same with the bus one (bouSide2X) if (sateazi_ > 360): sateazi_ = sateazi_ - 360 self.azimIni_.append(sateazi_) self.elevIni_.append(self.IniGNSS_.GNSS_Raws[index_1].elevation) self.satIdx.append( self.IniGNSS_.GNSS_Raws[index_1].prn_satellites_index) self.GNSSTime = self.IniGNSS_.GNSS_Raws[index_1].GNSS_time if (self.nlExcMode == 'statAuto'): for idx in range( len(self.bouSide1X) ): # index all the boundary line (usually only one double-decker bus, sometime 2) for index_sat in range( len(self.IniGNSS_.GNSS_Raws) ): # index all the satellite information in one epoch point0_x = 0.0 # yuanxin x point0_y = 0.0 # yuanxin y point1_x = float( self.bouSide1X[idx]) # point1 coordinate x point1_y = float( self.bouSide1Y[idx]) # point1 coordinate y point2_x = float( self.bouSide2X[idx]) # point2 coordinate x point2_y = float( self.bouSide2Y[idx]) # point2 coordinate y point_x = float(self.azim_[index_sat] ) # point_x (satellite azimuth in skyplot) point_y = float( self.elev_[index_sat] ) # point_y (satellite elevation in skyplot) self.DistanceThres_ = 10 area_error = math.fabs( self.IsInside(point1_x, point1_y, point2_x, point2_y, point0_x, point0_y, point_x, point_y)) if (self.bouSide1IniX[idx] > self.bouSide2IniX[idx]) and len( self.bouSide2IniX ) > 0: # make sure that bouSide1IniX is smaller azi_temp_ = self.bouSide2IniX[idx] self.bouSide2IniX[idx] = self.bouSide1IniX[idx] self.bouSide1IniX[idx] = azi_temp_ ang_ran_ = float(self.bouSide2IniX[idx] - self.bouSide1IniX[idx]) # print 'ang_ran_----------------------', ang_ran_, 'from', self.bouSide1IniX[idx], 'to', self.bouSide2IniX[idx] angDif1 = 0.0 angDif2 = 0.0 if (ang_ran_ < self.bouRang): angDif1 = float( math.fabs(self.azimIni_[index_sat] - self.bouSide1IniX[idx])) angDif2 = float( math.fabs(self.azimIni_[index_sat] - self.bouSide2IniX[idx])) # print 'angDif1= ',angDif1,'angDif2= ',angDif2 if (self.azimIni_[index_sat] > self.bouSide1IniX[idx] ) and (self.azimIni_[index_sat] < self.bouSide2IniX[idx]) and ( area_error > self.DistanceThres_) and ( angDif1 > 3) and (angDif2 > 3): # self.nlosGNSS_.GNSS_Raws[ index_sat].visable = 2 # for demonstration: this element will be deleted later self.satExcl_.append(self.satIdx[index_sat]) elif (ang_ran_ > self.bouRang): if (self.azimIni_[index_sat] > 0) and (self.azimIni_[index_sat] < self.bouSide1IniX[idx]) and ( area_error > self.DistanceThres_ ): # and (math.fabs(area_error)>3) : self.nlosGNSS_.GNSS_Raws[ index_sat].visable = 2 # for demonstration: this element will be deleted later self.satExcl_.append(self.satIdx[index_sat]) if (self.azimIni_[index_sat] < 360) and (self.azimIni_[index_sat] > self.bouSide2IniX[idx]) and ( area_error > self.DistanceThres_ ): # and (math.fabs(area_error)>3) : # print 'detect one NLOS signals at satellite-', self.satIdx[ # index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list', self.azimIni_[index_sat] # print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y self.nlosGNSS_.GNSS_Raws[ index_sat].visable = 2 # for demonstration: this element will be deleted later self.satExcl_.append(self.satIdx[index_sat]) self.GNSSNlos_pub.publish(self.nlosGNSS_) exclusionSatNum_ = exclusionSatNum() exclusionSatNum_.satlist = self.satExcl_ self.exclusionSatnum_Pub.publish(exclusionSatNum_) for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)): if (self.nlosGNSS_.GNSS_Raws[saIdx].visable != 2): # if satellite is none Light of sight (save) self.nlosGNSSDel_.GNSS_Raws.append( self.nlosGNSS_.GNSS_Raws[saIdx]) self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_) self.getExclSatNum() if (self.nlExcMode == 'statManu_Correction'): for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)): if ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index in self.mannuSat) and (len(self.nlosGNSS_.GNSS_Raws) > 9)): # if satellite is none Light of sight (save) # GNSS_Raw_Correction = GNSS_Raw() # afa = 4.0 # GNSS_Raw_Correction = self.nlosGNSS_.GNSS_Raws[saIdx] # kama1 = afa * ((1 / (math.cos(GNSS_Raw_Correction.elevation * math.pi / 180.0))) * (1 + math.cos(2 * GNSS_Raw_Correction.elevation * math.pi / 180.0))) # kama2 = afa * ((1 / (math.cos(GNSS_Raw_Correction.azimuth * math.pi / 180.0))) * (1 + math.cos(2 * GNSS_Raw_Correction.azimuth * math.pi / 180.0))) # kama = float(math.fabs(kama1) + math.fabs(kama2) ) # # GNSS_Raw_Correction.pseudorange = GNSS_Raw_Correction.pseudorange - kama # print 'kama1----', type(kama1),' kama2----', kama2,' kama----', kama, ' pseudorange----', GNSS_Raw_Correction.pseudorange self.nlosGNSSDel_.GNSS_Raws.append( self.nlosGNSS_.GNSS_Raws[saIdx]) ele = self.nlosGNSSDel_.GNSS_Raws[ -1].elevation * 3.14 / 180.0 azi = self.nlosGNSSDel_.GNSS_Raws[-1].azimuth * 3.14 / 180.0 cor_1 = self.lateralDis * cos(float(ele)) cor_1 = math.fabs(cor_1 * (1.0 + cos(2.0 * ele))) cor_2 = self.lateralDis * cos(float(azi)) cor_2 = math.fabs(cor_2 * (1.0 + cos(2.0 * azi))) print 'cor_1 ', cor_1, 'cor_2 ', cor_2 self.nlosGNSSDel_.GNSS_Raws[ -1].pseudorange = self.nlosGNSSDel_.GNSS_Raws[ -1].pseudorange - (cor_1 + cor_2) # if( (self.nlosGNSSDel_.GNSS_Raws[-1].elevation >=54 ) and (self.nlosGNSSDel_.GNSS_Raws[-1].elevation <= 72)): # self.nlosGNSSDel_.GNSS_Raws[-1].pseudorange = self.nlosGNSSDel_.GNSS_Raws[-1].pseudorange - (cor_1 + cor_2) # print 'self.nlosGNSSDel_.GNSS_Raws[-1].prn_satellites_index',self.nlosGNSSDel_.GNSS_Raws[-1].prn_satellites_index elif ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index not in self.mannuSat) or (len(self.nlosGNSS_.GNSS_Raws) <= 9)): #no correction # self.nlosGNSS_.GNSS_Raws[saIdx].visable =2 self.nlosGNSSDel_.GNSS_Raws.append( self.nlosGNSS_.GNSS_Raws[saIdx]) self.satExcl_ = self.mannuSat self.GNSSNlos_pub.publish(self.nlosGNSS_) self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_) exclusionSatNum_ = exclusionSatNum() exclusionSatNum_.satlist = self.satExcl_ self.exclusionSatnum_Pub.publish(exclusionSatNum_) self.getExclSatNum() if (self.nlExcMode == 'statManu_ExclusionClosedLoop'): for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)): dangerArea = 1 # if danger area, exclusion is needed if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 176810000) and (self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 176834000)): # print 'safe area ' dangerArea = 0 if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 176875000) and (self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 176908000)): dangerArea = 0 # print 'safe area ' # corner start: 176810000 #corner end: 176834000.0 # corner start: 176875000 # corner end: 176908000 cor_1 = 0 cor_2 = 0 # this satellites should be exlude if satellite is NLOS and the elevation angle is low if ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index in self.mannuSat) and (dangerArea == 1)): # if satellite is Light of sight (save) print 'cor_1 ', cor_1, 'cor_2 ', cor_2, 'GNSS time --', self.nlosGNSS_.GNSS_Raws[ -1].GNSS_time elif ((dangerArea == 0)): #no exclusion # self.nlosGNSS_.GNSS_Raws[saIdx].visable =2 self.nlosGNSSDel_.GNSS_Raws.append( self.nlosGNSS_.GNSS_Raws[saIdx]) self.GNSSCovariance = 4.3 + self.GNSSCovariance elif ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index not in self.mannuSat) and (dangerArea == 1)): #no exclusion # self.nlosGNSS_.GNSS_Raws[saIdx].visable =2 self.nlosGNSSDel_.GNSS_Raws.append( self.nlosGNSS_.GNSS_Raws[saIdx]) ele = self.nlosGNSS_.GNSS_Raws[ saIdx].elevation * 3.14 / 180.0 azi = self.nlosGNSS_.GNSS_Raws[saIdx].azimuth * 3.14 / 180.0 cor_1 = self.lateralDis * cos(float(ele)) cor_1 = math.fabs(cor_1 * (1.0 + cos(2.0 * ele))) cor_2 = self.lateralDis * cos(float(azi)) cor_2 = math.fabs(cor_2 * (1.0 + cos(2.0 * azi))) self.GNSSCovariance = self.GNSSCovariance + (cor_1 + cor_2) self.satExcl_ = self.mannuSat self.GNSSNlos_pub.publish(self.nlosGNSS_) self.GNSSCovariance = self.GNSSCovariance + 4.5 self.GNSSCovariance = self.GNSSCovariance * 0.6 print 'self.GNSSCovariance== ', self.GNSSCovariance self.nlosGNSSDel_.GNSS_Raws[-1].snr = self.GNSSCovariance self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_) exclusionSatNum_ = exclusionSatNum() exclusionSatNum_.satlist = self.satExcl_ self.exclusionSatnum_Pub.publish(exclusionSatNum_) self.getExclSatNum() if (self.nlExcMode == 'statManu_ExclusionOpenLoop'): for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)): dangerArea = 1 # if danger area, exclusion is needed if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 47745000) and (self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 47960000)): # print 'safe area ' self.mannuSat = [5, 24, 89, 13, 2, 88] #5,24 5,24,89,13,2,88 if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 47960000) and (self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 47990000)): self.mannuSat = [5, 90, 29, 15, 95, 87, 2, 88] # 5,90,29 5,90,29,15,95,87,2,88 # print 'safe area ' if ((self.nlosGNSS_.GNSS_Raws[-1].GNSS_time > 47990000) and (self.nlosGNSS_.GNSS_Raws[-1].GNSS_time < 48114000)): self.mannuSat = [24, 2, 87, 5, 88, 94, 90, 89] # 2,24,5 24,2,87,5,88,94,90,89 # print 'safe area ' # corner start: 176810000 #corner end: 176834000.0 # corner start: 176875000 # corner end: 176908000 cor_1 = 0 cor_2 = 0 # this satellites should be exlude if satellite is NLOS and the elevation angle is low if ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index in self.mannuSat) and (dangerArea == 1)): # if satellite is Light of sight (save) print 'cor_1 ', cor_1, 'cor_2 ', cor_2, 'GNSS time --', self.nlosGNSS_.GNSS_Raws[ -1].GNSS_time elif ((dangerArea == 0)): #no exclusion # self.nlosGNSS_.GNSS_Raws[saIdx].visable =2 self.nlosGNSSDel_.GNSS_Raws.append( self.nlosGNSS_.GNSS_Raws[saIdx]) self.GNSSCovariance = 4.3 + self.GNSSCovariance elif ((self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index not in self.mannuSat) and (dangerArea == 1)): #no exclusion # self.nlosGNSS_.GNSS_Raws[saIdx].visable =2 self.nlosGNSSDel_.GNSS_Raws.append( self.nlosGNSS_.GNSS_Raws[saIdx]) ele = self.nlosGNSS_.GNSS_Raws[ saIdx].elevation * 3.14 / 180.0 azi = self.nlosGNSS_.GNSS_Raws[saIdx].azimuth * 3.14 / 180.0 cor_1 = self.lateralDis * cos(float(ele)) cor_1 = math.fabs(cor_1 * (1.0 + cos(2.0 * ele))) cor_2 = self.lateralDis * cos(float(azi)) cor_2 = math.fabs(cor_2 * (1.0 + cos(2.0 * azi))) self.GNSSCovariance = self.GNSSCovariance + (cor_1 + cor_2) self.satExcl_ = self.mannuSat self.GNSSNlos_pub.publish(self.nlosGNSS_) self.GNSSCovariance = self.GNSSCovariance + 4.5 self.GNSSCovariance = self.GNSSCovariance * 0.6 print 'self.GNSSCovariance== ', self.GNSSCovariance self.nlosGNSSDel_.GNSS_Raws[-1].snr = self.GNSSCovariance self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_) exclusionSatNum_ = exclusionSatNum() exclusionSatNum_.satlist = self.satExcl_ self.exclusionSatnum_Pub.publish(exclusionSatNum_) self.getExclSatNum()
def CallGNSSRAWNlosDel_(self, data): # subscribe data after exclusion self.GNSSNRAWlosDel = GNSS_Raw_Array() self.GNSSNRAWlosDel = data
def correctNLOS(self, data): correctedGNSS_ = GNSS_Raw_Array()
def nlosExclusion_(self, dataPB, dataNG, mode, manuSatLis): self.posArr = PoseArray() self.IniGNSS_ = GNSS_Raw_Array() self.nlosGNSS_ = GNSS_Raw_Array() self.nlosGNSSDel_ = GNSS_Raw_Array() self.mannuSat = manuSatLis self.nlExcMode = mode self.posArr = dataPB self.IniGNSS_ = dataNG self.nlosGNSS_ = dataNG self.getSatNum(self.IniGNSS_) lenPosArr_ = 0.0 lenPosArr_ = len( self.posArr) # print 'len(self.posArr)------',len(self.posArr) if (lenPosArr_ > 2): lenPosArr_ = 2 for i in range(lenPosArr_): # x--azimuth y--elevation self.bouSide1X.append( 1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) * np.cos((self.posArr[i].orientation.x - 90.0 + self.calAngN) * 3.14159 / 180.0)) # start point azimuth self.bouSide1Y.append( 1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) * np.sin((self.posArr[i].orientation.x - 90.0 + self.calAngN) * 3.14159 / 180.0)) # start point elevation azi_temp = 0.0 azi_temp = (math.atan2(self.bouSide1Y[-1], self.bouSide1X[-1])) * 180.0 / (math.pi) if (azi_temp < 0.0): azi_temp = azi_temp + 360 self.bouSide1IniX.append( float(azi_temp)) # initial azimuth start point # print 'start point azimuth IN PUNLOSEXCLUSION===', azi_temp,self.bouSide1IniX[-1],len(self.bouSide1IniX) self.bouSide2X.append( 1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) * np.cos((self.posArr[i].orientation.z - 90.0 + self.calAngN) * 3.14159 / 180.0)) # end point azimuth self.bouSide2Y.append( 1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) * np.sin((self.posArr[i].orientation.z - 90.0 + self.calAngN) * 3.14159 / 180.0)) # end point elevation azi_temp = (math.atan2(self.bouSide2Y[-1], self.bouSide2X[-1])) * 180.0 / (math.pi) if (azi_temp < 0.0): azi_temp = azi_temp + 360 self.bouSide2IniX.append( float(azi_temp)) # initial azimuth start point # print 'start point azimuth===', azi_temp for index_1 in range( len(self.IniGNSS_.GNSS_Raws )): # index all the satellite information in one epoch self.azim_.append( (self.IniGNSS_.GNSS_Raws[index_1].elevation * (-0.55556) + 50.0) * np.cos(-1 * (self.IniGNSS_.GNSS_Raws[index_1].azimuth - 90.0) * 3.14159 / 180.0)) self.elev_.append( (self.IniGNSS_.GNSS_Raws[index_1].elevation * (-0.55556) + 50.0) * np.sin(-1 * (self.IniGNSS_.GNSS_Raws[index_1].azimuth - 90.0) * 3.14159 / 180.0)) sateazi_ = ( 360 - (self.IniGNSS_.GNSS_Raws[index_1].azimuth) ) + 90 # change azimuth value into the coordinate that the same with the bus one (bouSide2X) if (sateazi_ > 360): sateazi_ = sateazi_ - 360 self.azimIni_.append(sateazi_) self.elevIni_.append(self.IniGNSS_.GNSS_Raws[index_1].elevation) self.satIdx.append( self.IniGNSS_.GNSS_Raws[index_1].prn_satellites_index) self.GNSSTime = self.IniGNSS_.GNSS_Raws[index_1].GNSS_time if (self.nlExcMode == 'statAuto'): for idx in range( len(self.bouSide1X) ): # index all the boundary line (usually only one double-decker bus, sometime 2) for index_sat in range( len(self.IniGNSS_.GNSS_Raws) ): # index all the satellite information in one epoch point0_x = 0.0 # yuanxin x point0_y = 0.0 # yuanxin y point1_x = float( self.bouSide1X[idx]) # point1 coordinate x point1_y = float( self.bouSide1Y[idx]) # point1 coordinate y point2_x = float( self.bouSide2X[idx]) # point2 coordinate x point2_y = float( self.bouSide2Y[idx]) # point2 coordinate y point_x = float(self.azim_[index_sat] ) # point_x (satellite azimuth in skyplot) point_y = float( self.elev_[index_sat] ) # point_y (satellite elevation in skyplot) self.DistanceThres_ = 10 area_error = math.fabs( self.IsInside(point1_x, point1_y, point2_x, point2_y, point0_x, point0_y, point_x, point_y)) if (self.bouSide1IniX[idx] > self.bouSide2IniX[idx]) and len( self.bouSide2IniX ) > 0: # make sure that bouSide1IniX is smaller azi_temp_ = self.bouSide2IniX[idx] self.bouSide2IniX[idx] = self.bouSide1IniX[idx] self.bouSide1IniX[idx] = azi_temp_ ang_ran_ = float(self.bouSide2IniX[idx] - self.bouSide1IniX[idx]) # print 'ang_ran_----------------------', ang_ran_, 'from', self.bouSide1IniX[idx], 'to', self.bouSide2IniX[idx] angDif1 = 0.0 angDif2 = 0.0 if (ang_ran_ < self.bouRang): angDif1 = float( math.fabs(self.azimIni_[index_sat] - self.bouSide1IniX[idx])) angDif2 = float( math.fabs(self.azimIni_[index_sat] - self.bouSide2IniX[idx])) # print 'angDif1= ',angDif1,'angDif2= ',angDif2 if (self.azimIni_[index_sat] > self.bouSide1IniX[idx] ) and (self.azimIni_[index_sat] < self.bouSide2IniX[idx]) and ( area_error > self.DistanceThres_) and ( angDif1 > 3) and (angDif2 > 3): # self.nlosGNSS_.GNSS_Raws[ index_sat].visable = 2 # for demonstration: this element will be deleted later self.satExcl_.append(self.satIdx[index_sat]) elif (ang_ran_ > self.bouRang): if (self.azimIni_[index_sat] > 0) and (self.azimIni_[index_sat] < self.bouSide1IniX[idx]) and ( area_error > self.DistanceThres_ ): # and (math.fabs(area_error)>3) : self.nlosGNSS_.GNSS_Raws[ index_sat].visable = 2 # for demonstration: this element will be deleted later self.satExcl_.append(self.satIdx[index_sat]) if (self.azimIni_[index_sat] < 360) and (self.azimIni_[index_sat] > self.bouSide2IniX[idx]) and ( area_error > self.DistanceThres_ ): # and (math.fabs(area_error)>3) : # print 'detect one NLOS signals at satellite-', self.satIdx[ # index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list', self.azimIni_[index_sat] # print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y self.nlosGNSS_.GNSS_Raws[ index_sat].visable = 2 # for demonstration: this element will be deleted later self.satExcl_.append(self.satIdx[index_sat]) self.GNSSNlos_pub.publish(self.nlosGNSS_) exclusionSatNum_ = exclusionSatNum() exclusionSatNum_.satlist = self.satExcl_ self.exclusionSatnum_Pub.publish(exclusionSatNum_) for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)): if (self.nlosGNSS_.GNSS_Raws[saIdx].visable != 2): # if satellite is Light of sight (save) self.nlosGNSSDel_.GNSS_Raws.append( self.nlosGNSS_.GNSS_Raws[saIdx]) self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_) self.getExclSatNum() if (self.nlExcMode == 'statManu'): for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)): if (self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index not in self.mannuSat ): # if satellite is Light of sight (save) self.nlosGNSSDel_.GNSS_Raws.append( self.nlosGNSS_.GNSS_Raws[saIdx]) elif (self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index in self.mannuSat): # if satellite is Light of sight (save) self.nlosGNSS_.GNSS_Raws[saIdx].visable = 2 self.satExcl_ = self.mannuSat self.GNSSNlos_pub.publish(self.nlosGNSS_) self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_) exclusionSatNum_ = exclusionSatNum() exclusionSatNum_.satlist = self.satExcl_ self.exclusionSatnum_Pub.publish(exclusionSatNum_) self.getExclSatNum()
def __init__(self, parent=None): QMainWindow.__init__(self, parent) self.setWindowTitle('GNSS Evaluation') self.create_main_frame() self.bestPos_ = BESTPOS() self.navfixTruth_ = NavSatFix() self.navfix_ = NavSatFix() self.navfixprop_ = NavSatFix() self.GNSS_ = GNSS_Raw_Array() self.GNSSNRAWlosDel = GNSS_Raw_Array() self.SatNum = [] self.HDOP = [] self.error = [] self.errorEv = [] # error for further eveluation self.busAvail = [] # actual bus availibility self.GNSSTime = [] self.SatNumProp = [] self.HDOPProp = [] self.errorProp = [] self.errorPropEv = [] # error for further eveluation self.busAvailProp = [ ] # proposed bus availibility which is subjected to the performance of object detection self.GNSSTimeProp = [] self.lat_ = [] self.lon_ = [] self.latProp_ = [] self.lonProp_ = [] self.firstTime_ = 0.0 self.firstTimeProp_ = 0.0 self.preTime = 0.0 self.checkTimes = 0.0 self.finishSaving = 0.0 self.calAngNNLOS = 0.0 self.navfixAvailable = 0 self.navfixpropAvailable = 0 self.timer = QtCore.QTimer() QtCore.QObject.connect(self.timer, QtCore.SIGNAL("timeout()"), self.checkEnd) self.timer.start(3000) rospy.Subscriber('/novatel_data/bestpos', BESTPOS, self.callcptBestPos_) # Ground truth rospy.Subscriber('GNSS_', GNSS_Raw_Array, self.CallGNSS_) # Ublox Raw data rospy.Subscriber('novatel_data/inspvax', INSPVAX, self.callINSPVAXNLOS_) # yaw rospy.Subscriber( 'GNSS_NavFix', NavSatFix, self.GNSSNavCall) # conventional GNSS positioning result rospy.Subscriber( 'GNSS_NavFix_pro', NavSatFix, self.GNSSNavPropCall) # proposed GNSS positioning result rospy.Subscriber('GNSSRAWNlosDel_', GNSS_Raw_Array, self.CallGNSSRAWNlosDel_1) # Ublox rospy.Subscriber('velodyne_points_0', PointCloud2, self.PointCloudCall) # velodyne self.DOP_Pub = rospy.Publisher('DOP_', DOP, queue_size=100) # self.Error_Pub = rospy.Publisher('ErrorGNSSXYZ_', Error, queue_size=100) # self.DOPProp_Pub = rospy.Publisher('DOPProp_', DOP, queue_size=100) # self.ErrorProp_Pub = rospy.Publisher('ErrorGNSSXYZProp_', Error, queue_size=100) #
def CallGNSS_(self, data): # GNSS data # 1Hz self.GNSS_ = GNSS_Raw_Array() self.GNSS_ = data
def nlosExclusion_(self, dataPB, dataNG, mode, manuSatLis): self.posArr = PoseArray() self.IniGNSS_ = GNSS_Raw_Array() self.nlosGNSS_ = GNSS_Raw_Array() self.nlosGNSSDel_ = GNSS_Raw_Array() self.mannuSat= manuSatLis self.nlExcMode = mode self.posArr = dataPB self.IniGNSS_ = dataNG self.nlosGNSS_ = dataNG self.getSatNum(self.IniGNSS_) lenPosArr_ = 0.0 lenPosArr_ = len(self.posArr) print 'len(self.posArr)------',len(self.posArr) if(lenPosArr_ > 2): lenPosArr_ = 2 for i in range(lenPosArr_): # x--azimuth y--elevation azi_temp = float(self.posArr [i].orientation.x) # angle direction print 'start point azimuth===', azi_temp, 'self.calAngN', self.calAngN if (azi_temp < 0.0): azi_temp = azi_temp + 360 if (azi_temp > 360.0): azi_temp = azi_temp - 360.0 self.bouSide1IniX.append(float(azi_temp)) # initial azimuth start point azi_temp = float(self.posArr [i].orientation.z) # angle direction is reverse in skyplot and common coordinate print 'end point azimuth===', azi_temp, 'self.calAngN', self.calAngN if (azi_temp < 0): azi_temp = azi_temp + 360.0 if(azi_temp > 360.0): azi_temp = azi_temp -360.0 self.bouSide2IniX.append(azi_temp) # initial azimuth end point self.bouSide1X.append(1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) * np.cos((self.posArr[i].orientation.x - 90.0 + self.calAngN) * 3.14159 / 180.0)) # start point azimuth self.bouSide1Y.append(1 * (self.posArr[i].orientation.y * (-0.55556) + 50.0) * np.sin((self.posArr[i].orientation.x - 90.0 + self.calAngN) * 3.14159 / 180.0)) # start point elevation self.bouSide2X.append(1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) * np.cos((self.posArr[i].orientation.z - 90.0 + self.calAngN) * 3.14159 / 180.0)) # end point azimuth self.bouSide2Y.append(1 * (self.posArr[i].orientation.w * (-0.55556) + 50.0) * np.sin((self.posArr[i].orientation.z - 90.0 + self.calAngN) * 3.14159 / 180.0)) # end point elevation for index_1 in range(len(self.IniGNSS_.GNSS_Raws)): # index all the satellite information in one epoch self.azim_.append( (self.IniGNSS_.GNSS_Raws[index_1].elevation*(-0.55556)+50.0)* np.cos(-1*(self.IniGNSS_.GNSS_Raws[index_1].azimuth-90.0)*3.14159/180.0)) self.elev_.append( (self.IniGNSS_.GNSS_Raws[index_1].elevation*(-0.55556)+50.0)* np.sin(-1*(self.IniGNSS_.GNSS_Raws[index_1].azimuth-90.0)*3.14159/180.0)) self.azimIni_.append(self.IniGNSS_.GNSS_Raws[index_1].azimuth) self.elevIni_.append(self.IniGNSS_.GNSS_Raws[index_1].elevation) self.satIdx.append(self.IniGNSS_.GNSS_Raws[index_1].prn_satellites_index) self.GNSSTime = self.IniGNSS_.GNSS_Raws[index_1].GNSS_time if(self.nlExcMode=='statAuto'): for idx in range(len(self.bouSide1X)): # index all the boundary line (usually only one double-decker bus, sometime 2) for index_sat in range(len(self.IniGNSS_.GNSS_Raws)): # index all the satellite information in one epoch point0_x = 0.0 # yuanxin x point0_y = 0.0 # yuanxin y point1_x = float(self.bouSide1X[idx]) # point1 coordinate x point1_y = float(self.bouSide1Y[idx]) # point1 coordinate y point2_x = float(self.bouSide2X[idx]) # point2 coordinate x point2_y = float(self.bouSide2Y[idx]) # point2 coordinate y point_x = float(self.azim_[index_sat]) # point_x (satellite azimuth in skyplot) point_y = float(self.elev_[index_sat]) # point_y (satellite elevation in skyplot) area_error = math.fabs( self.IsInside(point1_x, point1_y, point2_x, point2_y, point0_x, point0_y, point_x, point_y)) if (self.bouSide1IniX[idx] < 0): self.bouSide1IniX[idx] = self.bouSide1IniX[idx] + 360 if(self.bouSide2IniX[idx]<0): self.bouSide2IniX[idx] = self.bouSide2IniX[idx] + 360 if (self.bouSide1IniX[idx] > self.bouSide2IniX[idx]) and len( self.bouSide2IniX) > 0: # make sure that listx_3_initial[i_bou_num] is smaller azi_temp = self.bouSide2IniX[idx] self.bouSide2IniX[idx] = self.bouSide1IniX[idx] self.bouSide1IniX[idx] = azi_temp ang_ran_ = float(self.bouSide2IniX[idx] - self.bouSide1IniX[idx]) # print 'ang_ran_----------------------', ang_ran_, 'from', self.bouSide1IniX[idx], 'to', self.bouSide2IniX[idx] if (ang_ran_ < self.bouRang): if (self.azimIni_[index_sat] > self.bouSide1IniX[idx]) and ( self.azimIni_[index_sat] < self.bouSide2IniX[idx]) and ( area_error > 5): # self.nlosGNSS_.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later self.satExcl_.append(self.satIdx[index_sat]) elif (ang_ran_ > self.bouRang): if (self.azimIni_[index_sat] > 0) and (self.azimIni_[index_sat] < self.bouSide1IniX[idx]) and (area_error > 5): # and (math.fabs(area_error)>3) : self.nlosGNSS_.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later self.satExcl_.append(self.satIdx[index_sat]) if (self.azimIni_[index_sat] < 360) and ( self.azimIni_[index_sat] > self.bouSide2IniX[idx]) and ( area_error > 5): # and (math.fabs(area_error)>3) : # print 'detect one NLOS signals at satellite-', self.satIdx[ # index_sat], 'area_error', area_error, 'satellite_initial_azimuth_list', self.azimIni_[index_sat] # print 'point1_x point1_y point2_x point2_y point_x point_y', point1_x, point1_y, point2_x, point2_y, point_x, point_y self.nlosGNSS_.GNSS_Raws[index_sat].visable = 2 # for demonstration: this element will be deleted later self.satExcl_.append(self.satIdx[index_sat]) self.GNSSNlos_pub.publish(self.nlosGNSS_) for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)): if (self.nlosGNSS_.GNSS_Raws[saIdx].visable != 2): # if satellite is Light of sight (save) self.nlosGNSSDel_.GNSS_Raws.append(self.nlosGNSS_.GNSS_Raws[saIdx]) self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_) self.getExclSatNum() if (self.nlExcMode == 'statManu'): for saIdx in range(len(self.nlosGNSS_.GNSS_Raws)): if (self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index not in self.mannuSat): # if satellite is Light of sight (save) self.nlosGNSSDel_.GNSS_Raws.append(self.nlosGNSS_.GNSS_Raws[saIdx]) elif(self.nlosGNSS_.GNSS_Raws[saIdx].prn_satellites_index in self.mannuSat): # if satellite is Light of sight (save) self.nlosGNSS_.GNSS_Raws[saIdx].visable =2 self.satExcl_ = self.mannuSat self.GNSSNlos_pub.publish(self.nlosGNSS_) self.GNSSNlosDel_pub.publish(self.nlosGNSSDel_) self.getExclSatNum() print 'self.satExcl_=',self.satExcl_,len(self.nlosGNSSDel_.GNSS_Raws),len(self.nlosGNSS_.GNSS_Raws),self.nlosGNSS_.GNSS_Raws[0].total_sv
def CallGNSSRAWNlosDel_1(self,data): self.GNSSNRAWlosDel = GNSS_Raw_Array() self.GNSSNRAWlosDel = data