Ejemplo n.º 1
0
def calcpostprobs_case2(G,z,t,n,K,vmean,vvar,ups):

# [pp,ppos,wp,seg_idx,ssamp] = calcpostprobs_case2(G,z,t,n,vmean,vvar,ups):
# Improved approximation of the posterior
# Path candidate selection is performed once per measurement
# Sample positions are returned in addition to paths
# Inputs:
# G- road network
# z- m-length list containing position measurements as 2-length lists
# t- list or array of measurement times
# n- sample size
# (vmean,vvar)- statistics of vehicle movement
# ups- measurement noise variance
# Outputs:
# pp- sampled paths with node indices (list of sequences of node indices)
# ppos- samped paths with node positions (list of list of node positions)
# wp- sample weights (list of floats)
# seg_idx- index (list of lists of integers)
# ssamp- position along segment of each sampled path (m x n array of floats)

    m = len(z)
    seg_idx = [[0 for i in range(n)] for j in range(m)]
    
    Tp = roadnet.create_turn_tab(G)
     
    vmax = vmean+3*math.sqrt(vvar)
        
    logwt = numpy.zeros(n)
    validsamp = [1 for k in range(n)]
    psamp = []
    ssamp = numpy.zeros((m,n))
    
    # First measurement
    # Find all edges on which the target could lie
    [epos,eidx] = roadnet.find_start_paths(G,z[0],ups)
    # Sample edges
    pst = []
    psamp = []
    ppos = []
    for i in range(n):
        # Draw path and distance along path
        [pidx_samp,ssamp[0,i]] = samp_start_path(z[0],epos,ups)
        psamp.append(eidx[pidx_samp])
        if psamp[i][-2] not in pst:
            pst.append(psamp[i][-2])
        if psamp[i][-1] not in pst:
            pst.append(psamp[i][-1])
        seg_idx[0][i] = 0
        
    # Second measurement
    # Select candidate paths (for both directions)
    [pe,ee] = roadnet.findnodes(G,z[1],ups)
    [cpath_pos,cpath_idx,turnpen] = roadnet.find_paths_by_IDs(G,Tp,pe,ee,pst,K)
    ncand = len(cpath_idx)
    pst = []
    for i in range(n):
        ncandi = 0
        cpath_idxi = []
        cpath_posi = []
        tpeni = []
        isrev = []
        s0 = []
        # Find candidate paths matching this path sample
        for a in range(ncand):
            if cpath_idx[a][0]==psamp[i][-2] and cpath_idx[a][1]==psamp[i][-1]:
                cpath_posi.append(cpath_pos[a])
                cpath_idxi.append(cpath_idx[a])
                tpeni.append(turnpen[a])
                isrev.append(0)
                s0.append(ssamp[0,i])
            elif cpath_idx[a][1]==psamp[i][-2] and cpath_idx[a][0]==psamp[i][-1]:
                cpath_posi.append(cpath_pos[a])
                cpath_idxi.append(cpath_idx[a])
                tpeni.append(turnpen[a])
                isrev.append(1)
                ell = roadnet.mynorm(cpath_pos[a][:,1]-cpath_pos[a][:,0])
                s0.append(ell-ssamp[0,i])
        
        # Draw a sample path if there are any matching candidates
        # print "cpath_idxi:", cpath_idxi

        if len(cpath_idxi)>0:
            [pidx_samp,seg_samp,ssamp[1,i],lwtj,validsamp[i]] = samp_path(z[1],t[1]-t[0],cpath_posi,tpeni,s0,ups,vmean,vvar)
            if validsamp[i]:
                psamp_new = cpath_idxi[pidx_samp][2:seg_samp+2]
                if isrev[pidx_samp]:
                    psamp[i] = [psamp[i][1],psamp[i][0]]
                    ell = roadnet.mynorm(cpath_pos[pidx_samp][:,1]-cpath_pos[pidx_samp][:,0])
                    ssamp[0,i] = ell-ssamp[0,i]
                psamp[i] = psamp[i]+psamp_new
                seg_idx[1][i] = seg_samp
                if psamp[i][-2] not in pst:
                    pst.append(psamp[i][-2])
                logwt[i] = lwtj
            # Set the weight to zero (effectively) if there are no matching candidates
            else:
                logwt[i] = -1e10
        else:
            logwt[i] = -1e10
    # Processing remaining measurements
    for j in range(2,m):
        # Select candidate paths
        [pe,ee] = roadnet.findnodes(G,z[j],ups)
        [cpath_pos,cpath_idx,turnpen] = roadnet.find_paths_by_IDs(G,Tp,pe,ee,pst,K)
        ncand = len(cpath_idx)
        pst = []
        for i in range(n):
            if validsamp[i]:
                cpath_posi = []
                cpath_idxi = []
                tpeni = []
                s0 = []
                # Find candidate paths matching this path sample
                for a in range(ncand):
                    if cpath_idx[a][0]==psamp[i][-2] and cpath_idx[a][1]==psamp[i][-1]:
                        cpath_posi.append(cpath_pos[a])
                        cpath_idxi.append(cpath_idx[a])
                        tpeni.append(turnpen[a])
                        s0.append(ssamp[j-1,i])
                # Draw a sample path if there are any matching candidates
                if len(cpath_idxi)>0:
                    [pidx_samp,seg_samp,ssamp[j,i],lwtj,validsamp[i]] =               samp_path(z[j],t[j]-t[j-1],cpath_posi,tpeni,s0,ups,vmean,vvar)
                    if validsamp[i]:
                        seg_idx[j][i] = seg_samp+len(psamp[i])-2
                        psamp_new = cpath_idxi[pidx_samp][2:seg_samp+2]
                        psamp[i] = psamp[i]+psamp_new
                        if psamp[i][-2] not in pst:
                            pst.append(psamp[i][-2])
                        logwt[i] += lwtj
                    # Set the weight to zero (effectively) if there are no matching candidates
                    else:
                        logwt[i] = -1e10
                else:
                    logwt[i] = -1e10
    
    # Normalise weights
    maxwt = -1e20
    for i in range(n):
        if logwt[i]>maxwt:
            maxwt = logwt[i]
        
    wtilde = numpy.zeros((n))
    for i in range(n):
        wtilde[i] = math.exp(logwt[i]-maxwt)
    wt = wtilde/numpy.sum(wtilde)
    
    ppos = []
    for i in range(n):
        nseg = len(psamp[i])-1
        ppos_samp = numpy.zeros((2,nseg+1))
        for j in range(nseg+1):
            pos = G.node[psamp[i][j]]["pos"]
            ppos_samp[0,j] = pos[0]
            ppos_samp[1,j] = pos[1]
        ppos.append(ppos_samp)
        
    return psamp, ppos, wt, seg_idx, ssamp
Ejemplo n.º 2
0
def calcpostprobs(G,z,t,n,K,vmean,vvar,ups,tpath):

# [pp,sp,wp] = calcpostprobs(G,z,t,n,K,vmean,vvar,ups):
# Improved approximation of the posterior
# Path candidate selection is performed once per measurement
# Inputs:
# G- road network
# z- m-length list containing position measurements as 2-length lists
# t- list or array measurement times
# n- sample size
# K- no. candidate paths
# (vmean,vvar)- statistics of vehicle movement
# ups- measurement noise variance
# Outputs:
# pp- sampled paths (list of sequences of node indices)
# sp- (estimated) distance of object along sampled paths (list of floats)
# wp- sample weights    (list of floats)

    m = len(z)
    
    Tp = roadnet.create_turn_tab(G)
     
    vmax = vmean+3*math.sqrt(vvar)
        
    logwt = numpy.zeros(n)
    psamp = []
    ssamp = numpy.zeros((n,m))
    
    # First measurement
    # Find all edges on which the target could lie
    [epos,eidx] = roadnet.find_start_paths(G,z[0],ups)
    # print "epos:",epos
    # print "eidx:",eidx
    # Sample edges (both directions are considered)

    pst = []
    pst_rev = []
    psamp = []
    for i in range(n):
        # Draw edge and distance along edge
        [pidx_samp,ssamp[i][0]] = samp_start_path(z[0],epos,ups)
        psamp.append(eidx[pidx_samp])
        if psamp[i][-2] not in pst:
            pst.append(psamp[i][-2])
        if psamp[i][-1] not in pst_rev:
            pst_rev.append(psamp[i][-1])

    # Second measurement
    # Select candidate paths (for both directions)
    [pe,ee] = roadnet.findnodes(G,z[1],ups)
    [cpath_pos,cpath_idx,turnpen] = roadnet.find_paths_by_IDs(G,Tp,pe,ee,pst,K,tpath)

    ncand = len(cpath_idx)

    [cpath_pos1,cpath_idx1,turnpen1] = roadnet.find_paths_by_IDs(G,Tp,pe,ee,pst_rev,K,tpath)

    ncand1 = len(cpath_idx1)
    pst = []
    for i in range(n):
        ncandi = 0
        cpath_idxi = []
        cpath_posi = []
        tpeni = []
        isrev = []
        s0 = []
        # Find candidate paths matching this path sample
        for a in range(ncand):
            if cpath_idx[a][0]==psamp[i][-2] and cpath_idx[a][1]==psamp[i][-1]:
                cpath_posi.append(cpath_pos[a])
                cpath_idxi.append(cpath_idx[a])
                tpeni.append(turnpen[a])
                isrev.append(0)
                s0.append(ssamp[i][0])
                ncandi = ncandi+1
        for a in range(ncand1):
            if cpath_idx1[a][0]==psamp[i][-1] and cpath_idx1[a][1]==psamp[i][-2]:
                cpath_posi.append(cpath_pos1[a])
                cpath_idxi.append(cpath_idx1[a])
                tpeni.append(turnpen1[a])
                isrev.append(1)
                ell = roadnet.mynorm(cpath_pos1[a][:,1]-cpath_pos1[a][:,0])
                s0.append(ell-ssamp[i][0])
        # Draw a sample path if there are any matching candidates
        if len(cpath_idxi)>0:
            [pidx_samp,seg_idx,ssamp[i][1],lwtj,isvalid] = samp_path(z[1],t[1]-t[0],cpath_posi,tpeni,s0,ups,vmean,vvar)
            psamp_new = cpath_idxi[pidx_samp][2:seg_idx+2]
            if isrev[pidx_samp]:
                psamp[i] = [psamp[i][1],psamp[i][0]]
            psamp[i] = psamp[i]+psamp_new
            # print "psamp:",psamp[i]
            if psamp[i][-2] not in pst:
                pst.append(psamp[i][-2])
            logwt[i] = lwtj
        # Set the weight to zero (effectively) if there are no matching candidates
        else:
            logwt[i] = -1e10
    
    # Process the remaining measurements
    for j in range(2,m):
        # Select candidate paths
        [pe,ee] = roadnet.findnodes(G,z[j],ups)
        [cpath_pos,cpath_idx,turnpen] = roadnet.find_paths_by_IDs(G,Tp,pe,ee,pst,K,[])
        ncand = len(cpath_idx)
        pst = []
        for i in range(n):
            cpath_posi = []
            cpath_idxi = []
            tpeni = []
            s0 = []
            # Find candidate paths matching this path sample
            for a in range(ncand):
                if cpath_idx[a][0]==psamp[i][-2] and cpath_idx[a][1]==psamp[i][-1]:
                    cpath_posi.append(cpath_pos[a])
                    cpath_idxi.append(cpath_idx[a])
                    tpeni.append(turnpen[a])
                    s0.append(ssamp[i][j-1])
            # Draw a sample path if there are any matching candidates
            if len(cpath_idxi)>0:
                [pidx_samp,seg_idx,ssamp[i][j],lwtj,isvalid] =samp_path(z[j],t[j]-t[j-1],cpath_posi,tpeni,s0,ups,vmean,vvar)
                psamp_new = cpath_idxi[pidx_samp][2:seg_idx+2]
                psamp[i] = psamp[i]+psamp_new
                if psamp[i][-2] not in pst:
                    pst.append(psamp[i][-2])
                logwt[i] += lwtj
            # Set the weight to zero (effectively) if there are no matching candidates
            else:
                logwt[i] = -1e10
            
    # Normalise weights
    maxwt = -1e20
    for i in range(n):
        if logwt[i]>maxwt:
            maxwt = logwt[i]
    wtilde = numpy.zeros((n))
    for i in range(n):
        wtilde[i] = math.exp(logwt[i]-maxwt)
    wt = wtilde/numpy.sum(wtilde)
    
    # Find the unique set of sampled paths
    npaths = 0
    pout = []
    seout = []
    wout = numpy.zeros((n))
    nout = 0
    for i in range(n):
        isin = 0
        if psamp[i] in pout:
            idx = pout.index(psamp[i])
            seout[idx] += wt[i]*ssamp[i][-1]
            wout[idx] += wt[i]
        else:
            pout.append(psamp[i])
            seout.append(wt[i]*ssamp[i][-1])
            wout[nout] = wt[i]
            nout += 1
            
    wout = wout[0:nout]
    seout = seout[0:nout]
    for i in range(nout):
        seout[i] /= wout[i]
        
    return pout, seout, wout