def eval_logp_x0_val_grad(self, particles, t):
"""
Evaluate gradient of sum log p(x_0)
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- t (float): time stamp
"""
# Calculate l1 according to (19a)
N = len(particles)
lparam = len(self.params)
lpz0_grad = numpy.zeros(lparam)
(zl, Pl) = self.get_states(particles)
(z0_grad, P0_grad) = self.get_initial_grad()
if (z0_grad is None and P0_grad is None):
lpz0 = self.eval_logp_x0(particles, t)
else:
lpz0 = 0.0
P0cho = scipy.linalg.cho_factor(self.P0)
ld = numpy.sum(numpy.log(numpy.diagonal(P0cho[0]))) * 2
for i in xrange(N):
(l1, l1_grad) = self.calc_l1_grad(zl[i], Pl[i], self.z0,
self.P0, z0_grad)
tmp = scipy.linalg.cho_solve(P0cho, l1)
lpz0 += -0.5 * (ld + numpy.trace(tmp))
for j in range(len(self.params)):
lpz0_grad[
j] -= 0.5 * mlnlg_compute.compute_logprod_derivative(
P0cho, P0_grad[j], l1, l1_grad[j])
return (lpz0, lpz0_grad)
def eval_logp_x0_val_grad(self, particles, t):
"""
Evaluate gradient of sum log p(x_0)
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- t (float): time stamp
"""
# Calculate l1 according to (19a)
N = len(particles)
lparam = len(self.params)
lpz0_grad = numpy.zeros(lparam)
(zl, Pl) = self.get_states(particles)
(z0_grad, P0_grad) = self.get_initial_grad()
if z0_grad is None and P0_grad is None:
lpz0 = self.eval_logp_x0(particles, t)
else:
lpz0 = 0.0
P0cho = scipy.linalg.cho_factor(self.P0)
ld = numpy.sum(numpy.log(numpy.diagonal(P0cho[0]))) * 2
for i in xrange(N):
(l1, l1_grad) = self.calc_l1_grad(zl[i], Pl[i], self.z0, self.P0, z0_grad)
tmp = scipy.linalg.cho_solve(P0cho, l1)
lpz0 += -0.5 * (ld + numpy.trace(tmp))
for j in range(len(self.params)):
lpz0_grad[j] -= 0.5 * mlnlg_compute.compute_logprod_derivative(P0cho, P0_grad[j], l1, l1_grad[j])
return (lpz0, lpz0_grad)
def eval_logp_xnext_val_grad(self, particles, x_next, u, t):
"""
Evaluate value and gradient of log p(x_{t+1}|x_t)
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- x_next (array-like): future states
- t (float): time stamp
Returns: ((array-like), (array-like))
"""
# Calculate l2 according to (16)
N = len(particles)
lparam = len(self.params)
(zl, Pl) = self.get_states(particles)
(zn, Pn) = self.get_states(x_next)
(A, f, Q) = self.get_pred_dynamics(u=u, t=t)
(A_grad, f_grad, Q_grad) = self.get_pred_dynamics_grad(u=u, t=t)
lpxn_grad = numpy.zeros(lparam)
if (A_grad is None and f_grad is None and Q_grad is None):
lpxn = self.eval_logp_xnext(particles, x_next, u, t)
else:
self.kf.set_dynamics(A=A, Q=Q, f_k=f)
lpxn = 0.0
Qcho = scipy.linalg.cho_factor(self.kf.Q, check_finite=False)
ld = numpy.sum(numpy.log(numpy.diagonal(Qcho[0]))) * 2
if (Q_grad is None):
Q_grad = numpy.zeros(
(len(self.params), self.kf.lz, self.kf.lz))
for k in xrange(N):
lz = len(self.z0)
lzP = lz + lz * lz
Mz = particles[k][lzP:].reshape((lz, lz))
(l2, l2_grad) = self.calc_l2_grad(zn[k], Pn[k], zl[k], Pl[k],
self.kf.A, self.kf.f_k, Mz,
A_grad, f_grad)
tmp = scipy.linalg.cho_solve(Qcho, l2)
lpxn += -0.5 * (ld + numpy.trace(tmp))
for j in range(len(self.params)):
lpxn_grad[
j] -= 0.5 * mlnlg_compute.compute_logprod_derivative(
Qcho, Q_grad[j], l2, l2_grad[j])
return (lpxn, lpxn_grad)
def eval_logp_xnext_val_grad(self, particles, x_next, u, t):
"""
Evaluate value and gradient of log p(x_{t+1}|x_t)
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- x_next (array-like): future states
- t (float): time stamp
Returns: ((array-like), (array-like))
"""
# Calculate l2 according to (16)
N = len(particles)
lparam = len(self.params)
(zl, Pl) = self.get_states(particles)
(zn, Pn) = self.get_states(x_next)
(A, f, Q) = self.get_pred_dynamics(u=u, t=t)
(A_grad, f_grad, Q_grad) = self.get_pred_dynamics_grad(u=u, t=t)
lpxn_grad = numpy.zeros(lparam)
if A_grad is None and f_grad is None and Q_grad is None:
lpxn = self.eval_logp_xnext(particles, x_next, u, t)
else:
self.kf.set_dynamics(A=A, Q=Q, f_k=f)
lpxn = 0.0
Qcho = scipy.linalg.cho_factor(self.kf.Q, check_finite=False)
ld = numpy.sum(numpy.log(numpy.diagonal(Qcho[0]))) * 2
if Q_grad is None:
Q_grad = numpy.zeros((len(self.params), self.kf.lz, self.kf.lz))
for k in xrange(N):
lz = len(self.z0)
lzP = lz + lz * lz
Mz = particles[k][lzP:].reshape((lz, lz))
(l2, l2_grad) = self.calc_l2_grad(
zn[k], Pn[k], zl[k], Pl[k], self.kf.A, self.kf.f_k, Mz, A_grad, f_grad
)
tmp = scipy.linalg.cho_solve(Qcho, l2)
lpxn += -0.5 * (ld + numpy.trace(tmp))
for j in range(len(self.params)):
lpxn_grad[j] -= 0.5 * mlnlg_compute.compute_logprod_derivative(Qcho, Q_grad[j], l2, l2_grad[j])
return (lpxn, lpxn_grad)
def eval_logp_y_val_grad(self, particles, y, t):
"""
Evaluate value and gradient of log p(y_t|x_t)
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- y (array-like): measurement
- t (float): time stamp
Returns: ((array-like), (array-like))
"""
N = len(particles)
lparam = len(self.params)
(y, C, h, R) = self.get_meas_dynamics(y=y, t=t)
(C_grad, h_grad, R_grad) = self.get_meas_dynamics_grad(y=y, t=t)
logpy_grad = numpy.zeros(lparam)
if (C_grad is None and h_grad is None and R_grad is None):
logpy = self.eval_logp_y(particles, y, t)
else:
self.kf.set_dynamics(C=C, R=R, h_k=h)
Rcho = scipy.linalg.cho_factor(self.kf.R, check_finite=False)
ld = numpy.sum(numpy.log(numpy.diagonal(Rcho[0]))) * 2
(zl, Pl) = self.get_states(particles)
logpy = 0.0
if (R_grad is None):
R_grad = numpy.zeros((len(self.params), len(y), len(y)))
for i in xrange(N):
# Calculate l3 according to (19b)
# Calculate l3 according to (19b)
(l3, l3_grad) = self.calc_l3_grad(y, zl[i], Pl[i])
tmp = scipy.linalg.cho_solve(Rcho, l3)
logpy += -0.5 * (ld + numpy.trace(tmp))
for j in range(len(self.params)):
logpy_grad[
j] -= 0.5 * mlnlg_compute.compute_logprod_derivative(
Rcho, R_grad[j], l3, l3_grad[j])
return (logpy, logpy_grad)
def eval_logp_y_val_grad(self, particles, y, t):
"""
Evaluate value and gradient of log p(y_t|x_t)
Args:
- particles (array-like): Model specific representation
of all particles, with first dimension = N (number of particles)
- y (array-like): measurement
- t (float): time stamp
Returns: ((array-like), (array-like))
"""
N = len(particles)
lparam = len(self.params)
(y, C, h, R) = self.get_meas_dynamics(y=y, t=t)
(C_grad, h_grad, R_grad) = self.get_meas_dynamics_grad(y=y, t=t)
logpy_grad = numpy.zeros(lparam)
if C_grad is None and h_grad is None and R_grad is None:
logpy = self.eval_logp_y(particles, y, t)
else:
self.kf.set_dynamics(C=C, R=R, h_k=h)
Rcho = scipy.linalg.cho_factor(self.kf.R, check_finite=False)
ld = numpy.sum(numpy.log(numpy.diagonal(Rcho[0]))) * 2
(zl, Pl) = self.get_states(particles)
logpy = 0.0
if R_grad is None:
R_grad = numpy.zeros((len(self.params), len(y), len(y)))
for i in xrange(N):
# Calculate l3 according to (19b)
# Calculate l3 according to (19b)
(l3, l3_grad) = self.calc_l3_grad(y, zl[i], Pl[i])
tmp = scipy.linalg.cho_solve(Rcho, l3)
logpy += -0.5 * (ld + numpy.trace(tmp))
for j in range(len(self.params)):
logpy_grad[j] -= 0.5 * mlnlg_compute.compute_logprod_derivative(Rcho, R_grad[j], l3, l3_grad[j])
return (logpy, logpy_grad)
