def _make_valid(self, covar): """ Trys to make a possibly degenerate covariance valid by - replacing nans and infs with high values/zeros - making the matrix symmetric - trying to make the matrix invertible by adding small offsets to the smallest eigenvalues Parameters ---------- covar : tensor a covariance matrix that is possibly degenerate Returns ------- covar_valid : tensor a covariance matrix that is hopefully valid """ # eliminate nans and infs (replace them with high values on the # diagonal and zeros else) bs = compat.get_dim_int(covar, 0) # covar.get_shape()[0] dim = compat.get_dim_int(covar, -1) # covar.get_shape()[-1] covar = tf.where(tf.math.is_finite(covar), covar, tf.eye(dim, batch_shape=[bs]) * 1e6) # make symmetric covar = (covar + tf.linalg.matrix_transpose(covar)) / 2. # add a bit of noise to the diagonal of covar to prevent # nans in the gradient of the svd noise = tf.random.uniform(covar.get_shape().as_list()[:-1], minval=0, maxval=0.001 / self.scale**2) s, u, v = tf.linalg.svd(covar + tf.linalg.diag(noise)) # test if the matrix is invertible invertible = self._is_invertible(s) # test if the matrix is positive definite pd = tf.reduce_all(tf.greater(s, 0), axis=-1) # try making a valid version of the covariance matrix by ensuring that # the minimum eigenvalue is at least 1e-4/self.scale min_eig = s[..., -1:] eps = tf.tile(tf.maximum(1e-4 / self.scale - min_eig, 0), [1, compat.get_dim_int(s, -1)]) covar_invertible = tf.matmul( u, tf.matmul(tf.linalg.diag(s + eps), v, adjoint_b=True)) # if the covariance matrix is valid, leave it as is, else replace with # the modified variant covar_valid = tf.where( tf.logical_and(invertible, pd)[:, None, None], covar, covar_invertible) # make symmetric again covar_valid = \ (covar_valid + tf.linalg.matrix_transpose(covar_valid)) / 2. return covar_valid
def _prediction_step(self, sigma_points, weights_m, actions, training): """ Prediction step of the UKF Parameters ---------- sigma_points : tensor [batch_size, num_sigma, dim_x] the old sigma points weights_m : tensor [batch_size, num_sigma] weights for computing the mean of the sigma points actions : tensor [batch_size, dim_u] the current actions training : bool training or testing? Returns ------- sigma_points_pred : tensor [batch_size, num_sigma, dim_x] the predicted sigma points Q : tensor [batch_size, dim_x, dim_x] the predicted process noise for this time step (averaged over sigma points) """ # move the sigma-dimension into the batch dimension sigma_points = tf.reshape(sigma_points, [-1, self.dim_x]) # tile the actions to match the shape of the sigma points actions = tf.tile(actions[:, None, :], [1, self.num_sigma, 1]) actions = tf.reshape(actions, [-1, 2]) sigma_points_pred, _ = \ self.context.run_process_model(sigma_points, actions, training) sigma_points_pred = self.context.correct_state(sigma_points_pred, diff=False) # get the process noise Q = self.context.get_process_noise(sigma_points, actions, training=training) # restore the sigma dimension sigma_points_pred = tf.reshape( sigma_points_pred, [self.batch_size, self.num_sigma, self.dim_x]) # when we use heteroscedastic noise, we get one Q per sigma point, so # we use the weighted mean if compat.get_dim_int(Q, 0) > self.batch_size: Q = tf.reshape( Q, [self.batch_size, self.num_sigma, self.dim_x, self.dim_x]) Q = tf.reduce_sum(tf.multiply(Q, weights_m[:, :, :, None]), axis=1) return sigma_points_pred, Q
def zca_whiten(self, data): """ Whiten noise sampled from a standard normal distribution Parameters ---------- data : tensor tf.float32 [batch_size, num_samples, dim_x] the sampled noise Returns ------- whitened : tensor tf.float64 [batch_size, num_samples, dim_x] the whitened noise samples """ # input tensor is [batch_size, num_samples, dim_x] num = compat.get_dim_int(data, 1) data = tf.cast(data, tf.float64) # center the samples mean = tf.reduce_mean(data, axis=1, keepdims=True) centered = data - tf.tile(mean, [1, num, 1]) # whiten # compute the current covariance diff_square = tf.matmul(centered[:, :, :, None], centered[:, :, None, :]) # sigma: [batch_size, dim_x, dim_x] sigma = tf.reduce_mean(diff_square, axis=1) # get the whitening matrix # s: [batch_size, dim_x], u: [batch_size, dim_x, dim_x] s, u, _ = tf.linalg.svd(sigma, full_matrices=True) s_inv = 1. / tf.sqrt(s + 1e-5) s_inv = tf.linalg.diag(s_inv) # w: [batch_size, dim_x, dim_x] w = tf.matmul(u, tf.matmul(s_inv, u, transpose_b=True)) whitened = tf.matmul(centered, w) return whitened
def _mixture_likelihood(self, diffs, weights, reduce_mean=False): """ Compute the negative log likelihood of y under a a gaussian mixture model defined by a set of particles and their weights. Parameters ---------- diffs : tensor difference between y and the states of the particles weights : tensor weights of the particles reduce_mean : bool, optional if true, return the mean likelihood loss over the complete tensor. The default is False. Returns ------- likelihood : tensor the negative log likelihood """ dim = compat.get_dim_int(diffs, -1) num = compat.get_dim_int(diffs, -2) # remove nans and infs and replace them with high values/zeros diffs = tf.where(tf.math.is_finite(diffs), diffs, tf.ones_like(diffs)*1e5) weights = tf.where(tf.math.is_finite(weights), weights, tf.zeros_like(weights)) weights /= tf.reduce_sum(weights, axis=-1, keepdims=True) covar = np.ones(dim, dtype=np.float32) for k in range(dim): covar[k] *= self.mixture_std covar = tf.linalg.diag(tf.square(covar)) if len(diffs.get_shape().as_list()) > 3: sl = compat.get_dim_int(diffs, 1) diffs = tf.reshape(diffs, [self.batch_size, -1, num, dim, 1]) covar = tf.tile(covar[None, None, None, :, :], [self.batch_size, sl, num, 1, 1]) else: sl = 1 diffs = tf.reshape(diffs, [self.batch_size, num, dim, 1]) covar = tf.tile(covar[None, None, :, :], [self.batch_size, num, 1, 1]) # transfer to float 64 for higher accuracy covar = tf.cast(covar, tf.float64) diffs = tf.cast(diffs, tf.float64) weights = tf.cast(weights, tf.float64) exponent = tf.matmul(tf.matmul(tf.linalg.matrix_transpose(diffs), tf.linalg.inv(covar)), diffs) exponent = tf.reshape(exponent, [self.batch_size, sl, num]) normalizer = tf.math.log(tf.linalg.det(covar)) + \ tf.cast(dim * tf.log(2*np.pi), tf.float64) log_like = -0.5 * (exponent + normalizer) log_like = tf.reshape(log_like, [self.batch_size, sl, num]) log_like = tf.where(tf.greater_equal(log_like, -500), log_like, tf.ones_like(log_like)*-500) exp = tf.exp(log_like) # the per particle likelihoods are weighted and summed in the particle # dimension weighted = weights * exp weighted = tf.reduce_sum(weighted, axis=-1) # compute the negative logarithm and undo the bias likelihood = - (tf.math.log(tf.maximum(weighted, 1e-300))) if reduce_mean: likelihood = tf.reduce_mean(likelihood) likelihood = tf.cast(likelihood, tf.float32) return likelihood
def _likelihood(self, diff, covar, reduce_mean=False): """ Compute the negative log likelihood of y under a gaussian with mean mu and covariance covar, where x and y are given as diff = y - mu Parameters ---------- diff : tensor difference between y and the mean of the gaussian covar : tensor covariance matrix of the gaussian reduce_mean : bool, optional if true, return the mean likelihood loss over the complete tensor. The default is False. Returns ------- likelihood : tensor the negative log likelihood """ dim = compat.get_dim_int(diff, -1) # transfer to float 64 for higher accuracy covar = tf.cast(covar, tf.float64) diff = tf.cast(diff, tf.float64) if dim > 1: if len(diff.get_shape().as_list()) > 2: diff = tf.reshape(diff, [self.batch_size, -1, dim, 1]) else: diff = tf.reshape(diff, [self.batch_size, dim, 1]) c_inv = tf.linalg.inv(covar) err = tf.matmul(tf.matmul(tf.linalg.matrix_transpose(diff), c_inv), diff) err = tf.reshape(err, [self.batch_size, -1]) # the error term needs to be finite and positive err = tf.where(tf.math.is_finite(err), err, tf.ones_like(err)*500) err = tf.where(tf.greater_equal(err, 0), err, tf.ones_like(err)*500) det = tf.reshape(tf.math.log(tf.linalg.det(covar)), [self.batch_size, -1]) # nans mostly come from too small values, so we replace them with # zero det = tf.where(tf.math.is_finite(det), det, tf.zeros_like(det)) with tf.control_dependencies( [tf.debugging.assert_all_finite(det, name='det', message='det')]): likelihood = err + det else: diff = tf.reshape(diff, [self.batch_size, -1]) covar = tf.reshape(covar, [self.batch_size, -1]) det = tf.math.log(covar) err = (diff**2)/covar # the error term needs to be finite and positive err = tf.where(tf.math.is_finite(err), err, tf.ones_like(err)*500) err = tf.where(tf.greater_equal(err, 0), err, tf.ones_like(err)*500) likelihood = det + err likelihood = 0.5 * (likelihood + dim * np.log(2*np.pi)) likelihood = tf.reshape(likelihood, [self.batch_size, -1]) likelihood = tf.cast(likelihood, tf.float32) if reduce_mean: likelihood = tf.reduce_mean(likelihood) return likelihood
def _prediction_step(self, particles_old, weights_old, actions, training): """ Prediction step of the PF Parameters ---------- particles_old : tensor [batch_size, num_particles, dim_x] the particles representing the current belief about the state weights_old : tensor [batch_size, num_particles] their weights actions : tensor [batch_size, dim_u] the current actions training : bool training or testing? Returns ------- particles_pred : tensor [batch_size, num_particles, dim_x] the predicted state for each particle Q : tensor [batch_size, dim_x, dim_x] the predicted process noise for this time step (averaged over particles) """ # move the particle-dimension into the batch dimension particles_old = tf.reshape(particles_old, [-1, self.dim_x]) # tile the actions to match the shape of the sigma points actions = tf.tile(actions[:, None, :], [1, self.num_particles, 1]) actions = tf.reshape(actions, [-1, 2]) particles_pred, _ = \ self.context.run_process_model(particles_old, actions, training) # get the process noise Q = self.context.get_process_noise(particles_old, actions, training) if compat.get_dim_int(Q, 0) > self.batch_size: Q = tf.reshape(Q, [self.batch_size, self.num_particles, self.dim_x, self.dim_x]) # restore the particle dimension particles_pred = tf.reshape(particles_pred, [self.batch_size, self.num_particles, self.dim_x]) # sample noise according to the estimated process noise noise = \ self.sam.sample(sample_shape=[self.batch_size*self.num_particles]) noise = tf.reshape(noise, [self.batch_size, self.num_particles, self.dim_x]) # whiten the noise to reduce undesired correlations in the sampled # sigma points noise = self.zca_whiten(noise) noise = tf.reshape(noise, [self.batch_size, self.num_particles, self.dim_x, 1]) scale = tf.linalg.sqrtm(tf.cast(Q, tf.float64)) if len(scale.get_shape()) == 3: # when using heteroscedastic noise, we get one Q per particle, but # with constant noise, we only get one Q for all particles in the # batch scale = tf.tile(scale[:, None, :, :], [1, self.num_particles, 1, 1]) # replace nans and infs in scale with zeros (off-diagonal) or ones # (diagonal) scale = tf.where(tf.math.is_finite(scale), scale, tf.eye(self.dim_x, batch_shape=[self.batch_size, self.num_particles], dtype=tf.float64, name=None)) noise = tf.linalg.matmul(scale, noise) # add the noise to the particles particles_pred += tf.squeeze(tf.cast(noise, tf.float32), axis=-1) particles_pred = self.context.correct_state(particles_pred, diff=False) # when we use heteroscedastic noise, we get one Q per particle, so # we output the weighted mean if len(Q.get_shape()) == 4: # weights are in log scale, to turn them into a distribution, we # exponentiate and normalize them == apply the softmax transform weights_dist = tf.nn.softmax(weights_old, axis=-1) Q = tf.reduce_sum(tf.multiply(Q, weights_dist[:, :, None, None]), axis=1) return particles_pred, Q