def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update
pointCloud_ = PointCloud2()
pointCloud_ = data
self.graphslam_PointCloud_pub.publish(pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statManu', self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1) and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time != self.GNSSNRAWlosDelTimeFlag)): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
geopoint.position.latitude = float(llh_[0])
geopoint.position.longitude = float(llh_[1])
geopoint.position.altitude = float(llh_[2])
self.graphslam_GeoPoint_pub.publish(geopoint)
def iterPosCal(self, GNSS_one_epoch_, calMode):
iterations = 0
itera_x = 0
itera_y = 0
itera_z = 0
self.calMode = calMode
self.getSatNum(GNSS_one_epoch_) # get satellites numbers
if (self.calMode
== 'LSGNSS') and (self.GPSNum >= 4) and (self.BeiNum > 1):
itera_x, itera_y, itera_z, itera_b_GPS, itera_b_Beidou, itera_bias_ = self.LSGNSSPosCalStep(
GNSS_one_epoch_, 0.1, 0.1, 0.1, 1.0,
1.0) # first iteration from (0.1, 0.1, 0.1, 1.0)
self.iterations_ = self.iterations_ + 1
while (
itera_bias_ > 1e-4
) and iterations < 10: # threshold for iterations:value and times
itera_x, itera_y, itera_z, itera_b_GPS, itera_b_Beidou, itera_bias_ = self.LSGNSSPosCalStep(
GNSS_one_epoch_, itera_x, itera_y, itera_z, itera_b_GPS,
itera_b_Beidou) # iteration
self.iterations_ = self.iterations_ + 1
iterations = iterations + 1 # add one iteration
elif (self.calMode == 'LSGPS') and (self.GPSNum >= 4):
itera_x, itera_y, itera_z, itera_b, itera_bias_ = self.LSGPSPosCalStep(
GNSS_one_epoch_, 0.1, 0.1, 0.1,
1.0) # first iteration from (0.1, 0.1, 0.1, 1.0)
self.iterations_ = self.iterations_ + 1
while (
itera_bias_ > 1e-4
) and iterations < 10: # threshold for iterations:value and times
itera_x, itera_y, itera_z, itera_b, itera_bias_ = self.LSGPSPosCalStep(
GNSS_one_epoch_, itera_x, itera_y, itera_z, itera_b)
self.iterations_ = self.iterations_ + 1
iterations = iterations + 1 # add one iteration
elif (self.calMode
== 'WLSGNSS') and (self.GPSNum >= 4) and (self.BeiNum > 1):
itera_x, itera_y, itera_z, itera_b_GPS, itera_b_Beidou, itera_bias_ = self.WLSGNSSPosCalStep(
GNSS_one_epoch_, 0.1, 0.1, 0.1, 1.0,
1.0) # first iteration from (0.1, 0.1, 0.1, 1.0)
self.iterations_ = self.iterations_ + 1
while (
itera_bias_ > 1e-4
) and iterations < 10: # threshold for iterations:value and times
itera_x, itera_y, itera_z, itera_b_GPS, itera_b_Beidou, itera_bias_ = self.WLSGNSSPosCalStep(
GNSS_one_epoch_, itera_x, itera_y, itera_z, itera_b_GPS,
itera_b_Beidou) # iteration
self.iterations_ = self.iterations_ + 1
iterations = iterations + 1 # add one iteration
print 'itera_b_GPS', itera_b_GPS, 'itera_b_Beidou-----------------------------------------', itera_b_Beidou
self.ecef_.append(float(itera_x))
self.ecef_.append(float(itera_y))
self.ecef_.append(float(itera_z))
self.pseudoResCal(GNSS_one_epoch_)
self.PosError()
self.llh_ = ecef2llh.xyz2llh(self.ecef_) # ecef to llh coordinates
iterations = 0.0 # initialize iterations variable
def CallGNSS_(self, data): # GNSS data # 1Hz
self.GNSS_ = data
self.puGNSSPosCalF_ = puGNSSPosCal.GNSSPosCal()
self.puGNSSPosCalF_.iterPosCal(self.GNSS_, 'WLSGNSS')
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_.ecef_)
navfix_.header.stamp.secs = self.GNSS_.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.GNSS_Navfix_pub.publish(navfix_) # print 'ecef',self.puGNSSPosCalF_.ecef_,'llh',llh_
def PointCloudCall(
self, data
): # 20 Hz As this function has highest frequency, good for update
self.pointCloud_ = PointCloud2()
self.pointCloud_ = data
# self.graphslam_PointCloud_pub.publish(self.pointCloud_) # for Graph slam
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_,
'statManu_ExclusionOpenLoop',
self.excluSatLis) # statManu
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if ((len(self.GNSSNRAWlosDel.GNSS_Raws) > 1)
and (self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time !=
self.GNSSNRAWlosDelTimeFlag)
): # only if there is a change, there will conduct the calculation
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
self.GNSSNRAWlosDelTimeFlag = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[
-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
# for Graph slam
graphNavfix_ = NavSatFix()
graphNavfix_ = navfix_
graphNavfix_.header = self.pointCloud_.header
self.graphslam_Navfix_pub.publish(graphNavfix_)
# for Graph slam
geopoint = GeoPointStamped()
geopoint.header = graphNavfix_.header
# calculate the ENU coordiantes initialLLH
enu_ = pugeomath.transformEcefToEnu(self.initialLLH,
self.puGNSSPosCalF_prop.ecef_)
print 'enu_ ->: ', enu_
geopoint.position.latitude = float(enu_[0])
geopoint.position.longitude = float(enu_[1])
# geopoint.position.altitude = float(llh_[2]) # use this to save the covariance in altitude component
geopoint.position.altitude = self.GNSSNRAWlosDel.GNSS_Raws[
-1].snr * self.HDOPProp # save the covariance
self.graphslam_GeoPoint_pub.publish(geopoint)
def PointCloudCall(self,data): # 20 Hz As this function has highest frequency, good for update
pointCloud_ = PointCloud2()
pointCloud_ = data
self.nlosExclusionF_ = puNlosExclusion.nlosExclusion(self.calAngNNLOS)
self.nlosExclusionF_.nlosExclusion_(self.posArr, self.GNSS_, 'statAuto', self.excluSatLis)
self.puGNSSPosCalF_prop = puGNSSPosCal.GNSSPosCal()
if(len(self.GNSSNRAWlosDel.GNSS_Raws) > 1):
# print 'self.GNSSNRAWlosDel',len(self.GNSSNRAWlosDel.GNSS_Raws)
self.puGNSSPosCalF_prop.iterPosCal(self.GNSSNRAWlosDel, 'WLSGNSS')
navfix_ = NavSatFix()
llh_ = []
llh_ = ecef2llh.xyz2llh(self.puGNSSPosCalF_prop.ecef_)
navfix_.header.stamp.secs = self.GNSSNRAWlosDel.GNSS_Raws[-1].GNSS_time
navfix_.latitude = float(llh_[0])
navfix_.longitude = float(llh_[1])
navfix_.altitude = float(llh_[2])
self.propGNSS_Navfix_pub.publish(navfix_)
def standardGT2kml(self):
print 'begin read standard data'
self.Fcsv_GNSS = csv.reader(
open('/home/wws/map_trajectory.csv',
'r')) # read csv context to csv_reader variable
for rowCsv in self.Fcsv_GNSS:
# self.lat_.append(float(rowCsv[1]))
# self.lon_.append(float(rowCsv[2]))
east = rowCsv[1]
north = rowCsv[2]
up = rowCsv[3]
prex_ = float(east)
prey_ = float(north)
origin_azimuth = 348.747632943 + 180 - 1.35 # 1.5
theta = -1 * (origin_azimuth - 90) * (3.141592 / 180.0)
# east = (prex_ * cos(theta) - prey_ * sin(theta)) - 1.3
# north = (prex_ * sin(theta) + prey_ * cos(theta)) + 1.3
east = (prex_ * cos(theta) - prey_ *
sin(theta)) + 1.6 # best suit for data in moko east
north = (prex_ * sin(theta) +
prey_ * cos(theta)) + 1.3 # best suit moko east
self.ENU = []
self.ENU.append(east)
self.ENU.append(north)
self.ENU.append(up)
self.ecef = enu2ecef.enu2ecef(self.oriLLH, self.ENU)
print 'self.ENU-> ', self.ENU, '\n'
print 'self.ecef-> ', self.ecef, '\n'
self.llh_cal = []
self.llh_cal = ecef2llh.xyz2llh(self.ecef)
self.lat_.append(float(self.llh_cal[0]))
self.lon_.append(float(self.llh_cal[1]))
print 'self.llh_cal-> ', self.llh_cal, '\n'
print 'finish tranversal standard data', len(self.lat_)
def csv2topic():
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('csv2topic',
anonymous=True) #initialize node with specific node name
GNSS_Raw_pub = rospy.Publisher(
'GNSS_RAW', GNSS_Raw_Array,
queue_size=10) #customerized GNSS raw data ros message
Navsatfix_pub = rospy.Publisher(
'navsatfix_llh', NavSatFix,
queue_size=10) #standard GNSSs meaasge with lat and longt
rospy.Subscriber('GNSS_exclued_data', GNSS_Raw_Array,
callback_GNSS_data_after_NLOS_exclusion)
rospy.Subscriber('Satellite_Info', Satellite_Info,
callback_Satellite_NLOS_exclusion)
GNSS_Raw_Array_1 = GNSS_Raw_Array(
) #create an variable in GNSS_Raw_Array format
GNSS_options = 1 # 0-GPS only 1-hybrid GNSS (GPS+Beidou)
sate_index_range = 330 # GPS only (sate_index_range=33) Hybrid GNSS positioning (sate_index_range=330)
line_index = 0 # variable represents line index in csv file
gnss_time = 0 # GNSS time contained in csv file for each epoch (epoch: each time point)
previous_gnss_time = 0.0 # previous GNSS time contained in csv file for each epoch (epoch: each time point)
previoustime = time.time() #initialize variable
currenttime = time.time() #initialize variable
plot_time = 0.0
#get data from .csv file--------------------begin----------------------------
for row_csv in csv_reader: #index all rows in csv file
plt.figure(1) # create figure 1
ax1 = plt.subplot(3, 1, 1)
ax2 = plt.subplot(3, 1, 2)
ax3 = plt.subplot(3, 1, 3)
gnss_time = row_csv[0] #get first data in each row (GNSS time exactly)
GNSS_Raw_1 = GNSS_Raw(
) # create customerized variable in GNSS_Raw format (element for GNSS_Raw_Array)
NavSatFix_llh = NavSatFix(
) # creat standard variable in NavSatFix format
# print '--------------begin one row----------'
# print row_csv[12],len(row_csv),type(row_csv)
# data_test=float(row_csv[3])
GNSS_Raw_1.GNSS_time = float(
row_csv[0]) # GNSS time contained in csv file
GNSS_Raw_1.total_sv = float(
row_csv[1]
) # total sitellites in one epoch (epoch: each time point)
GNSS_Raw_1.prn_satellites_index = float(
row_csv[2]
) # satellite index in this epoch (epoch: each time point)
GNSS_Raw_1.pseudorange = float(row_csv[3]) - float(row_csv[7]) - float(
row_csv[8]) + float(row_csv[9]) # pseudorange measurement
GNSS_Raw_1.snr = float(
row_csv[4]) # signal noise ratio for this satellite
GNSS_Raw_1.elevation = float(
row_csv[5]) # elevation for this satellite and receiver
GNSS_Raw_1.azimuth = float(row_csv[6]) # azimuth for this satellite
GNSS_Raw_1.err_tropo = float(row_csv[7]) # troposphere error in meters
GNSS_Raw_1.err_iono = float(row_csv[8]) # ionophere error in meters
GNSS_Raw_1.sat_clk_err = float(
row_csv[9]) # satellite clock bias caused error
GNSS_Raw_1.sat_pos_x = float(
row_csv[10]) # satellite positioning in x direction
GNSS_Raw_1.sat_pos_y = float(
row_csv[11]) # satellite positioning in y direction
GNSS_Raw_1.sat_pos_z = float(
row_csv[12]) # satellite positioning in Z direction
GNSS_Raw_1.visable = 0 # satellite visability juded by NLOS exclusion
if ((previous_gnss_time == gnss_time)
or (line_index == 0)): # differentiate each epoch
if (
GNSS_Raw_1.prn_satellites_index < sate_index_range
): ## GPS only (sate_index_range=33) Hybrid GNSS positioning (sate_index_range=330)
GNSS_Raw_Array_1.GNSS_Raws.append(
GNSS_Raw_1) #save data for each epoch
else:
GNSS_Raw_Array_1.header.frame_id = 'GNSS_Raw_' # set frame_id
# for del_index in range(len(GNSS_Raw_Array_1.GNSS_Raws)): # index all the satellites' visability in one epoch
# if (del_index >= len(GNSS_Raw_Array_1.GNSS_Raws)):
# break
# if (GNSS_Raw_Array_1.GNSS_Raws[del_index].elevation <= 20): # if satellite is low elevation satellite, delete this satellite
# del GNSS_Raw_Array_1.GNSS_Raws[del_index] # delete the low elevation satellites
GNSS_Raw_pub.publish(
GNSS_Raw_Array_1
) # publish GNSS_Raw_Array topic after one epoch
plot_time = plot_time + 1
plot_RawHdop.append(float(
DopCalculation(GNSS_Raw_Array_1))) # calculate dop matrix
plot_RawHdopTim.append(float(GNSS_Raw_1.GNSS_time))
GPS_satellite_number = []
Beidou_satellite_number = []
GPS_satellite_number, Beidou_satellite_number = get_GPS_Beidou_satellite_numbers(
GNSS_Raw_Array_1)
plot_Total_sate.append(
len(GPS_satellite_number) +
len(Beidou_satellite_number)) # save the Total satellites
plot_Beidou_sate.append(
len(Beidou_satellite_number)) # save the Beidou satellites
plot_GPS_sate.append(
len(GPS_satellite_number)) # save the GPS satellites
plot_Total_sate_Tim.append(float(
GNSS_Raw_1.GNSS_time)) # save the tinme for total satellites
print ' one epoch...... ', 'BeiN', len(
GPS_satellite_number), 'GPSN', len(
Beidou_satellite_number
) # indicates one epoch (epoch: each time point)
iterations = 0.0 # initialize iterations times
itera_x = 0.0 # initialize receiver's position x
itera_y = 0.0 # initialize receiver's position y
itera_z = 0.0 # initialize receiver's position z
itera_b = 0.0 # initialize receiver's clock bias error of GPS only positioning in meters
itera_bias_ = 0.0 # initialize total bias of GPS only positioning in each iteration
itera_b_GPS = 0.0 # initialize receiver's clock bias error of GPS in Hybrid GNSS positioning in meters
itera_b_Beidou = 0.0 # initialize receiver's clock bias error of Beidou in Hybrid GNSS positioning in meters
if (len(GNSS_Raw_Array_1.GNSS_Raws) > 3) and (
GNSS_options == 0
): #at least four satellites for least square calculation with GPS only
itera_x, itera_y, itera_z, itera_b, itera_bias_ = weighted_lesat_square_GNSS_positioning(
GNSS_Raw_Array_1, 0.1, 0.1, 0.1,
1.0) # first iteration from (0.1, 0.1, 0.1, 1.0)
while (
itera_bias_ > 1e-4
) and iterations < 10: #threshold for iterations:value and times
itera_x, itera_y, itera_z, itera_b, itera_bias_ = weighted_lesat_square_GNSS_positioning(
GNSS_Raw_Array_1, itera_x, itera_y, itera_z,
itera_b) # iteration
iterations = iterations + 1 # add one iteration
if (len(GNSS_Raw_Array_1.GNSS_Raws) > 4) and (
len(Beidou_satellite_number) >= 1
) and (
GNSS_options == 1
): # at least five satellites (at least one Beidou) for least square calculation with Hybrid GNSS
itera_x, itera_y, itera_z, itera_b_GPS, itera_b_Beidou, itera_bias_ = weighted_lesat_square_GNSS_positioning_hybrid_GNSS(
GNSS_Raw_Array_1, 0.1, 0.1, 0.1, 1.0,
1.0) # first iteration from (0.1, 0.1, 0.1, 1.0)
while (
itera_bias_ > 1e-4
) and iterations < 10: # threshold for iterations:value and times
itera_x, itera_y, itera_z, itera_b_GPS, itera_b_Beidou, itera_bias_ = weighted_lesat_square_GNSS_positioning_hybrid_GNSS(
GNSS_Raw_Array_1, itera_x, itera_y, itera_z,
itera_b_GPS, itera_b_Beidou) # iteration
iterations = iterations + 1 # add one iteration
print 'iterations=', iterations, 'itera_bias_=', itera_bias_ # how many iterations after sucessfully least square calculation, total bioas
print 'skyplot_python_subscribe.plot_total_sate', skyplot_python_subscribe.plot_total_sate
iterations = 0.0 #initialize iterations variable
#get llh information
if (
itera_x != 0
): # make sure that itera_x is not zero value, otherwise arctan(itera_y/itera_x) in xyz2llh function
itera_xyz = [] # create a list to save ecef coordiante
itera_xyz.append(float(itera_x)) # save x value
itera_xyz.append(float(itera_y)) # save y value
itera_xyz.append(float(itera_z)) # save z value
llh_output = ecef2llh.xyz2llh(
itera_xyz) # ecef to llh coordinates
if (latDic.has_key(str(int(gnss_time)))):
truthList = [] # create a list to save ground truth
truthList.append(float(latDic[str(
int(gnss_time))])) # get truth lat
truthList.append(float(lonDic[str(
int(gnss_time))])) # get truth lon
xyzTruth = llh2ecef.llh2xyz(truthList) # get xyz from ecef
delt_x = (xyzTruth[0] - itera_xyz[0]) * (
xyzTruth[0] - itera_xyz[0]) # get delta x (Error in x)
delt_y = (xyzTruth[1] - itera_xyz[1]) * (
xyzTruth[1] - itera_xyz[1]) # get delta y (Error in y)
delt_z = (xyzTruth[2] - itera_xyz[2]) * (
xyzTruth[2] - itera_xyz[2]) # get delta z (Error in z)
delErrTem = sqrt(delt_x + delt_y + delt_z)
if (delErrTem > 100):
delErrTem = 100
plot_RGNSSyGtBias.append(delErrTem) # save error
GNSSTimeList.append(float(GNSS_Raw_1.GNSS_time))
# print 'xyzTruth=',xyzTruth
# print 'llh output=',llh_output,'line_index=',line_index,'itera_xyz=',itera_xyz # llh coordinates and line index
NavSatFix_llh.latitude = float(
llh_output[0]) # save latitude to NavSatFix_llh message
NavSatFix_llh.longitude = float(
llh_output[1]) # save longitude to NavSatFix_llh message
plot_RawGNSSlat.append(float(
llh_output[0])) # save raw GNSS latitude for plotting
plot_RawGNSSlon.append(float(
llh_output[1])) # save raw GNSS longitude for plotting
Navsatfix_pub.publish(
NavSatFix_llh) #publish NavSatFix_llh message
#route of vehicles in matplotlib
plt.sca(ax1)
# plt.plot(lonTruth,latTruth, 'o', color='r',label="ground truth") # plot discrete position of receiver (Ground Truth)
# plt.plot(plot_RawGNSSlon , plot_RawGNSSlat,'*', color='g',label="GNSS positioning") # plot discrete position of receiver
listLatRaw.append(float(llh_output[0])) # save Raw Lat
listLonRaw.append(float(llh_output[1])) # save Raw lon
if (len(list_lat) > 0):
plot_ExclLat.append(float(list_lon[0]))
plot_ExclLon.append(float(list_lat[0]))
# plt.plot(list_lon,list_lat, '*', color='g')
del list_lat[:]
del list_lon[:]
plt.plot(plot_Beidou_sate_excluded_Tim,
plot_Beidou_sate_excluded_,
'*-',
color='r',
label="Excluded Beidou numbers"
) # plot discrete position of receiver
plt.plot(plot_Beidou_sate_excluded_Tim,
plot_GPS_sate_excluded_,
'*-',
color='g',
label="Excluded GPS Numbers"
) # plot discrete position of receiver
plt.plot(plot_Total_sate_Tim,
plot_Beidou_sate,
'*-',
linewidth='2',
color='b',
label="total Beid Numbers"
) # plot discrete position of receiver
plt.plot(plot_Total_sate_Tim,
plot_GPS_sate,
'*-',
linewidth='2',
color='fuchsia',
label="total GPS Numbers"
) # plot discrete position of receiver
ax1.legend(loc=10, ncol=3, shadow=True)
# plt.title('satellite exclusion numbers') # set title
plt.sca(ax2) # plot in another sca
plt.plot(GNSSTimeList,
plot_RGNSSyGtBias,
'*-',
color='g',
label="RawGNSSerr") # raw GNSS positioning error
plt.plot(plot_ExclGNSSyGtBiasTim,
plot_ExclGNSSyGtBias,
'*-',
linewidth='4',
color='r',
label="ExclGNSSerr") # excluded GNSS positioning error
ax2.legend(loc=9, ncol=3, shadow=True)
plt.sca(ax3) # plot in another sca
plt.xlabel('t/s---GPS time', fontdict={'size': 15, 'color': 'r'})
plt.plot(plot_RawHdopTim,
plot_RawHdop,
'*-',
linewidth='2',
color='g',
label="HDOP error") # Raw GNSS Hdop error
plt.plot(plot_excludedHdopTim,
plot_excludedHdop,
'*-',
linewidth='4',
color='r',
label="Excluded HDOP error") # excluded GNSS Hdop error
ax3.legend(loc=9, ncol=3, shadow=True)
plt.draw() # draw figure
plt.pause(
0.00000000001) # pause for a while to immitate multi-thread
time.sleep(0.4) # control frequency of topic publishment
plt.clf() # clean the plt
del GNSS_Raw_Array_1.GNSS_Raws[:] #relief the content
if (
GNSS_Raw_1.prn_satellites_index < sate_index_range
): # # GPS only (sate_index_range=33) Hybrid GNSS positioning (sate_index_range=330)
GNSS_Raw_Array_1.GNSS_Raws.append(
GNSS_Raw_1) # save GNSS_Raws to GNSS_Raw_Array_1 variable
previous_gnss_time = gnss_time # save time to previous time
line_index = line_index + 1 # add one line index
with open(
filename,
'w') as file_object: # save listLatRaw and listLonRaw to csv file
for sav_idx in range(len(listLatWiNLOSExc)):
str2 = str(listLatWiNLOSExc[sav_idx]) + ',' + str(
listLonWiNLOSExc[sav_idx])
file_object.write(str2)
file_object.write('\n')
# print 'str1 in one line....',str2
# print 'finish saving llhWitNLOSExlu.csv ... with ',listLatWiNLOSExc,'items'
with open(filename_2, 'w'
) as file_object_2: # save listLatRaw and listLonRaw to csv file
for sav_idx_2 in range(len(listLatRaw)):
str1 = str(listLatRaw[sav_idx_2]) + ',' + str(
listLonRaw[sav_idx_2])
file_object_2.write(str1)
file_object_2.write('\n')
# plt.draw()
rate = rospy.Rate(1) # 1hz
while not rospy.is_shutdown():
hello_str = "current time is %s" % rospy.get_time()
rate.sleep()
"""
def callback_GNSS_data_after_NLOS_exclusion(
data
): # callback function: get GNSS data (after NLOS exclusion) and implement least square
GNSS_exclusion_Array_1 = GNSS_Raw_Array(
) # create an variable in GNSS_Raw_Array format to save GNSS data after exclusion
GNSS_exclusion_Array_1 = data # get data from ros topic
GNSS_options = 1 # 0-GPS only 1-hybrid GNSS (GPS+Beidou)
sate_index_range = 330 # GPS only (sate_index_range=33) Hybrid GNSS positioning (sate_index_range=330)
GPS_satellite_number = []
Beidou_satellite_number = []
GPS_satellite_number, Beidou_satellite_number = get_GPS_Beidou_satellite_numbers(
GNSS_exclusion_Array_1) # get satellites numbers
iterations = 0.0 # initialize iterations times
itera_x = 0.0 # initialize receiver's position x
itera_y = 0.0 # initialize receiver's position y
itera_z = 0.0 # initialize receiver's position z
itera_b = 0.0 # initialize receiver's clock bias error of GPS only positioning in meters
itera_bias_ = 0.0 # initialize total bias of GPS only positioning in each iteration
itera_b_GPS = 0.0 # initialize receiver's clock bias error of GPS in Hybrid GNSS positioning in meters
itera_b_Beidou = 0.0 # initialize receiver's clock bias error of Beidou in Hybrid GNSS positioning in meters
if (len(GNSS_exclusion_Array_1.GNSS_Raws) > 3) and (
GNSS_options == 0
): # at least four satellites for least square calculation with GPS only
itera_x, itera_y, itera_z, itera_b, itera_bias_ = weighted_lesat_square_GNSS_positioning(
GNSS_exclusion_Array_1, 0.1, 0.1, 0.1,
1.0) # first iteration from (0.1, 0.1, 0.1, 1.0)
while (
itera_bias_ > 1e-4
) and iterations < 10: # threshold for iterations:value and times
itera_x, itera_y, itera_z, itera_b, itera_bias_ = weighted_lesat_square_GNSS_positioning(
GNSS_exclusion_Array_1, itera_x, itera_y, itera_z,
itera_b) # iteration
iterations = iterations + 1 # add one iteration
if (len(GNSS_exclusion_Array_1.GNSS_Raws) > 4) and (
len(Beidou_satellite_number) >= 1
) and (
GNSS_options == 1
): # at least five satellites (at least one Beidou) for least square calculation with Hybrid GNSS
itera_x, itera_y, itera_z, itera_b_GPS, itera_b_Beidou, itera_bias_ = weighted_lesat_square_GNSS_positioning_hybrid_GNSS(
GNSS_exclusion_Array_1, 0.1, 0.1, 0.1, 1.0,
1.0) # first iteration from (0.1, 0.1, 0.1, 1.0)
while (
itera_bias_ > 1e-4
) and iterations < 10: # threshold for iterations:value and times
itera_x, itera_y, itera_z, itera_b_GPS, itera_b_Beidou, itera_bias_ = weighted_lesat_square_GNSS_positioning_hybrid_GNSS(
GNSS_exclusion_Array_1, itera_x, itera_y, itera_z, itera_b_GPS,
itera_b_Beidou) # iteration
iterations = iterations + 1 # add one iteration
# print 'iterations after NLOS exclusion=', iterations, 'itera_bias_=', itera_bias_ # how many iterations after sucessfully least square calculation, total bioas
iterations = 0.0 # initialize iterations variable
# get llh information
if (
itera_x != 0
): # make sure that itera_x is not zero value, otherwise arctan(itera_y/itera_x) in xyz2llh function
itera_xyz = [] # create a list to save ecef coordiante
itera_xyz.append(float(itera_x)) # save x value
itera_xyz.append(float(itera_y)) # save y value
itera_xyz.append(float(itera_z)) # save z value
llh_output = ecef2llh.xyz2llh(itera_xyz) # ecef to llh coordinates
list_lat.append(float(llh_output[0])) # save latitude
list_lon.append(float(llh_output[1])) # save longitude
listLatWiNLOSExc.append(float(
llh_output[0])) # save latitude for csv saving
listLonWiNLOSExc.append(float(
llh_output[1])) # save latitude for csv saving
if (latDic.has_key(
str(int(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time)))):
truthList = [] # create a list to save ground truth
truthList.append(
float(latDic[str(
int(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time))])
) # get truth lat
truthList.append(
float(lonDic[str(
int(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time))])
) # get truth lon
xyzTruth = llh2ecef.llh2xyz(truthList) # get xyz from ecef
delt_x = (xyzTruth[0] - itera_xyz[0]) * (
xyzTruth[0] - itera_xyz[0]) # get delta x (Error in x)
delt_y = (xyzTruth[1] - itera_xyz[1]) * (
xyzTruth[1] - itera_xyz[1]) # get delta y (Error in y)
delt_z = (xyzTruth[2] - itera_xyz[2]) * (
xyzTruth[2] - itera_xyz[2]) # get delta z (Error in z)
delErrTem = sqrt(delt_x + delt_y + delt_z)
if (delErrTem > 100):
delErrTem = 100
plot_ExclGNSSyGtBias.append(float(delErrTem)) # save error
plot_ExclGNSSyGtBiasTim.append(
float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time))
# print 'xyzTruth afer exclusion=', xyzTruth
# print 'llh output after NLOS exclusion==', llh_output
plot_excludedHdop.append(float(DopCalculation(GNSS_exclusion_Array_1)))
plot_excludedHdopTim.append(
float(GNSS_exclusion_Array_1.GNSS_Raws[0].GNSS_time))
del GNSS_exclusion_Array_1.GNSS_Raws[:] # relief the content