Added HypersphericalParticleFilter

Author: FlorianPfaff

pyrecest/distributions/__init__.py

@@ -147,6 +147,9 @@
 from pyrecest.distributions.hypersphere_subset.custom_hyperhemispherical_distribution import (
     CustomHyperhemisphericalDistribution,
 )
+from pyrecest.distributions.hypersphere_subset.hyperspherical_dirac_distribution import (
+    HypersphericalDiracDistribution,
+)
 from pyrecest.distributions.hypersphere_subset.hyperhemispherical_dirac_distribution import (
     HyperhemisphericalDiracDistribution,
 )
@@ -254,6 +257,7 @@
 ]
 
 __all__ = aliases + [
+    "HypersphericalDiracDistribution",
     "GeneralizedKSineSkewedVonMisesDistribution",
     "AbstractBoundedDomainDistribution",
     "AbstractBoundedNonPeriodicDistribution",

pyrecest/filters/hyperspherical_particle_filter.py

@@ -0,0 +1,43 @@
+from .abstract_hyperspherical_filter import AbstractHypersphericalFilter
+from pyrecest.distributions import VonMisesDistribution, AbstractHypersphericalDistribution
+from pyrecest.distributions.hypersphere_subset.hyperspherical_dirac_distribution import HypersphericalDiracDistribution
+import warnings
+# pylint: disable=redefined-builtin,no-name-in-module,no-member
+from pyrecest.backend import tile, eye, sum, linalg
+
+class HypersphericalParticleFilter(AbstractHypersphericalFilter):
+    def __init__(self, n_particles, dim):
+        self.wd = HypersphericalDiracDistribution(tile(eye(dim, 1), (1, n_particles)))
+
+    @property
+    def filter_state(self):
+        return self._filter_state
+    
+    @filter_state.setter
+    def filter_state(self, new_state):
+        """Sets the filter  state to new_state if it is a type of AbstractHypersphericalDistribution."""
+        if not isinstance(new_state, AbstractHypersphericalDistribution):
+            raise TypeError("new_state must be an instance of AbstractHypersphericalDistribution")
+        if not isinstance(new_state, HypersphericalDiracDistribution):
+            new_state = HypersphericalDiracDistribution(new_state.sample(self._filter_state.d.shape[0]))
+        self._filter_state = new_state
+
+    def predict_identity(self, noise_distribution):
+        self.predict_nonlinear(lambda x: x, noise_distribution)
+
+    def update_identity(self, noise_distribution, z):
+        assert isinstance(noise_distribution, VonMisesDistribution), "Currently, only VMF distributed noise terms are supported."
+        if z is not None:
+            noise_distribution.mu = z
+            warnings.warn("Warning: update_identity: mu of noise_distribution is replaced by measurement...")
+
+    def update_nonlinear(self, likelihood, z=None):
+        if z is None:
+            self.wd = self.wd.reweigh(likelihood)
+        else:
+            self.wd = self.wd.reweigh(lambda x: likelihood(z, x))
+
+    def get_estimate_mean(self):
+        vec_sum = sum(self.wd.d * tile(self.wd.w, (self.dim, 1)), axis=1)
+        mean = vec_sum / linalg.norm(vec_sum)
+        return mean

pyrecest/tests/filters/test_hyperspherical_particle_filter.py

@@ -0,0 +1,52 @@
+import unittest
+import numpy as np
+import warnings
+from pyrecest.distributions import VonMisesFisherDistribution, HypersphericalDiracDistribution
+# from pyrecest.distributions
+from pyrecest.filters.hyperspherical_particle_filter import HypersphericalParticleFilter
+from pyrecest.backend import array, allclose, ones, linalg
+
+class HypersphericalParticleFilterTest(unittest.TestCase):
+    def test3D(self):
+        np.random.seed(1)
+        n_samples = 20000
+        vmf_init = VonMisesFisherDistribution(array([1.0, 0.0, 0.0]), 10)
+        vmf_sys = VonMisesFisherDistribution(array([0.0, 0.0, 1.0]), 10)
+        vmf_meas = VonMisesFisherDistribution(array([0.0, 0.0, 1.0]), 3)
+
+        hpf = HypersphericalParticleFilter(n_samples, 3)
+        hpf.filter_state = HypersphericalDiracDistribution(vmf_init.sample(n_samples))
+
+        est = hpf.get_estimate_mean()
+        self.assertIsInstance(est, np.ndarray)
+        vmf_init_mean = vmf_init.moment()
+        hpf_mean = est
+        self.assertTrue(np.allclose(vmf_init_mean, hpf_mean, atol=1e-2))
+
+        # Prediction step
+        hpf.predict_identity(vmf_sys)
+
+        # Update steps
+        with warnings.catch_warnings():
+            warnings.simplefilter("ignore")
+            hpf.update_identity(vmf_meas, array([0.0, 0.0, 1.0]))
+            hpf.update_identity(vmf_meas, array([0.0, 0.0, 1.0]))
+            hpf.update_identity(vmf_meas, array([0.0, 0.0, 1.0]))
+
+        hpf_est_mean = hpf.get_estimate_mean()
+        self.assertEqual(hpf_est_mean.shape, (3,))
+        expected_mean = array([0.0, 0.0, 1.0])
+        self.assertTrue(allclose(hpf_est_mean, expected_mean, atol=0.05))
+
+        # Reset state
+        hpf.filter_state = HypersphericalDiracDistribution(vmf_init.sample(n_samples))
+
+        # Nonlinear update with a function that returns ones
+        def f(z, x):
+            return ones(x.shape[1])
+
+        z = 3
+        est = hpf.get_estimate_mean()
+        self.assertAlmostEqual(linalg.norm(est), 1, delta=1e-10)
+        hpf.update_nonlinear(f, z)
+        self.assertTrue(allclose(est, hpf.get_estimate_mean(), atol=1e-2))