def eval_logp_y_fulltraj(self, straj, yt, tt):
sest = straj.get_smoothed_estimates()
M = sest.shape[1]
yp = 0.05 * sest ** 2
diff = yp - numpy.repeat(numpy.asarray(yt, dtype=float).reshape((-1, 1, 1)),
repeats=M, axis=1)
return numpy.sum(kalman.lognormpdf_scalar(diff.ravel(), self.R)) / M
def eval_logp_xnext_fulltraj(self, straj, ut, tt):
part = straj.get_smoothed_estimates()
M = part.shape[1]
cost = 8.0 * numpy.cos(1.2 * numpy.asarray(tt, dtype=float))
xp = 0.5 * part + 25.0 * part / (1 + part ** 2) + numpy.repeat(cost.reshape(-1, 1, 1), repeats=M, axis=1)
diff = part[1:] - xp[:-1]
logp = kalman.lognormpdf_scalar(diff.ravel(), self.Q)
return numpy.sum(logp) / M
def eval_logp_xnext_fulltraj(self, straj, ut, tt):
M = straj.shape[1]
part = straj
cost = 8.0 * numpy.cos(1.2 * numpy.asarray(tt, dtype=float))
xp = 0.5 * part + 25.0 * part / (1 + part ** 2) + numpy.repeat(cost.reshape(-1, 1, 1), repeats=M, axis=1)
diff = part[1:] - xp[:-1]
logp = kalman.lognormpdf_scalar(diff.ravel(), self.Q)
return numpy.sum(logp) / M
def eval_logp_x0(self, particles, t):
""" Calculate gradient of a term of the I1 integral approximation
as specified in [1].
The gradient is an array where each element is the derivative with
respect to the corresponding parameter"""
return kalman.lognormpdf_scalar(particles, self.P0)
def logp_xnext(self, particles, next_part, u, t):
""" Return the log-pdf value for the possible future state 'next' given input u """
pn = 0.5 * particles + 25.0 * particles / \
(1 + particles ** 2) + 8 * math.cos(1.2 * t)
return kalman.lognormpdf_scalar(pn.ravel() - next_part.ravel(), self.Q)
def measure(self, particles, y, t):
""" Return the log-pdf value of the measurement """
return kalman.lognormpdf_scalar(0.05 * particles ** 2 - y, self.R)
def __init__(self, P0, Q, R):
self.P0 = numpy.copy(P0)
self.Q = numpy.copy(Q)
self.R = numpy.copy(R)
self.logxn_max = kalman.lognormpdf_scalar(numpy.zeros((1,)), self.Q)
super(Model, self).__init__()
def logp_xnext(self, particles, next_part, u, t):
diff = next_part - particles
return kalman.lognormpdf_scalar(diff, self.Q)
def logp_xnext(self, particles, next_part, u, t):
diff = next_part - particles
return kalman.lognormpdf_scalar(diff, self.Q)