From b60a512cad9293ab526e62e1ecc0aad1fce67162 Mon Sep 17 00:00:00 2001 From: Florian Pfaff Date: Sun, 17 Sep 2023 11:39:29 +0200 Subject: [PATCH 1/2] Added HypertoroidalGridDistribution --- .../hypertoroidal_grid_distribution.py | 183 ++++++++++++++++++ .../test_hypertoroidal_grid_distribution.py | 42 ++++ 2 files changed, 225 insertions(+) create mode 100644 pyrecest/distributions/hypertorus/hypertoroidal_grid_distribution.py create mode 100644 pyrecest/tests/distributions/test_hypertoroidal_grid_distribution.py diff --git a/pyrecest/distributions/hypertorus/hypertoroidal_grid_distribution.py b/pyrecest/distributions/hypertorus/hypertoroidal_grid_distribution.py new file mode 100644 index 000000000..d5754bee6 --- /dev/null +++ b/pyrecest/distributions/hypertorus/hypertoroidal_grid_distribution.py @@ -0,0 +1,183 @@ +import numpy as np +from .abstract_hypertoroidal_distribution import AbstractHypertoroidalDistribution +from ..abstract_grid_distribution import AbstractGridDistribution +from scipy.fftpack import fftn, ifftn, fftshift, ifftshift +from .hypertoroidal_fourier_distribution import HypertoroidalFourierDistribution +from .hypertoroidal_dirac_distribution import HypertoroidalDiracDistribution + +class HypertoroidalGridDistribution(AbstractGridDistribution, AbstractHypertoroidalDistribution): + def __init__(self, grid_values, grid_type = "custom", grid = None, enforce_pdf_nonnegative=True, dim=None): + # Constructor + if dim is None and grid is None or grid.ndim<=1: + dim = 1 + elif dim is None: + dim = grid.shape[1] + + AbstractHypertoroidalDistribution.__init__(self, dim) + AbstractGridDistribution.__init__(self, grid_values, grid_type, grid, dim) + self.enforce_pdf_nonnegative = enforce_pdf_nonnegative + # Check if normalized. If not: Normalize + self.normalize_in_place() + + def get_closest_point(self, xs): + min_error = np.inf # Initialize with infinity + closest_point = None + + for point in self.grid: + error = AbstractHypertoroidalDistribution.angular_error(x, point) + if error < min_error: + min_error = error + closest_point = point + + return closest_point + + def get_manifold_size(self): + return AbstractHypertoroidalDistribution.get_manifold_size(self) + + @staticmethod + def generate_cartesian_product_grid(n_grid_points): + grid_individual_axis = [np.linspace(0, 2 * np.pi - 2 * np.pi / n, n) for n in n_grid_points] + meshgrids = np.meshgrid(*grid_individual_axis, indexing='ij') + grid = np.column_stack([grid.ravel() for grid in meshgrids]) + return grid + + def multiply(self, other): + assert np.all(self.grid == other.grid), 'Multiply:IncompatibleGrid: Can only multiply for equal grids.' + return super().multiply(other) + + def pdf(self, xs): + assert self.grid_type == 'CartesianProd', 'pdf is not defined. Can only interpolate for certain grid types.' + print('Warning: PDF:UseInterpolated: pdf is not defined. Using interpolation with Fourier series.') + + transformation = 'sqrt' if self.enforce_pdf_nonnegative else 'identity' + + print('Warning: Cannot interpolate if number of grid points are not specified. Assuming equidistant grid') + fd = HypertoroidalFourierDistribution.from_function_values( + np.reshape(self.grid_values, self.grid_density_description["n_grid_values"]), + self.grid_density_description["n_grid_values"] + (self.grid_density_description["n_grid_values"] % 2 == 0), + transformation + ) + return fd.pdf(xs) + + def get_grid(self): + if self.grid.size > 0: + return self.grid + elif self.grid_type == 'CartesianProd': + print('Warning: Grid:GenerateDuringRunTime: Generating grid anew on call to .getGrid(). If you require the grid frequently, store it in the class.') + return np.squeeze(self.generate_cartesian_product_grid(self.n_grid_points)) + else: + raise ValueError('Grid:UnknownGrid: Grid was not provided and is thus unavailable') + + def pdf_unnormalized(self, xs): + assert self.grid_type == 'CartesianProd', 'pdf is not defined. Can only interpolate for certain grid types.' + p = self.integrate() * self.pdf(xs) + return p + + def shift(self, shift_by): + if np.linalg.norm(shift_by) == 0: + return self + + assert self.grid_type == 'CartesianProd' + + # Initialize with some uniform distribution and then replace coefficient matrix + coeff_mat_tmp = np.zeros(tuple([3] * self.dim)) + coeff_mat_tmp[0] = 1 / (2 * np.pi) ** self.dim + hfd = HypertoroidalFourierDistribution(fftshift(coeff_mat_tmp), 'identity') + hfd.C = fftshift(fftn(np.reshape(self.grid_values, self.n_grid_points))) + hfd_shifted = hfd.shift(shift_by) + + shifted_distribution = self.__class__(self.grid, self.grid_values) + shifted_distribution.grid_values = np.reshape(ifftn(ifftshift(hfd_shifted.C), overwrite_x=True), self.grid_values.shape) + return shifted_distribution + + def value_of_closest(self, xa): + p = np.empty(xa.shape[1]) + for i in range(xa.shape[1]): + dists = np.sum(np.minimum((self.grid - xa[:, i]) ** 2, (2 * np.pi - (self.grid - xa[:, i])) ** 2), axis=0) + min_ind = np.argmin(dists) + p[i] = self.grid_values[min_ind] + return p + + def convolve(self, other): + assert self.grid_type == 'CartesianProd' and other.grid_type == 'CartesianProd' + assert np.all(self.n_grid_points == other.no_of_grid_points) + assert np.all(self.grid == other.grid) + + this_tensor = np.reshape(self.grid_values, self.n_grid_points) + other_tensor = np.reshape(other.grid_values, other.no_of_grid_points) + + res_tensor = (2 * np.pi) ** self.dim / self.grid_values.size * ifftn(fftn(this_tensor) * fftn(other_tensor)) + convolved_distribution = self.__class__(self.grid, np.reshape(res_tensor, self.grid_values.shape)) + return convolved_distribution + + @staticmethod + def from_distribution(distribution, n_grid_points, grid_type='CartesianProd', enforce_pdf_nonnegative = True): + assert isinstance(grid_type, str) + if grid_type == 'CartesianProd' and isinstance(distribution, HypertoroidalFourierDistribution) and \ + (distribution.C.shape == n_grid_points or (np.isscalar(n_grid_points) and n_grid_points == distribution.C.shape[0])): + grid_values = distribution.pdf_on_grid() + grid = HypertoroidalGridDistribution.generate_cartesian_product_grid(n_grid_points) + hgd = HypertoroidalGridDistribution(grid, grid_values.flatten()) + hgd.grid_type = grid_type + else: + hgd = HypertoroidalGridDistribution.from_function(distribution.pdf, n_grid_points, distribution.dim, grid_type=grid_type, enforce_pdf_nonnegative=enforce_pdf_nonnegative) + return hgd + + @staticmethod + def from_function(fun, n_gridpoints, dim, grid_type='CartesianProd', enforce_pdf_nonnegative = True): + assert isinstance(grid_type, str) + if grid_type == 'CartesianProd': + if np.isscalar(n_gridpoints): + n_gridpoints = np.repeat(n_gridpoints, dim) + else: + assert len(n_gridpoints) == dim + + grid = HypertoroidalGridDistribution.generate_cartesian_product_grid(n_gridpoints) + else: + raise ValueError("Grid scheme not recognized") + + grid_values = np.apply_along_axis(fun, 1, grid) + sgd = HypertoroidalGridDistribution(grid_values, grid=grid, enforce_pdf_nonnegative=enforce_pdf_nonnegative, grid_type=grid_type) + return sgd + + def plot(self, *args, **kwargs): + hdd = HypertoroidalDiracDistribution(self.grid, (1 / len(self.grid_values)) * np.ones(len(self.grid_values))) + hdd.w = self.grid_values.T + h = hdd.plot(*args, **kwargs) + return h + + def plot_interpolated(self, *args, **kwargs): + if self.dim > 2: + raise ValueError("Can currently only plot for T1 and T2 torus.") + if self.enforce_pdf_nonnegative: + transformation = 'sqrt' + else: + transformation = 'identity' + sizes = np.repeat(len(self.grid_values) ** (1 / self.dim), self.dim) + fd = HypertoroidalFourierDistribution.from_function_values(np.reshape(self.grid_values, [sizes, 1]), sizes, transformation) + h = fd.plot(*args, **kwargs) + return h + + def trigonometric_moment(self, n): + hwd = HypertoroidalDiracDistribution(self.grid, self.grid_values.T / sum(self.grid_values)) + m = hwd.trigonometric_moment(n) + return m + + def slice_at(self, dims, val, use_fftn=True): + assert self.grid_type == 'CartesianProd', "This operation is only supported for grids generated based on a Cartesian product." + assert all([dim <= self.dim for dim in dims]), "Cannot perform this operation for a dimension that is higher than the dimensionality of the distribution." + + fvals_on_grid = np.reshape(self.grid_values, self.n_grid_points) + if np.all(val == np.zeros(len(val))): + grid_shifted_full = fvals_on_grid + elif use_fftn: + # Use HypertoroidalFourierDistribution, which uses FFTN + shift_vec = np.zeros(self.dim) + shift_vec[dims] = -val + hfd_sqrt = HypertoroidalFourierDistribution.from_function_values(fvals_on_grid, self.n_grid_points, 'sqrt') + hfd_sqrt_shifted = hfd_sqrt.shift(shift_vec) + grid_shifted_full = hfd_sqrt_shifted.pdf_on_grid() + else: + + raise NotImplementedError("This part of the code requires further adaptation to work in Python.") + return grid_shifted_full \ No newline at end of file diff --git a/pyrecest/tests/distributions/test_hypertoroidal_grid_distribution.py b/pyrecest/tests/distributions/test_hypertoroidal_grid_distribution.py new file mode 100644 index 000000000..86d6053ba --- /dev/null +++ b/pyrecest/tests/distributions/test_hypertoroidal_grid_distribution.py @@ -0,0 +1,42 @@ +import numpy as np +from pyrecest.distributions.hypertorus.hypertoroidal_grid_distribution import HypertoroidalGridDistribution +import unittest +from pyrecest.distributions.hypertorus.toroidal_wrapped_normal_distribution import ToroidalWrappedNormalDistribution +from pyrecest.distributions.hypertorus.hypertoroidal_wrapped_normal_distribution import HypertoroidalWrappedNormalDistribution +from pyrecest.distributions.hypertorus.hypertoroidal_mixture import HypertoroidalMixture + + +class HypertoroidalGridDistributionTest(unittest.TestCase): + + def test_get_grid(self): + grid = np.array([[1, 2, 3, 4], [1, 2, 3, 4]]).T + hgd = HypertoroidalGridDistribution(np.array([1, 1, 1, 1]) / ((2 * np.pi) ** 2), grid=grid) + np.testing.assert_allclose(hgd.get_grid(), grid) + np.testing.assert_allclose(np.shape(hgd.get_grid()), (4, 2)) + + def test_approx_vmmixture_t2(self): + dist = HypertoroidalMixture([ + ToroidalWrappedNormalDistribution(np.array([1,1]),np.array([[1,0.5],[0.5,1]])), + ToroidalWrappedNormalDistribution(np.array([3,3]),np.array([[1,-0.5],[-0.5,1]]))], + np.array([0.5,0.5])) + + hgd = HypertoroidalGridDistribution.from_distribution(dist, 31) + np.testing.assert_almost_equal(hgd.grid_values, dist.pdf(hgd.get_grid()), decimal=6) + self.assertTrue((np.min(hgd.get_grid(), axis=0) == np.array([0, 0])).all()) + self.assertTrue((np.max(hgd.get_grid(), axis=0) > 6).all()) + self.assertEqual(hgd.grid_type, 'CartesianProd') + + def test_from_function_3D(self): + np.random.seed(0) + test_points = np.random.rand(3, 30) + C = np.array([[0.7, 0.4, 0.2], [0.4, 0.6, 0.1], [0.2, 0.1, 1]]) + mu = 2 * np.pi * np.random.rand(3, 1) + hwnd = HypertoroidalWrappedNormalDistribution(mu, C) + n_grid_points = [27, 27, 27] + hfd_id = HypertoroidalGridDistribution.from_function(hwnd.pdf, n_grid_points, 3, 'CartesianProd') + self.assertIsInstance(hfd_id, HypertoroidalGridDistribution) + np.testing.assert_almost_equal(hfd_id.pdf(test_points), hwnd.pdf(test_points), decimal=6) + + +if __name__ == '__main__': + unittest.main() From 47627e7f9ec041b4f20efcbfb5aa33afe6ca3363 Mon Sep 17 00:00:00 2001 From: Florian Pfaff Date: Sun, 17 Sep 2023 11:40:16 +0200 Subject: [PATCH 2/2] Added StateSpaceSubdivisionFilter --- .../filters/state_space_subdivision_filter.py | 114 ++++++++++++++++++ .../test_state_space_subdivision_filter.py | 108 +++++++++++++++++ 2 files changed, 222 insertions(+) create mode 100644 pyrecest/filters/state_space_subdivision_filter.py create mode 100644 pyrecest/tests/filters/test_state_space_subdivision_filter.py diff --git a/pyrecest/filters/state_space_subdivision_filter.py b/pyrecest/filters/state_space_subdivision_filter.py new file mode 100644 index 000000000..9b7421dda --- /dev/null +++ b/pyrecest/filters/state_space_subdivision_filter.py @@ -0,0 +1,114 @@ +import numpy as np +import warnings +from scipy.stats import multivariate_normal +from .abstract_hypercylindrical_filter import AbstractHypercylindricalFilter +from pyrecest.distributions.conditional.sd_half_cond_sd_half_grid_distribution import SdHalfCondSdHalfGridDistribution + +class StateSpaceSubdivisionFilter(AbstractHypercylindricalFilter): + def __init__(self): + self.apd = None + + def setState(self, apd_): + if not self.apd.is_empty() and (len(self.apd.gaussians) != len(apd_.gaussians)): + warnings.warn("Number of components differ.", "LinPeriodic:NoComponentsDiffer") + self.apd = apd_ + + def predictLinear(self, transitionDensity=None, covarianceMatrices=None, systemMatrices=None, linearInputVectors=None): + if transitionDensity is not None: + # Various assert checks + pass + n_areas = len(self.apd.linearDistributions) + + if transitionDensity is None and covarianceMatrices is None and systemMatrices is None and linearInputVectors is None: + # Case 1 + pass + elif transitionDensity is None: + # Case 2 + pass + else: + # Case 3 + pass + + def update(self, likelihoodPeriodicGrid=None, likelihoodsLinear=None): + if isinstance(likelihoodPeriodicGrid, AbstractDistribution): + likelihoodPeriodicGrid = likelihoodPeriodicGrid.pdf(self.apd.gd.getGrid()).T + + if likelihoodPeriodicGrid is None and likelihoodsLinear is None: + warnings.warn("Nothing to do for this update step.", "StateSpaceSubdivisionFilter:NoParamsForUpdate") + return + + if likelihoodPeriodicGrid is not None: + self.apd.gd.gridValues = self.apd.gd.gridValues * likelihoodPeriodicGrid + + if likelihoodsLinear is not None: + # Update current linear distributions + pass + + self.apd.gd = self.apd.gd.normalize(warnUnnorm=False) + + def get_estimate(self): + return self.apd + + def get_point_estimate(self): + return self.apd.hybridMean() + + def predict_linear(sysModel, sdGrid, sdSubDivGaussian): + grid = sdGrid.grid + sdHalfCondSdHalf = SdHalfCondSdHalfGridDistribution.fromFunction( + lambda x, y: sysModel.transitionDensity(0, x, y), + 17, + funDoesCartesianProduct=False, + gridType='eq_point_set_symm', + dim=2 * sysModel.dim + ) + + cartesian_product = np.array(np.meshgrid(range(grid.shape[1]), range(grid.shape[1]))).T.reshape(-1, 2) + + sdSubDivGaussianNew = [] + for i, j in cartesian_product: + temp = sdSubDivGaussian[i].multiply(sdSubDivGaussian[j]) + temp = sdSubDivGaussian[i].multiply(sdHalfCondSdHalf.gridValues[j, i]) + sdSubDivGaussianNew.append(temp) + sdSubDivGaussianNew = reduce(lambda x, y: x.add(y), sdSubDivGaussianNew) + + sdSubDivGaussianNew.gridValues = np.array([sdSubDivGaussianNew.gridValues[j, i] for i, j in cartesian_product]) + + sdGridNew = sdGrid + sdGridNew.gridValues = sdSubDivGaussianNew.gridValues + + return sdGridNew, sdSubDivGaussianNew + + def update(self, likelihood_periodic_grid, likelihoods_linear): + if isinstance(likelihood_periodic_grid, AbstractDistribution): + likelihood_periodic_grid = likelihood_periodic_grid.pdf(self.apd.gd.get_grid()).T + + assert (likelihood_periodic_grid.size == 0 or + np.array_equal(likelihood_periodic_grid.shape, self.apd.gd.grid_values.shape)) + assert (likelihoods_linear.size == 0 or + all([ld.dim == self.apd.linD for ld in likelihoods_linear])) + + assert len(likelihoods_linear) <= 1 or len(likelihoods_linear) == self.apd.gd.grid_values.shape[0] + + if likelihood_periodic_grid.size == 0 and likelihoods_linear.size == 0: + print("Warning: Nothing to do for this update step.") + return + + if likelihood_periodic_grid.size != 0: + self.apd.gd.grid_values *= likelihood_periodic_grid + + if likelihoods_linear.size != 0: + mu_preds = np.array([ld.mu for ld in self.apd.linear_distributions]).T + mu_likelihoods = np.array([ld.mu for ld in likelihoods_linear]).T + covs = np.dstack([ld.C for ld in self.apd.linear_distributions]) + np.dstack([ld.C for ld in likelihoods_linear]) + + self.apd.gd.grid_values *= multivariate_normal.pdf(mu_preds.T, mu_likelihoods.T, covs) + + for i, ld in enumerate(self.apd.linear_distributions): + j = i if len(likelihoods_linear) > 1 else 0 + C_est_inv_curr = np.linalg.inv(ld.C) + np.linalg.inv(likelihoods_linear[j].C) + mu_est_curr = np.linalg.solve(C_est_inv_curr, np.dot(ld.C, ld.mu) + np.dot(likelihoods_linear[j].C, likelihoods_linear[j].mu)) + + ld.mu = mu_est_curr + ld.C = np.linalg.inv(C_est_inv_curr) + + self.apd.gd.normalize(warn_unnorm=False) diff --git a/pyrecest/tests/filters/test_state_space_subdivision_filter.py b/pyrecest/tests/filters/test_state_space_subdivision_filter.py new file mode 100644 index 000000000..139ab613c --- /dev/null +++ b/pyrecest/tests/filters/test_state_space_subdivision_filter.py @@ -0,0 +1,108 @@ +import unittest +import numpy as np +from pyrecest.distributions.cart_prod.state_space_subdivision_distribution import StateSpaceSubdivisionDistribution +from pyrecest.distributions.hypersphere_subset.hyperhemispherical_grid_distribution import HyperhemisphericalGridDistribution +from pyrecest.distributions.cart_prod.custom_hypercylindrical_distribution import CustomHypercylindricalDistribution +from pyrecest.distributions.hypersphere_subset.hemispherical_uniform_distribution import HemisphericalUniformDistribution +from pyrecest.distributions import (HyperhemisphericalUniformDistribution, GaussianDistribution) + +class StateSpaceSubdivisionDistributionTest(unittest.TestCase): + + def test_constructor(self): + n = 100 + rbd = StateSpaceSubdivisionDistribution(HyperhemisphericalGridDistribution.from_distribution( + HyperhemisphericalUniformDistribution(3), n), + np.tile(GaussianDistribution(np.array([0, 0, 0]), 1000 * np.eye(3)), (n, 1))) + + self.assertEqual(rbd.linear_distributions.shape, (100, 1)) + self.assertEqual(rbd.gd.grid_values.shape, (100, 1)) + + # TODO more tests + + def test_from_function_h2xr(self): + np.random.seed(0) # Equivalent to rng default in MATLAB + hud = HemisphericalUniformDistribution() + gauss = GaussianDistribution(0, 1) + cd = CustomHypercylindricalDistribution(lambda x: hud.pdf(x[:3, :]) * gauss.pdf(x[3, :]), 3, 1) + + apd = StateSpaceSubdivisionDistribution.from_function(lambda x: cd.pdf(x), 101, 3, 1, 'hyperhemisphere') + + points_bounded = np.random.randn(3, 100) + points_cart = np.vstack((points_bounded / np.linalg.norm(points_bounded, axis=0), np.random.randn(1, 100))) + + np.testing.assert_allclose(apd.pdf(points_cart, 'nearest_neighbor', 'nearest_neighbor'), cd.pdf(points_cart), atol=5e-17) + np.testing.assert_allclose(apd.pdf(points_cart, 'nearest_neighbor', 'grid_default'), cd.pdf(points_cart), atol=5e-17) + +if __name__ == '__main__': + unittest.main() + + +""" + +import numpy as np +from pyrecest.distributions import VonMisesFisherDistribution + +from pyrecest.distributions.hypersphere_subset.hemispherical_grid_distribution import HemisphericalGridDistribution +from pyrecest.distributions.hypersphere_subset.hemispherical_uniform_distribution import HemisphericalUniformDistribution +from pyrecest.distributions.cart_prod.state_space_subdivision_gaussian_distribution import StateSpaceSubdivisionGaussianDistribution +from pyrecest.filters.state_space_subdivision_filter import StateSpaceSubdivisionFilter +from pyrecest.distributions import GaussianDistribution +from pyrecest.distributions.hypersphere_subset.custom_hemispherical_distribution import CustomHemisphericalDistribution + +class StateSpaceSubdivisionFilterTest: + def test_update(self): + n = 10 + gd_init = HemisphericalGridDistribution.from_distribution(HemisphericalUniformDistribution(), n) + apd = StateSpaceSubdivisionGaussianDistribution(gd_init, np.array([GaussianDistribution(np.array([1, 1]), 100 * np.eye(2)) for _ in range(n)])) + + lpf = StateSpaceSubdivisionFilter() + lpf.set_state(apd) + + # Update with the same distribution + lpf.update(None, apd.linear_distributions) + assert np.allclose(np.stack([d.mu for d in lpf.apd.linear_distributions]), np.tile(np.array([1, 1]), (n, 1)).T) + assert np.allclose(np.stack([d.cov for d in lpf.apd.linear_distributions]), np.tile(50 * np.eye(2), (n, 1, 1))) + + # Update with a non-diagonal distribution + gaussian_likelihood = GaussianDistribution(np.array([3, 5]), np.array([[25, 5], [5, 25]])) + posterior = lpf.apd.linear_distributions[0].multiply(gaussian_likelihood) + lpf.update(None, gaussian_likelihood) + assert np.allclose(np.stack([d.mu for d in lpf.apd.linear_distributions]), np.tile(posterior.mu, (n, 1)).T, rtol=1e-15) + assert np.allclose(np.stack([d.cov for d in lpf.apd.linear_distributions]), np.tile(posterior.cov, (n, 1, 1)), rtol=1e-15) + + # Update with a VMF and no linear part + vmf_full_sphere = VonMisesFisherDistribution(np.array([0, 1, 1]) / np.sqrt(2), 1) + noise = CustomHemisphericalDistribution(lambda x: vmf_full_sphere.pdf(x) + vmf_full_sphere.pdf(-x)) + lpf.update(noise) + assert np.allclose(np.stack([d.mu for d in lpf.apd.linear_distributions]), np.tile(posterior.mu, (n, 1)).T, rtol=1e-15) + assert np.allclose(np.stack([d.cov for d in lpf.apd.linear_distributions]), np.tile(posterior.cov, (n, 1, 1)), rtol=1e-15) + + # Should be the same as directly approximating the density + gd_forced_norm = HemisphericalGridDistribution.from_distribution(noise, n).normalize(tol=0) + assert np.allclose(lpf.apd.gd.grid_values, gd_forced_norm.grid_values, rtol=5e-16) + + # Update with a VMF and Gaussian with all Gaussians equal + likelihood = GaussianDistribution(np.array([1, -1]), np.array([[15, -0.5], [-0.5, 15]])) + posteriors = np.array([d.multiply(likelihood) for d in lpf.apd.linear_distributions]) + lpf.update(noise, likelihood) + + assert np.allclose(np.stack([d.mu for d in lpf.apd.linear_distributions]), np.stack([p.mu for p in posteriors]), rtol=5e-15) + assert np.allclose(np.stack([d.cov for d in lpf.apd.linear_distributions]), np.stack([p.cov for p in posteriors]), rtol=5e-15) + + posterior_numerical = CustomHemisphericalDistribution(lambda x: noise.pdf(x) * noise.pdf(x)) + gd_forced_norm = HemisphericalGridDistribution.from_distribution(posterior_numerical, n).normalize(tol=0) + + assert np.allclose(lpf.apd.gd.grid_values, gd_forced_norm.grid_values, rtol=5e-16) + + # Final test with different linear distributions + likelihoods = np.array([GaussianDistribution(np.array([i, -i]), np.array([[15, i / 10], [i / 10, 15]])) for i in range(1, n + 1)]) + posteriors = np.array([lpf.apd.linear_distributions[0].multiply(likelihood) for likelihood in likelihoods]) + lpf.update(noise, likelihoods) + + assert np.allclose(np.stack([d.mu for d in lpf.apd.linear_distributions]), np.stack([p.mu for p in posteriors]), rtol=5e-15) + assert np.allclose(np.stack([d.cov for d in lpf.apd.linear_distributions]), np.stack([p.cov for p in posteriors]), rtol=5e-15) + + assert np.sum(np.abs(lpf.apd.gd.grid_values - (1 / lpf.apd.gd.get_manifold_size()))) > np.sum(np.abs(gd_forced_norm.grid_values - (1 / lpf.apd.gd.get_manifold_size()))) + + +""" \ No newline at end of file