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)