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