def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo, maxsats=10)
if len(rtcm) != 0:
#print(rtcm)
rtcmfile.write(rtcm)
port.sendto(rtcm[:-2], (opts.udp_addr, opts.udp_port))
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print(rtcm)
rtcmfile.write(rtcm)
port.sendto(rtcm[:-2], (opts.udp_addr, opts.udp_port))
errset = {}
for svid in satinfo.rtcm_bits.error_history:
errset[svid] = satinfo.rtcm_bits.error_history[svid][-1]
save_satlog(rxm_raw.iTOW, errset)
print(satinfo.receiver_position)
return pos
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo)
if len(rtcm) != 0:
print("generated type 1")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print("generated type 3")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
errset = {}
for svid in satinfo.rtcm_bits.error_history:
errset[svid] = satinfo.rtcm_bits.error_history[svid][-1]
save_satlog(rxm_raw.iTOW, errset)
return pos
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo)
if len(rtcm) != 0:
print("generated type 1")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print("generated type 3")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
errset = {}
for svid in satinfo.rtcm_bits.error_history:
errset[svid] = satinfo.rtcm_bits.error_history[svid][-1]
save_satlog(rxm_raw.iTOW, errset)
return pos
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
# get get position the receiver calculated. We use this to check the calculations
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
if opts.plot:
plotter.plotPosition(pos, 0)
plotter.plotPosition(satinfo.average_position, 1)
import RTCMv2
rtcm = RTCMv2.generateRTCM2_Message1(satinfo)
rtcmfile.write(rtcm)
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) > 0:
rtcmfile.write(rtcm)
if 'NAV_POSECEF' in messages:
posecef = messages['NAV_POSECEF']
ourpos = util.PosVector(posecef.ecefX*0.01, posecef.ecefY*0.01, posecef.ecefZ*0.01)
posdiff = pos.distance(ourpos)
print("posdiff=%f pos=%s avg=%s %s" % (posdiff, pos.ToLLH(), satinfo.average_position.ToLLH(), time.ctime(satinfo.raw.gps_time)))
else:
print("pos=%s" % (pos.ToLLH()))
return pos
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo, maxsats=10)
if len(rtcm) != 0:
#print(rtcm)
rtcmfile.write(rtcm)
port.sendto(rtcm[:-2], (opts.udp_addr, opts.udp_port))
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print(rtcm)
rtcmfile.write(rtcm)
port.sendto(rtcm[:-2], (opts.udp_addr, opts.udp_port))
errset = {}
for svid in satinfo.rtcm_bits.error_history:
errset[svid] = satinfo.rtcm_bits.error_history[svid][-1]
save_satlog(rxm_raw.iTOW, errset)
print(satinfo.receiver_position)
return pos
def position_estimate(messages, satinfo):
'''process raw messages to calculate position
'''
rxm_raw = messages['RXM_RAW']
pos = positionEstimate.positionEstimate(satinfo)
if pos is None:
# not enough information for a fix
return
rtcm = RTCMv2.generateRTCM2_Message1(satinfo)
if len(rtcm) != 0:
print("generated type 1")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
rtcm = RTCMv2.generateRTCM2_Message3(satinfo)
if len(rtcm) != 0:
print("generated type 3")
rtcmfile.write(rtcm)
if not opts.nortcm:
dev2.write(rtcm)
return pos
def position_estimate(messages, satinfo):
rxm_raw = messages['RXM_RAW']
# If reference position is set, this automatically uses that reference position
# when solving for the clock error
pos = positionEstimate.positionEstimate(satinfo)
#print satinfo.reference_position, satinfo.position_estimate
adj = {}
raw = {}
cor = {}
for sv in satinfo.prCorrected:
cor[sv] = satinfo.geometricRange[sv] - satinfo.prCorrected[sv]
prAdjusted = cor[sv] + satinfo.receiver_clock_error * util.speedOfLight
adj[sv] = satinfo.geometricRange[sv] - prAdjusted
raw[sv] = adj[sv] + satinfo.tropospheric_correction[sv] + satinfo.ionospheric_correction[sv]
for sv in range(32):
adjlog.write("%f," % adj.get(sv,0))
rawlog.write("%f," % raw.get(sv,0))
corlog.write("%f," % cor.get(sv,0))
ellog.write("%f," % satinfo.elevation.get(sv,0))
azlog.write("%f," % satinfo.azimuth.get(sv,0))
adjlog.write('\n')
rawlog.write('\n')
corlog.write('\n')
ellog.write('\n')
azlog.write('\n')
def build_filter(info):
global filt
if filt is None:
est = positionEstimate.positionEstimate(satinfo)
if est is None:
# We use the least-squares method to bootstrap our one to avoid
# a requirement for mad particle space
return
print ("RP" + str(est))
mean = numpy.array([est.X, est.Y, est.Z, est.extra])
cov = numpy.diag([100 * state_cov, 100 * state_cov, 100 * state_cov, 100 * state_cov / util.speedOfLight])
init_pdf = pybayes.pdfs.GaussPdf(mean, cov)
p_xt_xtp = pybayes.pdfs.GaussCPdf(4, 4, p_xt_xtp_mu, p_xt_xtp_R)
p_yt_xt = pybayes.pdfs.GaussCPdf(32, 4, p_yt_xt_mu, p_yt_xt_R)
filt = pybayes.filters.ParticleFilter(n, init_pdf, p_xt_xtp, p_yt_xt)
def build_filter(info):
global filt
if filt is None:
est = positionEstimate.positionEstimate(satinfo)
if est is None:
# We use the least-squares method to bootstrap our one to avoid
# a requirement for mad particle space
return
print("RP" + str(est))
mean = numpy.array([est.X, est.Y, est.Z, est.extra])
cov = numpy.diag([100 * state_cov, 100 * state_cov, 100 * state_cov, 100 * state_cov / util.speedOfLight])
init_pdf = pybayes.pdfs.GaussPdf(mean, cov)
p_xt_xtp = pybayes.pdfs.GaussCPdf(4, 4, p_xt_xtp_mu, p_xt_xtp_R)
p_yt_xt = pybayes.pdfs.GaussCPdf(32, 4, p_yt_xt_mu, p_yt_xt_R)
filt = pybayes.filters.ParticleFilter(n, init_pdf, p_xt_xtp, p_yt_xt)
satinfo.min_quality = 0
while True:
msg = dev.receive_message(ignore_eof=False)
if msg is None:
break
if msg.name() in ['RXM_RAW', 'RXM_SFRB', 'AID_EPH']:
msg.unpack()
satinfo.add_message(msg)
else:
continue
if msg.name() == 'RXM_RAW':
positionEstimate.positionEstimate(satinfo)
for r in msg.recs:
if r.sv not in sats:
continue
if not satinfo.valid(r.sv):
continue
geo[r.sv].append(
satinfo.reference_position.distance(satinfo.satpos[r.sv]))
pr[r.sv].append(satinfo.prMeasured[r.sv] +
util.speedOfLight * satinfo.receiver_clock_error)
sm[r.sv].append(satinfo.prSmoothed[r.sv] +
util.speedOfLight * satinfo.receiver_clock_error)
cr[r.sv].append(satinfo.prCorrected[r.sv] +
satinfo.min_quality = 0
while True:
msg = dev.receive_message(ignore_eof=False)
if msg is None:
break
if msg.name() in ['RXM_RAW', 'RXM_SFRB', 'AID_EPH']:
msg.unpack()
satinfo.add_message(msg)
else:
continue
if msg.name() == 'RXM_RAW':
positionEstimate.positionEstimate(satinfo)
for r in msg.recs:
if r.sv not in sats:
continue
if not satinfo.valid(r.sv):
continue
geo[r.sv].append(satinfo.reference_position.distance(satinfo.satpos[r.sv]))
pr[r.sv].append(satinfo.prMeasured[r.sv] + util.speedOfLight*satinfo.receiver_clock_error)
sm[r.sv].append(satinfo.prSmoothed[r.sv] + util.speedOfLight*satinfo.receiver_clock_error)
cr[r.sv].append(satinfo.prCorrected[r.sv] + util.speedOfLight * satinfo.receiver_clock_error)
qi[r.sv].append(r.mesQI)
slip[r.sv].append(1 if satinfo.smooth.N[r.sv] == 1 else 0)
