bayesfilter 0.0.2__tar.gz

bayesfilter-0.0.2/PKG-INFO

@@ -0,0 +1,140 @@
+Metadata-Version: 2.1
+Name: bayesfilter
+Version: 0.0.2
+Author: Hugo Hadfield
+License: MIT
+Description-Content-Type: text/markdown
+Requires-Dist: numpy
+
+# BayesFilter
+
+BayesFilter is a Python library for Bayesian filtering and smoothing. This library provides tools for implementing Bayesian filters, Rauch-Tung-Striebel smoothers, and other related methods. The only dependency is NumPy.
+
+## Installation
+
+To install BayesFilter, just use `pip`:
+
+```bash
+pip install bayesfilter
+```
+
+## Usage
+
+### Basic Structure
+
+The library consists of several modules, each responsible for different parts of the Bayesian filtering and smoothing process:
+
+- `distributions.py`: Defines the distribution classes, including the Gaussian distribution used for the filters.
+- `filtering.py`: Implements the BayesianFilter class, which runs the filtering process.
+- `model.py`: Contains the StateTransitionModel class for state transitions.
+- `observation.py`: Defines the Observation class for observation models.
+- `smoothing.py`: Implements the RTS (Rauch-Tung-Striebel) smoother.
+- `unscented.py`: Provides functions for the unscented transform.
+- `utilities.py`: Contains utility functions used throughout the library.
+- `test_filtering_smoothing.py`: Contains tests for filtering and smoothing.
+
+### Example
+
+Here is a basic example of how to set up and run a Bayesian filter with the provided library:
+
+1. **Setup Functions**:
+
+```python
+def setup_functions():
+    def transition_func(x, delta_t_s):
+        return np.array([x[0]])
+    
+    def transition_jacobian_func(x, delta_t_s):
+        return np.array([[1.0]])
+
+    def observation_func(x):
+        return np.array([np.sin(x[0]), np.cos(x[0])])
+    
+    def observation_jacobian_func(x):
+        return np.array([[np.cos(x[0])], [-np.sin(x[0])]])
+    
+    return transition_func, transition_jacobian_func, observation_func, observation_jacobian_func
+```
+
+2. **Setup Filter and Observations**:
+
+```python
+def setup_filter_and_observations():
+    rng = np.random.default_rng(0)
+    transition_func, transition_jacobian_func, observation_func, observation_jacobian_func = setup_functions()
+
+    transition_model = StateTransitionModel(
+        transition_func, 
+        1e-8*np.eye(1),
+        transition_jacobian_func
+    )
+    initial_state = Gaussian(np.array([0.0]), np.eye(1))
+    filter = BayesianFilter(transition_model, initial_state)
+
+    true_state = np.array([-0.1])
+    noise_std = 0.2
+    observations = []
+    for theta in np.linspace(0, 2*np.pi, 1000):
+        observation = observation_func(true_state) + rng.normal(0, noise_std, 2)
+        observations.append(Observation(observation, noise_std*np.eye(2), observation_func, observation_jacobian_func))
+    return filter, observations, true_state
+```
+
+3. **Run Filter**:
+
+```python
+def test_filter_noisy_sin(use_jacobian=True):
+    filter, observations, true_state = setup_filter_and_observations()
+    filter.run(observations, np.linspace(0, 2*np.pi, 1000), 100.0, use_jacobian=use_jacobian)
+    np.testing.assert_allclose(filter.state.mean(), true_state, atol=1e-2)
+```
+
+### Tests
+
+The library includes a set of tests to ensure the functionality of the filtering and smoothing algorithms. These can be run as follows:
+
+```bash
+python test_filtering_smoothing.py
+```
+
+## Documentation
+
+### `distributions.py`
+
+Defines the Gaussian distribution class used for state representation and propagation.
+
+### `filtering.py`
+
+Implements the `BayesianFilter` class, responsible for running the filtering process with predict and update steps.
+
+### `model.py`
+
+Contains the `StateTransitionModel` class, representing the state transition model.
+
+### `observation.py`
+
+Defines the `Observation` class, representing the observation model.
+
+### `smoothing.py`
+
+Implements the `RTS` class for Rauch-Tung-Striebel smoothing.
+
+### `unscented.py`
+
+Provides functions for the unscented transform, including `unscented_transform` and `propagate_gaussian`.
+
+### `utilities.py`
+
+Contains utility functions like `propagate_covariance`.
+
+### `test_filtering_smoothing.py`
+
+Includes tests for filtering and smoothing to validate the implementation.
+
+## Author
+
+Hugo Hadfield
+
+## License
+
+This project is licensed under the MIT License.

bayesfilter-0.0.2/README.md

@@ -0,0 +1,132 @@
+# BayesFilter
+
+BayesFilter is a Python library for Bayesian filtering and smoothing. This library provides tools for implementing Bayesian filters, Rauch-Tung-Striebel smoothers, and other related methods. The only dependency is NumPy.
+
+## Installation
+
+To install BayesFilter, just use `pip`:
+
+```bash
+pip install bayesfilter
+```
+
+## Usage
+
+### Basic Structure
+
+The library consists of several modules, each responsible for different parts of the Bayesian filtering and smoothing process:
+
+- `distributions.py`: Defines the distribution classes, including the Gaussian distribution used for the filters.
+- `filtering.py`: Implements the BayesianFilter class, which runs the filtering process.
+- `model.py`: Contains the StateTransitionModel class for state transitions.
+- `observation.py`: Defines the Observation class for observation models.
+- `smoothing.py`: Implements the RTS (Rauch-Tung-Striebel) smoother.
+- `unscented.py`: Provides functions for the unscented transform.
+- `utilities.py`: Contains utility functions used throughout the library.
+- `test_filtering_smoothing.py`: Contains tests for filtering and smoothing.
+
+### Example
+
+Here is a basic example of how to set up and run a Bayesian filter with the provided library:
+
+1. **Setup Functions**:
+
+```python
+def setup_functions():
+    def transition_func(x, delta_t_s):
+        return np.array([x[0]])
+    
+    def transition_jacobian_func(x, delta_t_s):
+        return np.array([[1.0]])
+
+    def observation_func(x):
+        return np.array([np.sin(x[0]), np.cos(x[0])])
+    
+    def observation_jacobian_func(x):
+        return np.array([[np.cos(x[0])], [-np.sin(x[0])]])
+    
+    return transition_func, transition_jacobian_func, observation_func, observation_jacobian_func
+```
+
+2. **Setup Filter and Observations**:
+
+```python
+def setup_filter_and_observations():
+    rng = np.random.default_rng(0)
+    transition_func, transition_jacobian_func, observation_func, observation_jacobian_func = setup_functions()
+
+    transition_model = StateTransitionModel(
+        transition_func, 
+        1e-8*np.eye(1),
+        transition_jacobian_func
+    )
+    initial_state = Gaussian(np.array([0.0]), np.eye(1))
+    filter = BayesianFilter(transition_model, initial_state)
+
+    true_state = np.array([-0.1])
+    noise_std = 0.2
+    observations = []
+    for theta in np.linspace(0, 2*np.pi, 1000):
+        observation = observation_func(true_state) + rng.normal(0, noise_std, 2)
+        observations.append(Observation(observation, noise_std*np.eye(2), observation_func, observation_jacobian_func))
+    return filter, observations, true_state
+```
+
+3. **Run Filter**:
+
+```python
+def test_filter_noisy_sin(use_jacobian=True):
+    filter, observations, true_state = setup_filter_and_observations()
+    filter.run(observations, np.linspace(0, 2*np.pi, 1000), 100.0, use_jacobian=use_jacobian)
+    np.testing.assert_allclose(filter.state.mean(), true_state, atol=1e-2)
+```
+
+### Tests
+
+The library includes a set of tests to ensure the functionality of the filtering and smoothing algorithms. These can be run as follows:
+
+```bash
+python test_filtering_smoothing.py
+```
+
+## Documentation
+
+### `distributions.py`
+
+Defines the Gaussian distribution class used for state representation and propagation.
+
+### `filtering.py`
+
+Implements the `BayesianFilter` class, responsible for running the filtering process with predict and update steps.
+
+### `model.py`
+
+Contains the `StateTransitionModel` class, representing the state transition model.
+
+### `observation.py`
+
+Defines the `Observation` class, representing the observation model.
+
+### `smoothing.py`
+
+Implements the `RTS` class for Rauch-Tung-Striebel smoothing.
+
+### `unscented.py`
+
+Provides functions for the unscented transform, including `unscented_transform` and `propagate_gaussian`.
+
+### `utilities.py`
+
+Contains utility functions like `propagate_covariance`.
+
+### `test_filtering_smoothing.py`
+
+Includes tests for filtering and smoothing to validate the implementation.
+
+## Author
+
+Hugo Hadfield
+
+## License
+
+This project is licensed under the MIT License.

bayesfilter-0.0.2/bayesfilter/__init__.py

@@ -0,0 +1,8 @@
+"""
+bayesfilter.
+
+A python library for bayesian filtering and smoothing.
+"""
+
+__version__ = "0.0.2"
+__author__ = "Hugo Hadfield"

bayesfilter-0.0.2/bayesfilter/distributions.py

@@ -0,0 +1,163 @@
+
+from typing import List
+
+import numpy as np
+
+DEFAULT_ALPHA = 1e-3
+DEFAULT_BETA = 2.0
+
+
+def compute_lambda(n: int, alpha: float = DEFAULT_ALPHA) -> int:
+    """
+    Compute the lambda parameter for the unscented transform.
+    """
+    kappa = 3 - n
+    l = alpha*alpha*(n + kappa) - n
+    return l
+
+
+def compute_covariance_mean_weight(
+        dimension: int, 
+        alpha: float = DEFAULT_ALPHA, 
+        beta: float = DEFAULT_BETA
+    ) -> np.ndarray:
+    """
+    Compute the covariance weight for the sigma points representing the mean.
+    """
+    lamb = compute_lambda(dimension)
+    return lamb / (dimension + lamb) + (1 - alpha*alpha + beta)
+
+
+class Distribution:
+    """
+    Represents a generic probability distribution.
+    """
+    def dimension(self):
+        raise NotImplementedError
+
+    def mean(self):
+        raise NotImplementedError
+    
+    def covariance(self):
+        raise NotImplementedError
+    
+    def compute_sigma_points(self):
+        raise NotImplementedError
+    
+    def sample(self):
+        raise NotImplementedError
+    
+    def from_samples(self, samples: List[np.ndarray]):
+        raise NotImplementedError
+
+    def __repr__(self):
+        raise NotImplementedError
+    
+
+class Gaussian(Distribution):
+    """
+    Represents a Gaussian distribution.
+    """
+    def __init__(self, mean, covariance, rng=None):
+        self._mean = mean
+        self._covariance = covariance
+        self._rng = np.random.default_rng(rng)
+
+    @property
+    def rng(self):
+        return self._rng
+
+    def mean(self) -> np.ndarray:
+        """
+        Return the mean of the Gaussian distribution.
+        """
+        return self._mean
+    
+    def covariance(self) -> np.ndarray:
+        """
+        Return the covariance of the Gaussian distribution.
+        """
+        return self._covariance
+    
+    def dimension(self) -> int:
+        """
+        Return the dimension of the Gaussian distribution.
+        """
+        return len(self.mean())
+
+    def sqrt_covariance(self) -> np.ndarray:
+        """
+        Return the square root of the covariance matrix.
+        """
+        if self.dimension() == 1:
+            return np.array([[np.sqrt(self.covariance()[0, 0])]])
+        return np.linalg.cholesky(self.covariance())
+    
+    def compute_sigma_points(self) -> np.ndarray:
+        """
+        Compute the sigma points for the Gaussian distribution.
+        """
+        mean = self.mean()
+        sqrt_covariance = self.sqrt_covariance()
+        n = self.dimension()
+        lamb = compute_lambda(n)
+        factor = np.sqrt(n + lamb)
+        sigma_points = np.zeros((2*n + 1, n))
+        sigma_points[0, :] = mean
+        counter = 1
+        for i in range(n): # 2n + 1 sigma points
+            sigma_points[counter, :] = mean + factor*sqrt_covariance[:, i]
+            counter += 1
+            sigma_points[counter, :] = mean - factor*sqrt_covariance[:, i]
+            counter += 1
+        return sigma_points
+    
+    def compute_weights(self) -> np.ndarray:
+        """
+        Compute the weights for the sigma points.
+        """
+        n = self.dimension()
+        lamb = compute_lambda(n)
+        weights = np.zeros(2*n + 1)
+        # The first weight is for the mean
+        weights[0] = lamb / (n + lamb) 
+        for i in range(2*n):
+            weights[i + 1] = 1.0 / (2.0*(n + lamb))
+        return weights
+    
+    def from_sigma_points(self, sigma_points: np.ndarray, weights: np.ndarray):
+        """
+        Create a Gaussian distribution from the given sigma points.
+        Equations coming from https://arxiv.org/pdf/2104.01958
+        """
+        new_n = sigma_points.shape[1]
+        new_mean = np.zeros(new_n)
+        for i in range(sigma_points.shape[0]):
+            new_mean += weights[i] * sigma_points[i, :]
+        new_covariance = np.zeros((new_n, new_n))
+        for i in range(sigma_points.shape[0]):
+            diff = sigma_points[i, :] - new_mean
+            if i == 0:
+                new_covariance += compute_covariance_mean_weight(self.dimension()) * np.outer(diff, diff)
+            else:
+                new_covariance += weights[i] * np.outer(diff, diff)
+        return Gaussian(new_mean, new_covariance, self.rng)
+
+    def sample(self) -> np.ndarray:
+        """
+        Sample from the Gaussian distribution.
+        """
+        return self.rng.multivariate_normal(self.mean(), self.covariance())
+    
+    def from_samples(self, samples: List[np.ndarray]):
+        """
+        Create a Gaussian distribution from the given samples.
+        """
+        sample_array = np.array(samples, dtype=np.float64)
+        new_mean = np.mean(sample_array, axis=0)
+        new_covariance = np.cov(sample_array, rowvar=False)
+        return Gaussian(new_mean, new_covariance, self.rng)
+
+    def __repr__(self):
+        return f'Gaussian(mean={self.mean()}, covariance={self.covariance()})'
+    

bayesfilter-0.0.2/bayesfilter/filtering.py

@@ -0,0 +1,122 @@
+from typing import List, Tuple
+
+import numpy as np
+
+try:
+    import tqdm
+except ImportError:
+    tqdm = None
+
+from bayesfilter.distributions import Gaussian
+from bayesfilter.model import StateTransitionModel
+from bayesfilter.observation import Observation
+
+class BayesianFilter:
+    def __init__(self, state_transition_model: StateTransitionModel, initial_state: Gaussian):
+        """
+        Set up the bayesian filter with the state transition model and initial state
+        """
+        self.transition_model = state_transition_model
+        self.state = initial_state
+
+    def predict(self, state: Gaussian, delta_t_s: float, use_jacobian: bool = True) -> Gaussian:
+        """
+        Predict the next state from a given state
+        """
+        # Predict the next state
+        return self.transition_model.predict(state, delta_t_s, use_jacobian=use_jacobian)
+    
+    def update(self, observation: Observation, predicted_state: Gaussian, use_jacobian: bool = False) -> Gaussian:
+        """
+        Condition the state on a observation
+        """
+        # Predict the observation (with noise)
+        predicted_obsurement, cross_covariance = observation.predict_with_cross_covariance(predicted_state, use_jacobian=use_jacobian)
+
+        # Compute the Kalman gain
+        kalman_gain = cross_covariance@np.linalg.inv(predicted_obsurement.noise_covariance)
+
+        # Compute the residual
+        residual = observation.observation - observation.observation_func(predicted_state.mean())
+
+        # Compute the new state
+        new_state = Gaussian(
+            mean=predicted_state.mean() + kalman_gain@residual,
+            covariance=predicted_state.covariance() - kalman_gain@predicted_obsurement.noise_covariance@kalman_gain.T
+        )
+
+        # Update the state
+        self.state = new_state
+        return new_state
+
+    def set_state(self, state: Gaussian):
+        """
+        Directly set the filter state
+        """
+        self.state = state
+
+    def run_synchronous(self, observations: List[Observation], times_s: List[float], use_jacobian=False) -> List[Gaussian]:
+        """
+        Run the BayesianFilter on a list of observations.
+        Assumes that the observations are at a roughly fixed time interval.
+        use_jacobian allows you to switch between an extended and unscented prediction and update
+        """
+        output_states = [self.state]  
+        for i, observation in enumerate(observations[:len(times_s)-1]):
+            delta_t_s = times_s[i+1] - times_s[i]
+            predicted_state = self.predict(self.state, delta_t_s, use_jacobian=use_jacobian)
+            self.update(observation, predicted_state, use_jacobian=use_jacobian)
+            output_states.append(self.state)
+        return output_states
+
+    def run(
+        self, 
+        observations: List[Observation], 
+        times_s: List[float], 
+        rate_hz: float, 
+        use_jacobian=False
+    ) -> Tuple[List[Gaussian], List[float]]:
+        """
+        Run the bayesian filter on a list of observations at a fixed rate, start at the start of times_s and end at the 
+        end of times_s. times_s referres to the observation times. use_jacobian allows you to switch
+        between an extended linearisation and an unscented transform for the prediction
+        """
+        filter_states = []
+        filter_times = []
+        start_time = times_s[0]
+        end_time = times_s[-1]
+        current_obs_index = 0
+        # Step through the times from start to finish
+        # Use tqdm to keep track of progress if available
+        print(f"Running BayesianFilter at {rate_hz} Hz with use_jacobian: {use_jacobian}", flush=True)
+        delta_t_s = 1.0/rate_hz
+        iterator = np.arange(start_time, end_time + delta_t_s, delta_t_s)
+        if tqdm is not None:
+            iterator = tqdm.tqdm(iterator)
+        for current_time in iterator:
+            # Gets a list of all observations that have occured between previous time and curent_time
+            current_obs = []
+            if current_obs_index < len(times_s):
+                next_obs_time = times_s[current_obs_index]
+                while next_obs_time <= current_time:
+                    current_obs.append(observations[current_obs_index])
+                    current_obs_index += 1
+                    if current_obs_index >= len(times_s):
+                        break
+                    next_obs_time = times_s[current_obs_index]
+            
+            # Predict the next state
+            predicted_state = self.predict(self.state, delta_t_s, use_jacobian=use_jacobian)
+
+            # Update the state with the observations, if we have any
+            for observation in current_obs:
+                predicted_state = self.update(observation, predicted_state, use_jacobian=use_jacobian)
+
+            # Set the state
+            self.set_state(predicted_state)
+
+            # Append the state
+            filter_states.append(self.state)
+            filter_times.append(current_time)
+
+        return filter_states, filter_times

bayesfilter-0.0.2/bayesfilter/model.py

@@ -0,0 +1,84 @@
+
+from typing import Callable, Optional
+
+import numpy as np
+
+from bayesfilter.distributions import Gaussian
+from bayesfilter.utilities import propagate_covariance
+from bayesfilter.unscented import (
+    propagate_gaussian, propagate_gaussian_cross_cov
+)
+
+
+class StateTransitionModel:
+    """
+    Represents a state transition model
+    """
+    def __init__(
+            self, 
+            transition_func: Callable[[np.ndarray, float], np.ndarray],
+            transition_noise_covariance: np.ndarray,
+            transition_jacobian_func: Optional[Callable[[np.ndarray, float], np.ndarray]] = None
+        ):
+        self.transition_func = transition_func
+        self.dimension = len(transition_noise_covariance)
+        self.transition_noise_covariance = transition_noise_covariance
+        self.transition_jacobian_func = transition_jacobian_func
+
+    def predict_no_noise_jacobian(self, state: Gaussian, delta_t_s: float):
+        """
+        Predict the next state without adding transition noise, uses the Jacobian of the
+        state transition function
+        """
+        new_state = Gaussian(
+            mean=self.transition_func(state.mean(), delta_t_s),
+            covariance=propagate_covariance(
+                jacobian=self.get_jacobian(state, delta_t_s),
+                covariance=state.covariance()
+            )
+        )
+        return new_state
+    
+    def predict_no_noise(self, state: Gaussian, delta_t_s: float, use_jacobian: bool = False):
+        """
+        Predict the next state without adding transition noise, uses the unscented transform
+        """
+        if use_jacobian:
+            return self.predict_no_noise_jacobian(state, delta_t_s)
+        # Unscented transform
+        new_mean, new_covariance = propagate_gaussian(
+            state.mean(),
+            state.covariance(),
+            lambda x: self.transition_func(x, delta_t_s)
+        )
+        return Gaussian(new_mean, new_covariance)
+    
+    def predict(self, state: Gaussian, delta_t_s: float, use_jacobian: bool = False):
+        """
+        Predict the next state
+        """
+        new_state = self.predict_no_noise(state, delta_t_s, use_jacobian)
+        return Gaussian(new_state.mean(), new_state.covariance() + self.transition_noise_covariance)
+    
+    def predict_with_cross_covariance(self, state: Gaussian, delta_t_s: float, use_jacobian=False):
+        """
+        Predict the next state and return the cross-covariance too
+        """
+        if use_jacobian:
+            pred_mean = self.transition_func(state.mean(), delta_t_s)
+            transition_func_jac = self.get_jacobian(state, delta_t_s)
+            pred_cov = propagate_covariance(transition_func_jac, state.covariance())
+            cross_cov = pred_cov@transition_func_jac.T
+            return Gaussian(pred_mean, pred_cov + self.transition_noise_covariance), cross_cov
+        pred_mean, pred_cov, cross_cov = propagate_gaussian_cross_cov(
+            state.mean(),
+            state.covariance(),
+            lambda x: self.transition_func(x, delta_t_s)
+        )
+        return Gaussian(pred_mean, pred_cov + self.transition_noise_covariance), cross_cov
+
+    def get_jacobian(self, state: Gaussian, delta_t_s: float):
+        if self.transition_jacobian_func is None:
+            raise ValueError("Transition Jacobian function not provided")
+        return self.transition_jacobian_func(state.mean(), delta_t_s)
+    

bayesfilter-0.0.2/bayesfilter/observation.py

@@ -0,0 +1,87 @@
+
+from typing import Callable, Optional
+
+import numpy as np
+
+from bayesfilter.distributions import Gaussian
+from bayesfilter.utilities import propagate_covariance
+from bayesfilter.unscented import (
+    propagate_gaussian, propagate_gaussian_cross_cov
+)
+
+class Observation:
+    """
+    Represents an observation model
+    """
+    def __init__(
+            self, 
+            observation: np.ndarray, 
+            noise_covariance: np.ndarray, 
+            observation_func: Callable[[np.ndarray], np.ndarray],
+            jacobian_func: Optional[Callable[[np.ndarray], np.ndarray]] = None
+        ):
+        self.observation = observation.copy()
+        self.noise_covariance = noise_covariance.copy()
+        self.observation_func = observation_func
+        self.dimension = len(noise_covariance)
+        self.jacobian_func = jacobian_func
+    
+    def predict_no_noise_jacobian(self, state: Gaussian):
+        """
+        Predict an observation from a state without adding observation noise
+        """
+        # Predict observation
+        pred_obs = self.observation_func(state.mean())
+        # Predict covariance
+        jac = self.get_jacobian(state.mean())
+        pred_cov = propagate_covariance(
+            jacobian=jac,
+            covariance=state.covariance()
+        )
+        return Observation(pred_obs, pred_cov, self.observation_func)
+    
+    def predict_no_noise(self, state: Gaussian, use_jacobian: bool = False):
+        """
+        Predict an observation from a state without adding observation noise
+        """
+        if use_jacobian:
+            return self.predict_no_noise_jacobian(state)
+        # Unscented transform
+        pred_mean, pred_cov = propagate_gaussian(
+            state.mean(),
+            state.covariance(),
+            self.observation_func
+        )
+        return Observation(pred_mean, pred_cov, self.observation_func)
+
+    def predict(self, state: Gaussian, use_jacobian: bool = False):
+        """
+        Predict an observation from a state
+        """
+        observation = self.predict_no_noise(state, use_jacobian)
+        observation.noise_covariance += self.noise_covariance
+        return observation
+    
+    def predict_with_cross_covariance(self, state: Gaussian, use_jacobian: bool = False):
+        """
+        Predict an observation from a state and also return the cross-covariance
+        """
+        if use_jacobian:
+            predicted_obs = self.observation_func(state.mean())
+            observation_jacobian = self.get_jacobian(state.mean())
+            pred_cov = propagate_covariance(observation_jacobian, state.covariance())
+            cross_covariance = state.covariance()@observation_jacobian.T
+            return Observation(predicted_obs, pred_cov + self.noise_covariance, self.observation_func), cross_covariance
+        
+        pred_mean, pred_cov, cross_cov = propagate_gaussian_cross_cov(
+            state.mean(),
+            state.covariance(),
+            self.observation_func
+        )
+        return Observation(pred_mean, pred_cov + self.noise_covariance, self.observation_func), cross_cov
+    
+    def get_jacobian(self, state: np.ndarray):
+        if self.jacobian_func is None:
+            raise ValueError("Observation Jacobian function not provided")
+        return self.jacobian_func(state)
+    

bayesfilter-0.0.2/bayesfilter/smoothing.py

@@ -0,0 +1,69 @@
+from typing import List, Tuple
+
+import numpy as np
+
+from bayesfilter.distributions import Gaussian
+from bayesfilter.observation import Observation
+from bayesfilter.filtering import BayesianFilter
+
+
+class RTS:
+    """
+    General Rauch-Tung-Striebel smoother. 
+    Equations from BAYESIAN FILTERING AND SMOOTHING by Simo Sarkka, what an absolute banger of a textbook.
+    """
+    def __init__(self, filter: BayesianFilter):
+        self.filter = filter
+        
+    def apply(self, filter_states: List[Gaussian], time_list_s: List[float], use_jacobian=False):
+        smoother_states = []
+        # We start the smoother with the last filter state
+        current_covariance = filter_states[-1].covariance()
+        current_mean = filter_states[-1].mean()
+        current_mean_s = current_mean
+        current_covariance_s = current_covariance
+        smoother_states.append(Gaussian(current_mean_s, current_covariance_s))
+        delta_t_s = time_list_s[-1] - time_list_s[-2]
+
+        # We iterate backwards through the states
+        for i in range(len(filter_states)-2, -1, -1):
+            # Get the data
+            current_state_object = filter_states[i]
+            current_mean = current_state_object.mean()
+            current_covariance = current_state_object.covariance()
+            delta_t_s = time_list_s[i+1] - time_list_s[i]
+
+            # Predict
+            pred_state, cross_covariance = self.filter.transition_model.predict_with_cross_covariance(
+                current_state_object, delta_t_s, use_jacobian=use_jacobian
+            )
+
+            # Calculate the smoother gain
+            G_k = cross_covariance@np.linalg.inv(pred_state.covariance())
+
+            # Calculate the new mean and covariance
+            smoothed_state = current_mean + G_k@(current_mean_s - pred_state.mean())
+            smoothed_covariance = current_covariance + G_k@(current_covariance_s - pred_state.covariance())@G_k.T
+
+            # Now update the current state
+            current_mean_s = smoothed_state
+            current_covariance_s = smoothed_covariance
+
+            # Append the state
+            smoother_states.append(Gaussian(smoothed_state, smoothed_covariance))
+
+        smoother_states.reverse()
+        return smoother_states
+    
+    def smooth(
+        self, 
+        observations: List[Observation], 
+        times_s: List[float], 
+        rate_hz: float = 1.0,
+        use_jacobian = False,
+    ) -> Tuple[List[Gaussian], List[float]]:
+        """
+        Smooths the observations using the general RTS algorithm
+        """
+        filter_states, filter_times = self.filter.run(observations, times_s, rate_hz, use_jacobian=use_jacobian)
+        return self.apply(filter_states, filter_times, use_jacobian=use_jacobian)

bayesfilter-0.0.2/bayesfilter/test_filtering_smoothing.py

@@ -0,0 +1,87 @@
+
+import numpy as np
+
+from bayesfilter.distributions import Gaussian
+from bayesfilter.filtering import BayesianFilter
+from bayesfilter.observation import Observation
+from bayesfilter.model import StateTransitionModel
+from bayesfilter.smoothing import RTS
+    
+
+def setup_functions():
+
+    def transition_func(x, delta_t_s):
+        return np.array([x[0]])
+    
+    def transition_jacobian_func(x, delta_t_s):
+        return np.array([[1.0]])
+
+    def observation_func(x):
+        return np.array([np.sin(x[0]), np.cos(x[0])])
+    
+    def observation_jacobian_func(x):
+        return np.array([[np.cos(x[0])], [-np.sin(x[0])]])
+    
+    return transition_func, transition_jacobian_func, observation_func, observation_jacobian_func
+
+
+def setup_filter_and_observations():
+    # Set the random seed
+    rng = np.random.default_rng(0)
+
+    # Set up the functions
+    transition_func, transition_jacobian_func, observation_func, observation_jacobian_func = setup_functions()
+
+    # Set up the filter
+    transition_model = StateTransitionModel(
+        transition_func, 
+        1e-8*np.eye(1),
+        transition_jacobian_func
+    )
+    initial_state = Gaussian(np.array([0.0]), np.eye(1))
+    filter = BayesianFilter(transition_model, initial_state)
+
+    # Set up the observations
+    true_state = np.array([-0.1])
+    noise_std = 0.2
+    observations = []
+    for theta in np.linspace(0, 2*np.pi, 1000):
+        observation = observation_func(true_state) + rng.normal(0, noise_std, 2)
+        observations.append(Observation(observation, noise_std*np.eye(2), observation_func, observation_jacobian_func))
+    return filter, observations, true_state
+        
+
+def test_filter_noisy_sin(use_jacobian=True):
+    filter, observations, true_state = setup_filter_and_observations()
+    
+    filter.run(observations, np.linspace(0, 2*np.pi, 1000), 100.0, use_jacobian=use_jacobian)
+    np.testing.assert_allclose(filter.state.mean(), true_state, atol=1e-2)
+
+
+def test_smoother_noisy_sin(enable_debug_plots=False, use_jacobian=True):
+    filter, observations, true_state = setup_filter_and_observations()
+    
+    filter_states, filter_times = filter.run(observations, np.linspace(0, 2*np.pi, 1000), 100.0, use_jacobian=use_jacobian)
+    smoother = RTS(filter)
+    smoother_states = smoother.apply(filter_states, np.linspace(0, 2*np.pi, 1000), use_jacobian=use_jacobian)
+    assert len(smoother_states) == len(filter_states)
+    assert len(smoother_states) == len(filter_times)
+
+    if enable_debug_plots:
+        import matplotlib.pyplot as plt
+        plt.plot(filter_times, [state.mean()[0] for state in filter_states], label=f"{'Extended' if use_jacobian else 'Unscented'} Filter")
+        plt.plot(filter_times, [state.mean()[0] for state in smoother_states], label=f"{'Extended' if use_jacobian else 'Unscented'} Smoother")
+        plt.plot(filter_times, [true_state[0] for _ in filter_times], label="True value")
+        plt.legend()
+        plt.show()
+
+    np.testing.assert_allclose(smoother_states[0].mean(), true_state, atol=1e-2)
+    np.testing.assert_allclose(smoother_states[-1].mean(), true_state, atol=1e-2)
+
+
+if __name__ == "__main__":
+    test_filter_noisy_sin(True)
+    test_filter_noisy_sin(False)
+    test_smoother_noisy_sin(False, True)
+    test_smoother_noisy_sin(False, False)
+    print("All tests passed")

bayesfilter-0.0.2/bayesfilter/unscented.py

@@ -0,0 +1,106 @@
+from typing import Callable, List, Tuple
+
+import numpy as np
+
+from bayesfilter.distributions import Distribution, Gaussian, compute_covariance_mean_weight
+
+
+def unscented_transform(
+    distribution: Distribution,
+    non_linear_function: Callable,
+) -> Distribution:
+    """
+    Propagate the distribution through the non-linear function using the unscented transform.
+    """
+    sigma_points = distribution.compute_sigma_points()
+    transformed_sigma_points = np.array([non_linear_function(sp) for sp in sigma_points])
+    weights = distribution.compute_weights()
+    return distribution.from_sigma_points(transformed_sigma_points, weights)
+
+def unscented_transform_cross_cov(
+    distribution: Distribution,
+    non_linear_function: Callable,
+) -> Tuple[Distribution, np.ndarray]:
+    """
+    Propagate the distribution through the non-linear function using the unscented transform.
+    Return the cross-covariance between the mean and the transformed sigma points.
+    """
+    sigma_points = distribution.compute_sigma_points()
+    transformed_sigma_points = np.array([non_linear_function(sp) for sp in sigma_points])
+    weights = distribution.compute_weights()
+    transformed_distribution = distribution.from_sigma_points(transformed_sigma_points, weights)
+    # Compute the cross-covariance
+    cross_covariance = np.zeros((distribution.dimension(), transformed_sigma_points.shape[1]))
+    for i in range(sigma_points.shape[0]):
+        diff_x = sigma_points[i, :] - distribution.mean()
+        diff_y = transformed_sigma_points[i, :] - transformed_distribution.mean()
+        if i == 0:
+            cross_covariance += compute_covariance_mean_weight(distribution.dimension()) * np.outer(diff_x, diff_y)
+        else:
+            cross_covariance += weights[i] * np.outer(diff_x, diff_y)
+    return transformed_distribution, cross_covariance
+
+
+def propagate_samples(
+    distribution: Distribution,
+    non_linear_function: Callable,
+    num_samples: int,
+) -> Distribution:
+    """
+    Sample a distribution and propagate the samples through the non-linear function.
+    Reestimate the distribution from the transformed samples.
+    """
+    samples = [distribution.sample() for _ in range(num_samples)]
+    transformed_samples = [non_linear_function(sample) for sample in samples]
+    return distribution.from_samples(transformed_samples)
+
+
+def propagate_gaussian(
+    mean: np.ndarray,
+    covariance: np.ndarray,
+    non_linear_function: Callable,
+) -> Tuple[np.ndarray, np.ndarray]:
+    """
+    Propagate a Gaussian through a function with the unscented transform.
+    """
+    gaussian = Gaussian(mean, covariance)
+    new_gaussian = unscented_transform(gaussian, non_linear_function)
+    return new_gaussian.mean(), new_gaussian.covariance()
+
+
+def propagate_gaussian_cross_cov(
+    mean: np.ndarray,
+    covariance: np.ndarray,
+    non_linear_function: Callable
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """
+    Propagate a Gaussian through a function with the unscented transform.
+    Return the cross-covariance between the mean and the transformed sigma points.
+    """
+    gaussian = Gaussian(mean, covariance)
+    new_gaussian, cross_covariance = unscented_transform_cross_cov(gaussian, non_linear_function)
+    return new_gaussian.mean(), new_gaussian.covariance(), cross_covariance
+
+
+def test_unscented_transform_linear_func():
+    """
+    Test the unscented transform with a linear function.
+    """
+    mean = np.array([1.0, 2.0])
+    covariance = np.array([[1.0, 0.0], [0.0, 1.0]])
+    gaussian = Gaussian(mean, covariance)
+
+    def linear_function(x):
+        return np.array([2.0*x[0], 3.0*x[1]])
+    
+    jacobian = np.array([[2.0, 0.0], [0.0, 3.0]])
+
+    transformed_gaussian = unscented_transform(gaussian, linear_function)
+    expected_mean = linear_function(mean)
+    expected_covariance = jacobian @ covariance @ jacobian.T
+    assert np.allclose(transformed_gaussian.mean(), expected_mean)
+    assert np.allclose(transformed_gaussian.covariance(), expected_covariance)
+
+
+if __name__ == '__main__':
+    test_unscented_transform_linear_func()

bayesfilter-0.0.2/bayesfilter/utilities.py

@@ -0,0 +1,11 @@
+
+import numpy as np
+
+def propagate_covariance(
+    jacobian: np.ndarray,
+    covariance: np.ndarray
+) -> np.ndarray:
+    """
+    Propagate a covariance through a function
+    """
+    return np.dot(np.dot(jacobian, covariance), jacobian.T)

bayesfilter-0.0.2/bayesfilter.egg-info/PKG-INFO

@@ -0,0 +1,140 @@
+Metadata-Version: 2.1
+Name: bayesfilter
+Version: 0.0.2
+Author: Hugo Hadfield
+License: MIT
+Description-Content-Type: text/markdown
+Requires-Dist: numpy
+
+# BayesFilter
+
+BayesFilter is a Python library for Bayesian filtering and smoothing. This library provides tools for implementing Bayesian filters, Rauch-Tung-Striebel smoothers, and other related methods. The only dependency is NumPy.
+
+## Installation
+
+To install BayesFilter, just use `pip`:
+
+```bash
+pip install bayesfilter
+```
+
+## Usage
+
+### Basic Structure
+
+The library consists of several modules, each responsible for different parts of the Bayesian filtering and smoothing process:
+
+- `distributions.py`: Defines the distribution classes, including the Gaussian distribution used for the filters.
+- `filtering.py`: Implements the BayesianFilter class, which runs the filtering process.
+- `model.py`: Contains the StateTransitionModel class for state transitions.
+- `observation.py`: Defines the Observation class for observation models.
+- `smoothing.py`: Implements the RTS (Rauch-Tung-Striebel) smoother.
+- `unscented.py`: Provides functions for the unscented transform.
+- `utilities.py`: Contains utility functions used throughout the library.
+- `test_filtering_smoothing.py`: Contains tests for filtering and smoothing.
+
+### Example
+
+Here is a basic example of how to set up and run a Bayesian filter with the provided library:
+
+1. **Setup Functions**:
+
+```python
+def setup_functions():
+    def transition_func(x, delta_t_s):
+        return np.array([x[0]])
+    
+    def transition_jacobian_func(x, delta_t_s):
+        return np.array([[1.0]])
+
+    def observation_func(x):
+        return np.array([np.sin(x[0]), np.cos(x[0])])
+    
+    def observation_jacobian_func(x):
+        return np.array([[np.cos(x[0])], [-np.sin(x[0])]])
+    
+    return transition_func, transition_jacobian_func, observation_func, observation_jacobian_func
+```
+
+2. **Setup Filter and Observations**:
+
+```python
+def setup_filter_and_observations():
+    rng = np.random.default_rng(0)
+    transition_func, transition_jacobian_func, observation_func, observation_jacobian_func = setup_functions()
+
+    transition_model = StateTransitionModel(
+        transition_func, 
+        1e-8*np.eye(1),
+        transition_jacobian_func
+    )
+    initial_state = Gaussian(np.array([0.0]), np.eye(1))
+    filter = BayesianFilter(transition_model, initial_state)
+
+    true_state = np.array([-0.1])
+    noise_std = 0.2
+    observations = []
+    for theta in np.linspace(0, 2*np.pi, 1000):
+        observation = observation_func(true_state) + rng.normal(0, noise_std, 2)
+        observations.append(Observation(observation, noise_std*np.eye(2), observation_func, observation_jacobian_func))
+    return filter, observations, true_state
+```
+
+3. **Run Filter**:
+
+```python
+def test_filter_noisy_sin(use_jacobian=True):
+    filter, observations, true_state = setup_filter_and_observations()
+    filter.run(observations, np.linspace(0, 2*np.pi, 1000), 100.0, use_jacobian=use_jacobian)
+    np.testing.assert_allclose(filter.state.mean(), true_state, atol=1e-2)
+```
+
+### Tests
+
+The library includes a set of tests to ensure the functionality of the filtering and smoothing algorithms. These can be run as follows:
+
+```bash
+python test_filtering_smoothing.py
+```
+
+## Documentation
+
+### `distributions.py`
+
+Defines the Gaussian distribution class used for state representation and propagation.
+
+### `filtering.py`
+
+Implements the `BayesianFilter` class, responsible for running the filtering process with predict and update steps.
+
+### `model.py`
+
+Contains the `StateTransitionModel` class, representing the state transition model.
+
+### `observation.py`
+
+Defines the `Observation` class, representing the observation model.
+
+### `smoothing.py`
+
+Implements the `RTS` class for Rauch-Tung-Striebel smoothing.
+
+### `unscented.py`
+
+Provides functions for the unscented transform, including `unscented_transform` and `propagate_gaussian`.
+
+### `utilities.py`
+
+Contains utility functions like `propagate_covariance`.
+
+### `test_filtering_smoothing.py`
+
+Includes tests for filtering and smoothing to validate the implementation.
+
+## Author
+
+Hugo Hadfield
+
+## License
+
+This project is licensed under the MIT License.

bayesfilter-0.0.2/bayesfilter.egg-info/SOURCES.txt

@@ -0,0 +1,16 @@
+README.md
+setup.py
+bayesfilter/__init__.py
+bayesfilter/distributions.py
+bayesfilter/filtering.py
+bayesfilter/model.py
+bayesfilter/observation.py
+bayesfilter/smoothing.py
+bayesfilter/test_filtering_smoothing.py
+bayesfilter/unscented.py
+bayesfilter/utilities.py
+bayesfilter.egg-info/PKG-INFO
+bayesfilter.egg-info/SOURCES.txt
+bayesfilter.egg-info/dependency_links.txt
+bayesfilter.egg-info/requires.txt
+bayesfilter.egg-info/top_level.txt

bayesfilter-0.0.2/bayesfilter.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

bayesfilter-0.0.2/bayesfilter.egg-info/requires.txt

@@ -0,0 +1 @@
+numpy

bayesfilter-0.0.2/bayesfilter.egg-info/top_level.txt

@@ -0,0 +1 @@
+bayesfilter

bayesfilter-0.0.2/setup.cfg

@@ -0,0 +1,4 @@
+[egg_info]
+tag_build = 
+tag_date = 0
+

bayesfilter-0.0.2/setup.py

@@ -0,0 +1,15 @@
+
+from setuptools import setup
+
+setup(
+    name='bayesfilter',
+    version='0.0.2',
+    packages=['bayesfilter'],
+    install_requires=[
+        'numpy',
+    ],
+    license='MIT',
+    author='Hugo Hadfield',
+    long_description=open('README.md').read(),
+    long_description_content_type='text/markdown',
+)