def measure(self, particles, y, t):
""" Return the log-pdf value of the measurement """
logyprob = numpy.empty(len(particles), dtype=float)
for k in range(len(particles)):
logyprob[k] = kalman.lognormpdf(particles[k].reshape(-1, 1) - y,
self.R)
return logyprob
def measure(self, particles, y, t):
""" Return the log-pdf value of the measurement """
logyprob = numpy.empty(len(particles), dtype=float)
for k in range(len(particles)):
logyprob[k] = kalman.lognormpdf(
particles[k].reshape(-1, 1) - y, self.R)
return logyprob
def logp_xnext_full(self, part, past_trajs, pind,
future_trajs, find, ut, yt, tt, cur_ind):
diff = future_trajs[0].pa.part[find] - part
logpxnext = numpy.empty(len(diff), dtype=float)
for k in range(len(logpxnext)):
logpxnext[k] = kalman.lognormpdf(diff[k].reshape(-1, 1), numpy.asarray(self.Q).reshape(1, 1))
return logpxnext
def logp_xnext_full(self, part, past_trajs, pind,
future_trajs, find, ut, yt, tt, cur_ind):
diff = future_trajs[0].pa.part[find] - part
logpxnext = numpy.empty(len(diff), dtype=float)
for k in range(len(logpxnext)):
logpxnext[k] = kalman.lognormpdf(diff[k].reshape(-1, 1), numpy.asarray(self.Q).reshape(1, 1))
return logpxnext
def logp_q(self, particles, next_part, u, y, t):
logpq = numpy.empty(len(particles), dtype=float)
err = y - particles
C = numpy.eye(1)
P = self.Q
S = C.dot(P).dot(C.T) + self.R
Pn = P - P.dot(C.T).dot(scipy.linalg.solve(S, C.dot(P)))
for i in xrange(len(logpq)):
m = particles[i] + P.dot(C.T).dot(scipy.linalg.solve(S, err[i].reshape((-1, 1)))).ravel()
logpq[i] = kalman.lognormpdf(m.reshape((-1, 1)) - next_part[i].reshape((-1, 1)), Pn).ravel()
return logpq
def logp_q(self, particles, next_part, u, y, t):
logpq = numpy.empty(len(particles), dtype=float)
err = y - particles
C = numpy.eye(1)
P = self.Q
S = C.dot(P).dot(C.T) + self.R
Pn = P - P.dot(C.T).dot(scipy.linalg.solve(S, C.dot(P)))
for i in xrange(len(logpq)):
m = particles[i] + P.dot(C.T).dot(scipy.linalg.solve(S, err[i].reshape((-1, 1)))).ravel()
logpq[i] = kalman.lognormpdf(m.reshape((-1, 1)) - next_part[i].reshape((-1, 1)), Pn).ravel()
return logpq
def measure(self, particles, y, t):
""" Return the log-pdf value of the measurement """
N = len(particles)
# If there isn't any observation, return zero(particles weight don't change)
if len(y) <= 1:
return numpy.zeros(len(particles), dtype=float)
masterEstimation = y[0]
slaveEstimation = y[1]
# todo: handle situations that both of master and slave are present(it's so rare)
coefficient = 1.0
if masterEstimation[0] == 1:
guess = numpy.array(masterEstimation[1:])
print("############# Master")
print(guess)
elif slaveEstimation[0] == 1:
coefficient = 4
guess = numpy.array(slaveEstimation[1:])
print("############# Slave")
print(guess)
################################################################
### Restart algorithm
# logyprob = numpy.empty(len(particles), dtype=float)
# for k in range(len(particles)):
# particle = particles[k]
# dist = numpy.linalg.norm(particle[:2] - guess) / 100
# if self.LastObserveDist > 2.0:
# print("self.LastObserveDist:", self.LastObserveDist)
# logyprob[k] = kalman.lognormpdf(dist, self.R * 0.1)
# else:
# logyprob[k] = kalman.lognormpdf(dist, self.R * coefficient)
################################################################
################################################################
### Normal
logyprob = numpy.empty(len(particles), dtype=float)
wights = numpy.empty(len(particles), dtype=float)
for k in range(len(particles)):
particle = particles[k]
if self.cross_wall(particle[:2], guess):
dist = numpy.linalg.norm(particle[:2] - guess) * 10
else:
dist = numpy.linalg.norm(particle[:2] - guess)
# dist = dist ** 3
# weight = 1 / (dist + 0.0000001) * 1/N
# wights[k] = weight
logyprob[k] = kalman.lognormpdf(dist, self.R * coefficient)
# logyprob[k] = math.log(weight)
# sumWeight = numpy.sum(wights)
# logyprob = numpy.log(wights / sumWeight)
# logyprob = kalman.lognormpdf(logyprob, self.R )
################################################################
################################################################
### Map constraint in update step (other part is in prediction step):
# logyprob = numpy.empty(len(particles), dtype=float)
# for i in range(len(particles[:, :2])):
# orgn = self.oldParticles[:, :2][i]
# particle = particles[:, :2][i]
#
#
# if (self.cross_wall(orgn,particle)):
# logyprob[i] = -math.inf
# else:
# dist = numpy.linalg.norm(particle[:2] - guess) / 100
# logyprob[i] = kalman.lognormpdf(dist, self.R * coefficient)
################################################################
################################################################
### Backup
# logyprob = numpy.empty(len(particles), dtype=float)
#
# for k in range(len(particles)):
# particle = particles[k]
# dist = numpy.linalg.norm(particle[:2] - guess) / 100
#
# # print("#########")
# # print(particle)
# # print(dist)
# # weight = 1/(dist +1)
# # print(weight)
# # print(kalman.lognormpdf(dist, self.R))
# if self.LastObserveDist > 2.0:
# print("self.LastObserveDist:", self.LastObserveDist)
# logyprob[k] = kalman.lognormpdf(dist, self.R * 0.1)
# else:
# logyprob[k] = kalman.lognormpdf(dist, self.R * coefficient)
# # logyprob[k] = numpy.log(numpy.array([[weight]]))
#
# #
# # if self.world.is_free(p[0],p[1]):
# # p_d_val = self.world.distance_to_nearest_beacon(p[0],p[1])
# # p_d = numpy.array([p_d_val])
# # if self.USE_BEACON :
# # logyprob[k] = kalman.lognormpdf(numpy.linalg.norm(particles[k][:2] - y), self.R)
# # else:
# # logyprob[k] = kalman.lognormpdf(numpy.array([0]), self.R)
# # else:
# # logyprob[k] = numpy.array([-100])
################################################################
return logyprob
def measure_nonlin(self, particles, y, t):
N = len(particles)
lpy = numpy.empty((N,))
for i in xrange(N):
lpy[i] = kalman.lognormpdf(y[0] - particles[i][0], self.R_xi)
return lpy
def logp_xnext(self, particles, next_part, u, t):
logpxn = numpy.empty(len(particles), dtype=float)
for k in range(len(particles)):
logpxn[k] = kalman.lognormpdf(particles[k].reshape(-1, 1) - next_part[k].reshape(-1, 1), self.Q)
return logpxn
def logp_xnext(self, particles, next_part, u, t):
logpxn = numpy.empty(len(particles), dtype=float)
for k in range(len(particles)):
logpxn[k] = kalman.lognormpdf(particles[k].reshape(-1, 1) - next_part[k].reshape(-1, 1), self.Q)
return logpxn
