Exemple #1
0
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
Exemple #3
0
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
Exemple #4
0
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
Exemple #5
0
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
Exemple #6
0
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')
Exemple #8
0
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)
Exemple #9
0
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)
Exemple #10
0
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)