├── .github └── workflows │ └── python-publish.yml ├── .gitignore ├── README.md ├── bayesfilter ├── __init__.py ├── distributions.py ├── filtering.py ├── model.py ├── observation.py ├── smoothing.py ├── test_filtering_smoothing.py ├── unscented.py ├── utilities.py └── version.py ├── licence.txt └── setup.py /.github/workflows/python-publish.yml: -------------------------------------------------------------------------------- 1 | # This workflow will upload a Python Package using Twine when a release is created 2 | # For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python#publishing-to-package-registries 3 | 4 | # This workflow uses actions that are not certified by GitHub. 5 | # They are provided by a third-party and are governed by 6 | # separate terms of service, privacy policy, and support 7 | # documentation. 8 | 9 | name: Upload Python Package 10 | 11 | on: 12 | release: 13 | types: [published] 14 | 15 | permissions: 16 | contents: read 17 | id-token: write # IMPORTANT: this permission is mandatory for trusted publishing 18 | 19 | jobs: 20 | deploy: 21 | 22 | runs-on: ubuntu-latest 23 | environment: 24 | name: pypi 25 | url: https://pypi.org/p/bayesfilter 26 | permissions: 27 | id-token: write # IMPORTANT: this permission is mandatory for trusted publishing 28 | steps: 29 | - uses: actions/checkout@v4 30 | - name: Set up Python 31 | uses: actions/setup-python@v3 32 | with: 33 | python-version: '3.x' 34 | - name: Install dependencies 35 | run: | 36 | python -m pip install --upgrade pip 37 | pip install build 38 | - name: Build package 39 | run: python -m build 40 | - name: Publish package 41 | uses: pypa/gh-action-pypi-publish@release/v1 42 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/latest/usage/project/#working-with-version-control 110 | .pdm.toml 111 | .pdm-python 112 | .pdm-build/ 113 | 114 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 115 | __pypackages__/ 116 | 117 | # Celery stuff 118 | celerybeat-schedule 119 | celerybeat.pid 120 | 121 | # SageMath parsed files 122 | *.sage.py 123 | 124 | # Environments 125 | .env 126 | .venv 127 | env/ 128 | venv/ 129 | ENV/ 130 | env.bak/ 131 | venv.bak/ 132 | 133 | # Spyder project settings 134 | .spyderproject 135 | .spyproject 136 | 137 | # Rope project settings 138 | .ropeproject 139 | 140 | # mkdocs documentation 141 | /site 142 | 143 | # mypy 144 | .mypy_cache/ 145 | .dmypy.json 146 | dmypy.json 147 | 148 | # Pyre type checker 149 | .pyre/ 150 | 151 | # pytype static type analyzer 152 | .pytype/ 153 | 154 | # Cython debug symbols 155 | cython_debug/ 156 | 157 | # PyCharm 158 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 159 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 160 | # and can be added to the global gitignore or merged into this file. For a more nuclear 161 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 162 | #.idea/ 163 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BayesFilter 2 | 3 | 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. 4 | 5 | ## Installation 6 | 7 | To install BayesFilter, just use `pip`: 8 | 9 | ```bash 10 | pip install bayesfilter 11 | ``` 12 | 13 | ## Usage 14 | 15 | ### Basic Structure 16 | 17 | The library consists of several modules, each responsible for different parts of the Bayesian filtering and smoothing process: 18 | 19 | - `distributions.py`: Defines the distribution classes, including the Gaussian distribution used for the filters. 20 | - `filtering.py`: Implements the BayesianFilter class, which runs the filtering process. 21 | - `model.py`: Contains the StateTransitionModel class for state transitions. 22 | - `observation.py`: Defines the Observation class for observation models. 23 | - `smoothing.py`: Implements the RTS (Rauch-Tung-Striebel) smoother. 24 | - `unscented.py`: Provides functions for the unscented transform. 25 | - `utilities.py`: Contains utility functions used throughout the library. 26 | - `test_filtering_smoothing.py`: Contains tests for filtering and smoothing. 27 | 28 | ### Example 29 | 30 | Here is a basic example of how to set up and run a Bayesian filter with the provided library: 31 | 32 | 1. **Setup Functions**: 33 | 34 | ```python 35 | def setup_functions(): 36 | def transition_func(x, delta_t_s): 37 | return np.array([x[0]]) 38 | 39 | def transition_jacobian_func(x, delta_t_s): 40 | return np.array([[1.0]]) 41 | 42 | def observation_func(x): 43 | return np.array([np.sin(x[0]), np.cos(x[0])]) 44 | 45 | def observation_jacobian_func(x): 46 | return np.array([[np.cos(x[0])], [-np.sin(x[0])]]) 47 | 48 | return transition_func, transition_jacobian_func, observation_func, observation_jacobian_func 49 | ``` 50 | 51 | 2. **Setup Filter and Observations**: 52 | 53 | ```python 54 | def setup_filter_and_observations(): 55 | rng = np.random.default_rng(0) 56 | transition_func, transition_jacobian_func, observation_func, observation_jacobian_func = setup_functions() 57 | 58 | transition_model = StateTransitionModel( 59 | transition_func, 60 | 1e-8*np.eye(1), 61 | transition_jacobian_func 62 | ) 63 | initial_state = Gaussian(np.array([0.0]), np.eye(1)) 64 | filter = BayesianFilter(transition_model, initial_state) 65 | 66 | true_state = np.array([-0.1]) 67 | noise_std = 0.2 68 | observations = [] 69 | for theta in np.linspace(0, 2*np.pi, 1000): 70 | observation = observation_func(true_state) + rng.normal(0, noise_std, 2) 71 | observations.append(Observation(observation, noise_std*np.eye(2), observation_func, observation_jacobian_func)) 72 | return filter, observations, true_state 73 | ``` 74 | 75 | 3. **Run Filter**: 76 | 77 | ```python 78 | def test_filter_noisy_sin(use_jacobian=True): 79 | filter, observations, true_state = setup_filter_and_observations() 80 | filter.run(observations, np.linspace(0, 2*np.pi, 1000), 100.0, use_jacobian=use_jacobian) 81 | np.testing.assert_allclose(filter.state.mean(), true_state, atol=1e-2) 82 | ``` 83 | 84 | ### Tests 85 | 86 | The library includes a set of tests to ensure the functionality of the filtering and smoothing algorithms. These can be run as follows: 87 | 88 | ```bash 89 | python test_filtering_smoothing.py 90 | ``` 91 | 92 | ## Documentation 93 | 94 | ### `distributions.py` 95 | 96 | Defines the Gaussian distribution class used for state representation and propagation. 97 | 98 | ### `filtering.py` 99 | 100 | Implements the `BayesianFilter` class, responsible for running the filtering process with predict and update steps. 101 | 102 | ### `model.py` 103 | 104 | Contains the `StateTransitionModel` class, representing the state transition model. 105 | 106 | ### `observation.py` 107 | 108 | Defines the `Observation` class, representing the observation model. 109 | 110 | ### `smoothing.py` 111 | 112 | Implements the `RTS` class for Rauch-Tung-Striebel smoothing. 113 | 114 | ### `unscented.py` 115 | 116 | Provides functions for the unscented transform, including `unscented_transform` and `propagate_gaussian`. 117 | 118 | ### `utilities.py` 119 | 120 | Contains utility functions like `propagate_covariance`. 121 | 122 | ### `test_filtering_smoothing.py` 123 | 124 | Includes tests for filtering and smoothing to validate the implementation. 125 | 126 | ## Author 127 | 128 | Hugo Hadfield 129 | 130 | ## License 131 | 132 | This project is licensed under the MIT License. -------------------------------------------------------------------------------- /bayesfilter/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | bayesfilter. 3 | 4 | A python library for bayesian filtering and smoothing. 5 | """ 6 | 7 | from .version import __version__ 8 | __author__ = "Hugo Hadfield" -------------------------------------------------------------------------------- /bayesfilter/distributions.py: -------------------------------------------------------------------------------- 1 | 2 | from typing import List 3 | 4 | import numpy as np 5 | 6 | DEFAULT_ALPHA = 1e-3 7 | DEFAULT_BETA = 2.0 8 | 9 | 10 | def compute_lambda(n: int, alpha: float = DEFAULT_ALPHA) -> int: 11 | """ 12 | Compute the lambda parameter for the unscented transform. 13 | """ 14 | kappa = 3 - n 15 | l = alpha*alpha*(n + kappa) - n 16 | return l 17 | 18 | 19 | def compute_covariance_mean_weight( 20 | dimension: int, 21 | alpha: float = DEFAULT_ALPHA, 22 | beta: float = DEFAULT_BETA 23 | ) -> np.ndarray: 24 | """ 25 | Compute the covariance weight for the sigma points representing the mean. 26 | """ 27 | lamb = compute_lambda(dimension) 28 | return lamb / (dimension + lamb) + (1 - alpha*alpha + beta) 29 | 30 | 31 | class Distribution: 32 | """ 33 | Represents a generic probability distribution. 34 | """ 35 | def dimension(self): 36 | raise NotImplementedError 37 | 38 | def mean(self): 39 | raise NotImplementedError 40 | 41 | def covariance(self): 42 | raise NotImplementedError 43 | 44 | def compute_sigma_points(self): 45 | raise NotImplementedError 46 | 47 | def sample(self): 48 | raise NotImplementedError 49 | 50 | def from_samples(self, samples: List[np.ndarray]): 51 | raise NotImplementedError 52 | 53 | def __repr__(self): 54 | raise NotImplementedError 55 | 56 | 57 | class Gaussian(Distribution): 58 | """ 59 | Represents a Gaussian distribution. 60 | """ 61 | def __init__(self, mean, covariance, rng=None): 62 | self._mean = mean 63 | self._covariance = covariance 64 | self._rng = np.random.default_rng(rng) 65 | 66 | @property 67 | def rng(self): 68 | return self._rng 69 | 70 | def mean(self) -> np.ndarray: 71 | """ 72 | Return the mean of the Gaussian distribution. 73 | """ 74 | return self._mean 75 | 76 | def covariance(self) -> np.ndarray: 77 | """ 78 | Return the covariance of the Gaussian distribution. 79 | """ 80 | return self._covariance 81 | 82 | def dimension(self) -> int: 83 | """ 84 | Return the dimension of the Gaussian distribution. 85 | """ 86 | return len(self.mean()) 87 | 88 | def sqrt_covariance(self) -> np.ndarray: 89 | """ 90 | Return the square root of the covariance matrix. 91 | """ 92 | if self.dimension() == 1: 93 | return np.array([[np.sqrt(self.covariance()[0, 0])]]) 94 | return np.linalg.cholesky(self.covariance()) 95 | 96 | def compute_sigma_points(self) -> np.ndarray: 97 | """ 98 | Compute the sigma points for the Gaussian distribution. 99 | """ 100 | mean = self.mean() 101 | sqrt_covariance = self.sqrt_covariance() 102 | n = self.dimension() 103 | lamb = compute_lambda(n) 104 | factor = np.sqrt(n + lamb) 105 | sigma_points = np.zeros((2*n + 1, n)) 106 | sigma_points[0, :] = mean 107 | counter = 1 108 | for i in range(n): # 2n + 1 sigma points 109 | sigma_points[counter, :] = mean + factor*sqrt_covariance[:, i] 110 | counter += 1 111 | sigma_points[counter, :] = mean - factor*sqrt_covariance[:, i] 112 | counter += 1 113 | return sigma_points 114 | 115 | def compute_weights(self) -> np.ndarray: 116 | """ 117 | Compute the weights for the sigma points. 118 | """ 119 | n = self.dimension() 120 | lamb = compute_lambda(n) 121 | weights = np.zeros(2*n + 1) 122 | # The first weight is for the mean 123 | weights[0] = lamb / (n + lamb) 124 | for i in range(2*n): 125 | weights[i + 1] = 1.0 / (2.0*(n + lamb)) 126 | return weights 127 | 128 | def from_sigma_points(self, sigma_points: np.ndarray, weights: np.ndarray): 129 | """ 130 | Create a Gaussian distribution from the given sigma points. 131 | Equations coming from https://arxiv.org/pdf/2104.01958 132 | """ 133 | new_n = sigma_points.shape[1] 134 | new_mean = np.zeros(new_n) 135 | for i in range(sigma_points.shape[0]): 136 | new_mean += weights[i] * sigma_points[i, :] 137 | new_covariance = np.zeros((new_n, new_n)) 138 | for i in range(sigma_points.shape[0]): 139 | diff = sigma_points[i, :] - new_mean 140 | if i == 0: 141 | new_covariance += compute_covariance_mean_weight(self.dimension()) * np.outer(diff, diff) 142 | else: 143 | new_covariance += weights[i] * np.outer(diff, diff) 144 | return Gaussian(new_mean, new_covariance, self.rng) 145 | 146 | def sample(self) -> np.ndarray: 147 | """ 148 | Sample from the Gaussian distribution. 149 | """ 150 | return self.rng.multivariate_normal(self.mean(), self.covariance()) 151 | 152 | def from_samples(self, samples: List[np.ndarray]): 153 | """ 154 | Create a Gaussian distribution from the given samples. 155 | """ 156 | sample_array = np.array(samples, dtype=np.float64) 157 | new_mean = np.mean(sample_array, axis=0) 158 | new_covariance = np.cov(sample_array, rowvar=False) 159 | return Gaussian(new_mean, new_covariance, self.rng) 160 | 161 | def __repr__(self): 162 | return f'Gaussian(mean={self.mean()}, covariance={self.covariance()})' 163 | -------------------------------------------------------------------------------- /bayesfilter/filtering.py: -------------------------------------------------------------------------------- 1 | from typing import List, Tuple 2 | 3 | import numpy as np 4 | 5 | try: 6 | import tqdm 7 | except ImportError: 8 | tqdm = None 9 | 10 | from bayesfilter.distributions import Gaussian 11 | from bayesfilter.model import StateTransitionModel 12 | from bayesfilter.observation import Observation 13 | 14 | 15 | class BayesianFilter: 16 | def __init__(self, state_transition_model: StateTransitionModel, initial_state: Gaussian): 17 | """ 18 | Set up the bayesian filter with the state transition model and initial state 19 | """ 20 | self.transition_model = state_transition_model 21 | self.state = initial_state 22 | 23 | def predict(self, state: Gaussian, delta_t_s: float, use_jacobian: bool = True) -> Gaussian: 24 | """ 25 | Predict the next state from a given state 26 | """ 27 | # Predict the next state 28 | return self.transition_model.predict(state, delta_t_s, use_jacobian=use_jacobian) 29 | 30 | def update(self, observation: Observation, predicted_state: Gaussian, use_jacobian: bool = False) -> Gaussian: 31 | """ 32 | Condition the state on a observation 33 | """ 34 | # Predict the observation (with noise) 35 | predicted_obsurement, cross_covariance = observation.predict_with_cross_covariance(predicted_state, use_jacobian=use_jacobian) 36 | 37 | # Compute the Kalman gain 38 | kalman_gain = cross_covariance@np.linalg.inv(predicted_obsurement.noise_covariance) 39 | 40 | # Compute the residual 41 | residual = observation.observation - observation.observation_func(predicted_state.mean()) 42 | 43 | # Compute the new state 44 | new_state = Gaussian( 45 | mean=predicted_state.mean() + (kalman_gain@residual).flatten(), 46 | covariance=predicted_state.covariance() - kalman_gain@predicted_obsurement.noise_covariance@kalman_gain.T 47 | ) 48 | 49 | # Update the state 50 | self.state = new_state 51 | return new_state 52 | 53 | def set_state(self, state: Gaussian): 54 | """ 55 | Directly set the filter state 56 | """ 57 | self.state = state 58 | 59 | def run_synchronous(self, observations: List[Observation], times_s: List[float], use_jacobian=False) -> List[Gaussian]: 60 | """ 61 | Run the BayesianFilter on a list of observations. 62 | Assumes that the observations are at a roughly fixed time interval. 63 | use_jacobian allows you to switch between an extended and unscented prediction and update 64 | """ 65 | output_states = [self.state] 66 | for i, observation in enumerate(observations[:len(times_s)-1]): 67 | delta_t_s = times_s[i+1] - times_s[i] 68 | predicted_state = self.predict(self.state, delta_t_s, use_jacobian=use_jacobian) 69 | self.update(observation, predicted_state, use_jacobian=use_jacobian) 70 | output_states.append(self.state) 71 | return output_states 72 | 73 | def run( 74 | self, 75 | observations: List[Observation], 76 | times_s: List[float], 77 | rate_hz: float, 78 | use_jacobian=False 79 | ) -> Tuple[List[Gaussian], List[float]]: 80 | """ 81 | Run the bayesian filter on a list of observations at a fixed rate, start at the start of times_s and end at the 82 | end of times_s. times_s referres to the observation times. use_jacobian allows you to switch 83 | between an extended linearisation and an unscented transform for the prediction 84 | """ 85 | filter_states = [] 86 | filter_times = [] 87 | start_time = times_s[0] 88 | end_time = times_s[-1] 89 | current_obs_index = 0 90 | # Step through the times from start to finish 91 | # Use tqdm to keep track of progress if available 92 | print(f"Running BayesianFilter at {rate_hz} Hz with use_jacobian: {use_jacobian}", flush=True) 93 | delta_t_s = 1.0/rate_hz 94 | iterator = np.arange(start_time, end_time + delta_t_s, delta_t_s) 95 | if tqdm is not None: 96 | iterator = tqdm.tqdm(iterator) 97 | for current_time in iterator: 98 | # Gets a list of all observations that have occured between previous time and curent_time 99 | current_obs = [] 100 | if current_obs_index < len(times_s): 101 | next_obs_time = times_s[current_obs_index] 102 | while next_obs_time <= current_time: 103 | current_obs.append(observations[current_obs_index]) 104 | current_obs_index += 1 105 | if current_obs_index >= len(times_s): 106 | break 107 | next_obs_time = times_s[current_obs_index] 108 | 109 | # Predict the next state 110 | predicted_state = self.predict(self.state, delta_t_s, use_jacobian=use_jacobian) 111 | 112 | # Update the state with the observations, if we have any 113 | for observation in current_obs: 114 | predicted_state = self.update(observation, predicted_state, use_jacobian=use_jacobian) 115 | 116 | # Set the state 117 | self.set_state(predicted_state) 118 | 119 | # Append the state 120 | filter_states.append(self.state) 121 | filter_times.append(current_time) 122 | 123 | return filter_states, filter_times 124 | -------------------------------------------------------------------------------- /bayesfilter/model.py: -------------------------------------------------------------------------------- 1 | 2 | from typing import Callable, Optional 3 | 4 | import numpy as np 5 | 6 | from bayesfilter.distributions import Gaussian 7 | from bayesfilter.utilities import propagate_covariance 8 | from bayesfilter.unscented import ( 9 | propagate_gaussian, propagate_gaussian_cross_cov 10 | ) 11 | 12 | 13 | class StateTransitionModel: 14 | """ 15 | Represents a state transition model 16 | """ 17 | def __init__( 18 | self, 19 | transition_func: Callable[[np.ndarray, float], np.ndarray], 20 | transition_noise_covariance: np.ndarray, 21 | transition_jacobian_func: Optional[Callable[[np.ndarray, float], np.ndarray]] = None 22 | ): 23 | self.transition_func = transition_func 24 | self.dimension = len(transition_noise_covariance) 25 | self.transition_noise_covariance = transition_noise_covariance 26 | self.transition_jacobian_func = transition_jacobian_func 27 | 28 | def predict_no_noise_jacobian(self, state: Gaussian, delta_t_s: float): 29 | """ 30 | Predict the next state without adding transition noise, uses the Jacobian of the 31 | state transition function 32 | """ 33 | new_state = Gaussian( 34 | mean=self.transition_func(state.mean(), delta_t_s), 35 | covariance=propagate_covariance( 36 | jacobian=self.get_jacobian(state, delta_t_s), 37 | covariance=state.covariance() 38 | ) 39 | ) 40 | return new_state 41 | 42 | def predict_no_noise(self, state: Gaussian, delta_t_s: float, use_jacobian: bool = False): 43 | """ 44 | Predict the next state without adding transition noise, uses the unscented transform 45 | """ 46 | if use_jacobian: 47 | return self.predict_no_noise_jacobian(state, delta_t_s) 48 | # Unscented transform 49 | new_mean, new_covariance = propagate_gaussian( 50 | state.mean(), 51 | state.covariance(), 52 | lambda x: self.transition_func(x, delta_t_s) 53 | ) 54 | return Gaussian(new_mean, new_covariance) 55 | 56 | def predict(self, state: Gaussian, delta_t_s: float, use_jacobian: bool = False): 57 | """ 58 | Predict the next state 59 | """ 60 | new_state = self.predict_no_noise(state, delta_t_s, use_jacobian) 61 | return Gaussian(new_state.mean(), new_state.covariance() + self.transition_noise_covariance) 62 | 63 | def predict_with_cross_covariance(self, state: Gaussian, delta_t_s: float, use_jacobian=False): 64 | """ 65 | Predict the next state and return the cross-covariance too 66 | """ 67 | if use_jacobian: 68 | pred_mean = self.transition_func(state.mean(), delta_t_s) 69 | transition_func_jac = self.get_jacobian(state, delta_t_s) 70 | pred_cov = propagate_covariance(transition_func_jac, state.covariance()) 71 | cross_cov = pred_cov@transition_func_jac.T 72 | return Gaussian(pred_mean, pred_cov + self.transition_noise_covariance), cross_cov 73 | pred_mean, pred_cov, cross_cov = propagate_gaussian_cross_cov( 74 | state.mean(), 75 | state.covariance(), 76 | lambda x: self.transition_func(x, delta_t_s) 77 | ) 78 | return Gaussian(pred_mean, pred_cov + self.transition_noise_covariance), cross_cov 79 | 80 | def get_jacobian(self, state: Gaussian, delta_t_s: float): 81 | if self.transition_jacobian_func is None: 82 | raise ValueError("Transition Jacobian function not provided") 83 | return self.transition_jacobian_func(state.mean(), delta_t_s) 84 | -------------------------------------------------------------------------------- /bayesfilter/observation.py: -------------------------------------------------------------------------------- 1 | 2 | from typing import Callable, Optional 3 | 4 | import numpy as np 5 | 6 | from bayesfilter.distributions import Gaussian 7 | from bayesfilter.utilities import propagate_covariance 8 | from bayesfilter.unscented import ( 9 | propagate_gaussian, propagate_gaussian_cross_cov 10 | ) 11 | 12 | class Observation: 13 | """ 14 | Represents an observation model 15 | """ 16 | def __init__( 17 | self, 18 | observation: np.ndarray, 19 | noise_covariance: np.ndarray, 20 | observation_func: Callable[[np.ndarray], np.ndarray], 21 | jacobian_func: Optional[Callable[[np.ndarray], np.ndarray]] = None 22 | ): 23 | self.observation = observation.copy() 24 | self.noise_covariance = noise_covariance.copy() 25 | self.observation_func = observation_func 26 | self.dimension = len(noise_covariance) 27 | self.jacobian_func = jacobian_func 28 | 29 | def predict_no_noise_jacobian(self, state: Gaussian): 30 | """ 31 | Predict an observation from a state without adding observation noise 32 | """ 33 | # Predict observation 34 | pred_obs = self.observation_func(state.mean()) 35 | # Predict covariance 36 | jac = self.get_jacobian(state.mean()) 37 | pred_cov = propagate_covariance( 38 | jacobian=jac, 39 | covariance=state.covariance() 40 | ) 41 | return Observation(pred_obs, pred_cov, self.observation_func) 42 | 43 | def predict_no_noise(self, state: Gaussian, use_jacobian: bool = False): 44 | """ 45 | Predict an observation from a state without adding observation noise 46 | """ 47 | if use_jacobian: 48 | return self.predict_no_noise_jacobian(state) 49 | # Unscented transform 50 | pred_mean, pred_cov = propagate_gaussian( 51 | state.mean(), 52 | state.covariance(), 53 | self.observation_func 54 | ) 55 | return Observation(pred_mean, pred_cov, self.observation_func) 56 | 57 | def predict(self, state: Gaussian, use_jacobian: bool = False): 58 | """ 59 | Predict an observation from a state 60 | """ 61 | observation = self.predict_no_noise(state, use_jacobian) 62 | observation.noise_covariance += self.noise_covariance 63 | return observation 64 | 65 | def predict_with_cross_covariance(self, state: Gaussian, use_jacobian: bool = False): 66 | """ 67 | Predict an observation from a state and also return the cross-covariance 68 | """ 69 | if use_jacobian: 70 | predicted_obs = self.observation_func(state.mean()) 71 | observation_jacobian = self.get_jacobian(state.mean()) 72 | pred_cov = propagate_covariance(observation_jacobian, state.covariance()) 73 | cross_covariance = state.covariance()@observation_jacobian.T 74 | return Observation(predicted_obs, pred_cov + self.noise_covariance, self.observation_func), cross_covariance 75 | 76 | pred_mean, pred_cov, cross_cov = propagate_gaussian_cross_cov( 77 | state.mean(), 78 | state.covariance(), 79 | self.observation_func 80 | ) 81 | return Observation(pred_mean, pred_cov + self.noise_covariance, self.observation_func), cross_cov 82 | 83 | def get_jacobian(self, state: np.ndarray): 84 | if self.jacobian_func is None: 85 | raise ValueError("Observation Jacobian function not provided") 86 | return self.jacobian_func(state) 87 | 88 | -------------------------------------------------------------------------------- /bayesfilter/smoothing.py: -------------------------------------------------------------------------------- 1 | from typing import List, Tuple 2 | 3 | import numpy as np 4 | 5 | from bayesfilter.distributions import Gaussian 6 | from bayesfilter.observation import Observation 7 | from bayesfilter.filtering import BayesianFilter 8 | 9 | 10 | class RTS: 11 | """ 12 | General Rauch-Tung-Striebel smoother. 13 | Equations from BAYESIAN FILTERING AND SMOOTHING by Simo Sarkka, what an absolute banger of a textbook. 14 | """ 15 | def __init__(self, filter: BayesianFilter): 16 | self.filter = filter 17 | 18 | def apply(self, filter_states: List[Gaussian], time_list_s: List[float], use_jacobian=False): 19 | smoother_states = [] 20 | # We start the smoother with the last filter state 21 | current_covariance = filter_states[-1].covariance() 22 | current_mean = filter_states[-1].mean() 23 | current_mean_s = current_mean 24 | current_covariance_s = current_covariance 25 | smoother_states.append(Gaussian(current_mean_s, current_covariance_s)) 26 | delta_t_s = time_list_s[-1] - time_list_s[-2] 27 | 28 | # We iterate backwards through the states 29 | for i in range(len(filter_states)-2, -1, -1): 30 | # Get the data 31 | current_state_object = filter_states[i] 32 | current_mean = current_state_object.mean() 33 | current_covariance = current_state_object.covariance() 34 | delta_t_s = time_list_s[i+1] - time_list_s[i] 35 | 36 | # Predict 37 | pred_state, cross_covariance = self.filter.transition_model.predict_with_cross_covariance( 38 | current_state_object, delta_t_s, use_jacobian=use_jacobian 39 | ) 40 | 41 | # Calculate the smoother gain 42 | G_k = cross_covariance@np.linalg.inv(pred_state.covariance()) 43 | 44 | # Calculate the new mean and covariance 45 | smoothed_state = current_mean + G_k@(current_mean_s - pred_state.mean()) 46 | smoothed_covariance = current_covariance + G_k@(current_covariance_s - pred_state.covariance())@G_k.T 47 | 48 | # Now update the current state 49 | current_mean_s = smoothed_state 50 | current_covariance_s = smoothed_covariance 51 | 52 | # Append the state 53 | smoother_states.append(Gaussian(smoothed_state, smoothed_covariance)) 54 | 55 | smoother_states.reverse() 56 | return smoother_states 57 | 58 | def smooth( 59 | self, 60 | observations: List[Observation], 61 | times_s: List[float], 62 | rate_hz: float = 1.0, 63 | use_jacobian = False, 64 | ) -> Tuple[List[Gaussian], List[float]]: 65 | """ 66 | Smooths the observations using the general RTS algorithm 67 | """ 68 | filter_states, filter_times = self.filter.run(observations, times_s, rate_hz, use_jacobian=use_jacobian) 69 | return self.apply(filter_states, filter_times, use_jacobian=use_jacobian) 70 | -------------------------------------------------------------------------------- /bayesfilter/test_filtering_smoothing.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | from bayesfilter.distributions import Gaussian 5 | from bayesfilter.filtering import BayesianFilter 6 | from bayesfilter.observation import Observation 7 | from bayesfilter.model import StateTransitionModel 8 | from bayesfilter.smoothing import RTS 9 | 10 | 11 | def setup_functions(): 12 | 13 | def transition_func(x, delta_t_s): 14 | return np.array([x[0]]) 15 | 16 | def transition_jacobian_func(x, delta_t_s): 17 | return np.array([[1.0]]) 18 | 19 | def observation_func(x): 20 | return np.array([np.sin(x[0]), np.cos(x[0])]) 21 | 22 | def observation_jacobian_func(x): 23 | return np.array([[np.cos(x[0])], [-np.sin(x[0])]]) 24 | 25 | return transition_func, transition_jacobian_func, observation_func, observation_jacobian_func 26 | 27 | 28 | def setup_filter_and_observations(): 29 | # Set the random seed 30 | rng = np.random.default_rng(0) 31 | 32 | # Set up the functions 33 | transition_func, transition_jacobian_func, observation_func, observation_jacobian_func = setup_functions() 34 | 35 | # Set up the filter 36 | transition_model = StateTransitionModel( 37 | transition_func, 38 | 1e-8*np.eye(1), 39 | transition_jacobian_func 40 | ) 41 | initial_state = Gaussian(np.array([0.0]), np.eye(1)) 42 | filter = BayesianFilter(transition_model, initial_state) 43 | 44 | # Set up the observations 45 | true_state = np.array([-0.1]) 46 | noise_std = 0.2 47 | observations = [] 48 | for theta in np.linspace(0, 2*np.pi, 1000): 49 | observation = observation_func(true_state) + rng.normal(0, noise_std, 2) 50 | observations.append(Observation(observation, noise_std*np.eye(2), observation_func, observation_jacobian_func)) 51 | return filter, observations, true_state 52 | 53 | 54 | def test_filter_noisy_sin(use_jacobian=True): 55 | filter, observations, true_state = setup_filter_and_observations() 56 | 57 | filter.run(observations, np.linspace(0, 2*np.pi, 1000), 100.0, use_jacobian=use_jacobian) 58 | np.testing.assert_allclose(filter.state.mean(), true_state, atol=1e-2) 59 | 60 | 61 | def test_smoother_noisy_sin(enable_debug_plots=False, use_jacobian=True): 62 | filter, observations, true_state = setup_filter_and_observations() 63 | 64 | filter_states, filter_times = filter.run(observations, np.linspace(0, 2*np.pi, 1000), 100.0, use_jacobian=use_jacobian) 65 | smoother = RTS(filter) 66 | smoother_states = smoother.apply(filter_states, np.linspace(0, 2*np.pi, 1000), use_jacobian=use_jacobian) 67 | assert len(smoother_states) == len(filter_states) 68 | assert len(smoother_states) == len(filter_times) 69 | 70 | if enable_debug_plots: 71 | import matplotlib.pyplot as plt 72 | plt.plot(filter_times, [state.mean()[0] for state in filter_states], label=f"{'Extended' if use_jacobian else 'Unscented'} Filter") 73 | plt.plot(filter_times, [state.mean()[0] for state in smoother_states], label=f"{'Extended' if use_jacobian else 'Unscented'} Smoother") 74 | plt.plot(filter_times, [true_state[0] for _ in filter_times], label="True value") 75 | plt.legend() 76 | plt.show() 77 | 78 | np.testing.assert_allclose(smoother_states[0].mean(), true_state, atol=1e-2) 79 | np.testing.assert_allclose(smoother_states[-1].mean(), true_state, atol=1e-2) 80 | 81 | 82 | if __name__ == "__main__": 83 | test_filter_noisy_sin(True) 84 | test_filter_noisy_sin(False) 85 | test_smoother_noisy_sin(False, True) 86 | test_smoother_noisy_sin(False, False) 87 | print("All tests passed") 88 | -------------------------------------------------------------------------------- /bayesfilter/unscented.py: -------------------------------------------------------------------------------- 1 | from typing import Callable, List, Tuple 2 | 3 | import numpy as np 4 | 5 | from bayesfilter.distributions import Distribution, Gaussian, compute_covariance_mean_weight 6 | 7 | 8 | def unscented_transform( 9 | distribution: Distribution, 10 | non_linear_function: Callable, 11 | ) -> Distribution: 12 | """ 13 | Propagate the distribution through the non-linear function using the unscented transform. 14 | """ 15 | sigma_points = distribution.compute_sigma_points() 16 | transformed_sigma_points = np.array([non_linear_function(sp) for sp in sigma_points]) 17 | weights = distribution.compute_weights() 18 | return distribution.from_sigma_points(transformed_sigma_points, weights) 19 | 20 | def unscented_transform_cross_cov( 21 | distribution: Distribution, 22 | non_linear_function: Callable, 23 | ) -> Tuple[Distribution, np.ndarray]: 24 | """ 25 | Propagate the distribution through the non-linear function using the unscented transform. 26 | Return the cross-covariance between the mean and the transformed sigma points. 27 | """ 28 | sigma_points = distribution.compute_sigma_points() 29 | transformed_sigma_points = np.array([non_linear_function(sp) for sp in sigma_points]) 30 | weights = distribution.compute_weights() 31 | transformed_distribution = distribution.from_sigma_points(transformed_sigma_points, weights) 32 | # Compute the cross-covariance 33 | cross_covariance = np.zeros((distribution.dimension(), transformed_sigma_points.shape[1])) 34 | for i in range(sigma_points.shape[0]): 35 | diff_x = sigma_points[i, :] - distribution.mean() 36 | diff_y = transformed_sigma_points[i, :] - transformed_distribution.mean() 37 | if i == 0: 38 | cross_covariance += compute_covariance_mean_weight(distribution.dimension()) * np.outer(diff_x, diff_y) 39 | else: 40 | cross_covariance += weights[i] * np.outer(diff_x, diff_y) 41 | return transformed_distribution, cross_covariance 42 | 43 | 44 | def propagate_samples( 45 | distribution: Distribution, 46 | non_linear_function: Callable, 47 | num_samples: int, 48 | ) -> Distribution: 49 | """ 50 | Sample a distribution and propagate the samples through the non-linear function. 51 | Reestimate the distribution from the transformed samples. 52 | """ 53 | samples = [distribution.sample() for _ in range(num_samples)] 54 | transformed_samples = [non_linear_function(sample) for sample in samples] 55 | return distribution.from_samples(transformed_samples) 56 | 57 | 58 | def propagate_gaussian( 59 | mean: np.ndarray, 60 | covariance: np.ndarray, 61 | non_linear_function: Callable, 62 | ) -> Tuple[np.ndarray, np.ndarray]: 63 | """ 64 | Propagate a Gaussian through a function with the unscented transform. 65 | """ 66 | gaussian = Gaussian(mean, covariance) 67 | new_gaussian = unscented_transform(gaussian, non_linear_function) 68 | return new_gaussian.mean(), new_gaussian.covariance() 69 | 70 | 71 | def propagate_gaussian_cross_cov( 72 | mean: np.ndarray, 73 | covariance: np.ndarray, 74 | non_linear_function: Callable 75 | ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: 76 | """ 77 | Propagate a Gaussian through a function with the unscented transform. 78 | Return the cross-covariance between the mean and the transformed sigma points. 79 | """ 80 | gaussian = Gaussian(mean, covariance) 81 | new_gaussian, cross_covariance = unscented_transform_cross_cov(gaussian, non_linear_function) 82 | return new_gaussian.mean(), new_gaussian.covariance(), cross_covariance 83 | 84 | 85 | def test_unscented_transform_linear_func(): 86 | """ 87 | Test the unscented transform with a linear function. 88 | """ 89 | mean = np.array([1.0, 2.0]) 90 | covariance = np.array([[1.0, 0.0], [0.0, 1.0]]) 91 | gaussian = Gaussian(mean, covariance) 92 | 93 | def linear_function(x): 94 | return np.array([2.0*x[0], 3.0*x[1]]) 95 | 96 | jacobian = np.array([[2.0, 0.0], [0.0, 3.0]]) 97 | 98 | transformed_gaussian = unscented_transform(gaussian, linear_function) 99 | expected_mean = linear_function(mean) 100 | expected_covariance = jacobian @ covariance @ jacobian.T 101 | assert np.allclose(transformed_gaussian.mean(), expected_mean) 102 | assert np.allclose(transformed_gaussian.covariance(), expected_covariance) 103 | 104 | 105 | if __name__ == '__main__': 106 | test_unscented_transform_linear_func() 107 | -------------------------------------------------------------------------------- /bayesfilter/utilities.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | def propagate_covariance( 5 | jacobian: np.ndarray, 6 | covariance: np.ndarray 7 | ) -> np.ndarray: 8 | """ 9 | Propagate a covariance through a function 10 | """ 11 | return np.dot(np.dot(jacobian, covariance), jacobian.T) 12 | -------------------------------------------------------------------------------- /bayesfilter/version.py: -------------------------------------------------------------------------------- 1 | __version__ = '0.0.5' -------------------------------------------------------------------------------- /licence.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2024 Hugo Hadfield. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 8 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | 2 | from setuptools import setup 3 | import os 4 | 5 | here = os.path.abspath(os.path.dirname(__file__)) 6 | exec(open(os.path.join(here, 'bayesfilter/version.py')).read()) 7 | 8 | 9 | setup( 10 | name='bayesfilter', 11 | version=__version__, 12 | packages=['bayesfilter'], 13 | install_requires=[ 14 | 'numpy', 15 | ], 16 | license='MIT', 17 | author='Hugo Hadfield', 18 | author_email="hadfield.hugo@gmail.com", 19 | long_description=open('README.md').read(), 20 | long_description_content_type='text/markdown', 21 | description="A pure Python/NumPy library for Bayesian filtering and smoothing", 22 | url="https://github.com/hugohadfield/bayesfilter", 23 | python_requires='>=3.6', 24 | classifiers=[ 25 | 'Development Status :: 3 - Alpha', 26 | 'Intended Audience :: Science/Research', 27 | 'License :: OSI Approved :: MIT License', 28 | 'Programming Language :: Python :: 3', 29 | 'Topic :: Scientific/Engineering :: Mathematics', 30 | ], 31 | ) --------------------------------------------------------------------------------