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
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
