├── .gitignore ├── LICENSE ├── MANIFEST.in ├── README.md ├── docs ├── Makefile ├── conf.py ├── index.rst └── make.bat ├── requirements.txt ├── research ├── bsq │ ├── bsq_mtran.py │ ├── bsq_tracking.py │ ├── bsq_ungm.py │ └── journal_figure.py ├── gpq │ ├── gpq_tracking.py │ ├── icinco_demo.py │ ├── journal_figure.py │ └── polar2cartesian.py ├── gpqd │ ├── gpqd_base.py │ ├── hybrid_demo.py │ ├── mlsp2016_demo.py │ └── tests │ │ └── test_gpqd.py ├── tpq │ ├── figprint.py │ ├── gpr_vs_tpr.py │ ├── synthetic.py │ ├── tpq_base.py │ ├── tpq_constant_velocity.py │ └── tpq_ungm.py └── truncated_mt_demo.py ├── setup.cfg ├── setup.py └── ssmtoybox ├── __init__.py ├── bq ├── __init__.py ├── bqkern.py ├── bqmod.py └── bqmtran.py ├── mtran.py ├── ssinf.py ├── ssmod.py ├── tests ├── __init__.py ├── test_bqkern.py ├── test_bqmod.py ├── test_bqmtran.py ├── test_mtran.py ├── test_mult_dot_einsum.py ├── test_ssinf.py ├── test_ssmod.py └── test_utils.py └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Created by https://www.gitignore.io/api/pycharm+all 3 | 4 | ### PyCharm+all ### 5 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm 6 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 7 | 8 | # User-specific stuff: 9 | .idea/**/workspace.xml 10 | .idea/**/tasks.xml 11 | .idea/dictionaries 12 | 13 | # Sensitive or high-churn files: 14 | .idea/**/dataSources/ 15 | .idea/**/dataSources.ids 16 | .idea/**/dataSources.xml 17 | .idea/**/dataSources.local.xml 18 | .idea/**/sqlDataSources.xml 19 | .idea/**/dynamic.xml 20 | .idea/**/uiDesigner.xml 21 | 22 | # Gradle: 23 | .idea/**/gradle.xml 24 | .idea/**/libraries 25 | 26 | # CMake 27 | cmake-build-debug/ 28 | 29 | # Mongo Explorer plugin: 30 | .idea/**/mongoSettings.xml 31 | 32 | ## File-based project format: 33 | *.iws 34 | 35 | ## Plugin-specific files: 36 | 37 | # IntelliJ 38 | /out/ 39 | 40 | # mpeltonen/sbt-idea plugin 41 | .idea_modules/ 42 | 43 | # JIRA plugin 44 | atlassian-ide-plugin.xml 45 | 46 | # Cursive Clojure plugin 47 | .idea/replstate.xml 48 | 49 | # Ruby plugin and RubyMine 50 | /.rakeTasks 51 | 52 | # Crashlytics plugin (for Android Studio and IntelliJ) 53 | com_crashlytics_export_strings.xml 54 | crashlytics.properties 55 | crashlytics-build.properties 56 | fabric.properties 57 | 58 | # Python 59 | 60 | ### PyCharm+all Patch ### 61 | # Ignores the whole idea folder 62 | # See https://github.com/joeblau/gitignore.io/issues/186 and https://github.com/joeblau/gitignore.io/issues/360 63 | 64 | .idea/ 65 | 66 | 67 | ### Python ### 68 | # Byte-compiled / optimized / DLL files 69 | __pycache__/ 70 | *.py[cod] 71 | *$py.class 72 | 73 | # C extensions 74 | *.so 75 | 76 | # Distribution / packaging 77 | .Python 78 | build/ 79 | develop-eggs/ 80 | dist/ 81 | downloads/ 82 | eggs/ 83 | .eggs/ 84 | lib/ 85 | lib64/ 86 | parts/ 87 | sdist/ 88 | var/ 89 | wheels/ 90 | *.egg-info/ 91 | .installed.cfg 92 | *.egg 93 | 94 | # PyInstaller 95 | # Usually these files are written by a python script from a template 96 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 97 | *.manifest 98 | *.spec 99 | 100 | # Installer logs 101 | pip-log.txt 102 | pip-delete-this-directory.txt 103 | 104 | # Unit test / coverage reports 105 | htmlcov/ 106 | .tox/ 107 | .coverage 108 | .coverage.* 109 | .cache 110 | .pytest_cache/ 111 | nosetests.xml 112 | coverage.xml 113 | *.cover 114 | .hypothesis/ 115 | 116 | # Translations 117 | *.mo 118 | *.pot 119 | 120 | # Flask stuff: 121 | instance/ 122 | .webassets-cache 123 | 124 | # Scrapy stuff: 125 | .scrapy 126 | 127 | # Sphinx documentation 128 | docs/_build/ 129 | 130 | # PyBuilder 131 | target/ 132 | 133 | # Jupyter Notebook 134 | .ipynb_checkpoints 135 | 136 | # pyenv 137 | .python-version 138 | 139 | # celery beat schedule file 140 | celerybeat-schedule.* 141 | 142 | # SageMath parsed files 143 | *.sage.py 144 | 145 | # Environments 146 | .env 147 | .venv 148 | env/ 149 | venv/ 150 | ENV/ 151 | env.bak/ 152 | venv.bak/ 153 | 154 | # Spyder project settings 155 | .spyderproject 156 | .spyproject 157 | 158 | # Rope project settings 159 | .ropeproject 160 | 161 | # mkdocs documentation 162 | /site 163 | 164 | # mypy 165 | .mypy_cache/ 166 | 167 | 168 | # End of https://www.gitignore.io/api/python -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Jakub Prüher 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include LICENSE 2 | 3 | include README.md 4 | 5 | include requirements.txt -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SSM Toybox 2 | Python 3 implementation of the nonlinear sigma-point Kalman filters based on Bayesian quadrature. The filter is understood to be composed of moment transforms, which lend it it's uniqueness, which is why the "moment transform" is the key term used troughout the publications and the toybox. 3 | 4 | ## Structure 5 | The toybox itself is confined in the `ssmtoybox` folder together with the unit tests. 6 | 7 | Under `research` is a code for reproducing results in the following publications (chronological order): 8 | * `gpq`: Gaussian Process Quadrature Moment Transform [1] 9 | * `gpqd`: Gaussian Process Quadrautre Moment Transform with Derivatives [2] 10 | * `tpq`: Student's t-Process Quadrature Moment Transform [3] 11 | * `bsq`: Bayes-Sard Quadrature Moment Transform [4] 12 | 13 | 14 | ### Build documentation 15 | ``` 16 | cd docs 17 | sphinx-apidoc -o ./ ../ssmtoybox ../ssmtoybox/tests 18 | make html 19 | ``` 20 | 21 | 22 | ### Why toybox? 23 | The aim of this project was mainly to provide a code base for testing ideas during my Ph.D. research related to the application of Bayesian quadrature for improvement of Kalman filter estimates in terms of crediblity. The code was never meant to be used seriously as a toolbox. 24 | 25 | 26 | ### References 27 | [1]: [[DOI](http://dx.doi.org/10.1109/TAC.2017.2774444) | [PDF](https://arxiv.org/abs/1701.01356)] 28 | Prüher, J. & Straka, O. *Gaussian Process Quadrature Moment Transform*, IEEE Transactions on Automatic Control, 2017 29 | 30 | [2]: [[DOI](https://doi.org/10.1109/MLSP.2016.7738903) | [PDF](https://ieeexplore.ieee.org/document/7738903)] Prüher, J., & Sarkka, S. (2016). *On the Use of Gradient Information in Gaussian Process Quadratures*. In 2016 IEEE 26th International Workshop on Machine Learning for Signal Processing (MLSP) (pp. 1–6). 31 | 32 | [3]: [[DOI](http://dx.doi.org/10.23919/ICIF.2017.8009742) | [PDF](https://arxiv.org/abs/1703.05189)] 33 | Prüher, J.; Tronarp, F.; Karvonen, T.; Särkkä, S. & Straka, O. *Student-t Process Quadratures for Filtering of 34 | Non-linear Systems with Heavy-tailed Noise*, 20th International Conference on Information Fusion (Fusion), 1-8, 2017 35 | 36 | [4]: [[DOI](https://doi.org/10.1109/TAC.2020.2991698) | [PDF](https://export.arxiv.org/pdf/1811.11474)] Pruher, J., Karvonen, T., Oates, C. J., Straka, O., & Sarkka, S. (2021). *Improved Calibration of Numerical Integration Error in Sigma-Point Filters*. IEEE Transactions on Automatic Control, 66(3), 1286–1292. 37 | 38 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SPHINXPROJ = SSMToybox 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # Configuration file for the Sphinx documentation builder. 4 | # 5 | # This file does only contain a selection of the most common options. For a 6 | # full list see the documentation: 7 | # http://www.sphinx-doc.org/en/master/config 8 | 9 | # -- Path setup -------------------------------------------------------------- 10 | 11 | # If extensions (or modules to document with autodoc) are in another directory, 12 | # add these directories to sys.path here. If the directory is relative to the 13 | # documentation root, use os.path.abspath to make it absolute, like shown here. 14 | # 15 | import os 16 | import sys 17 | sys.path.insert(0, os.path.abspath('..')) 18 | sys.path.insert(0, os.path.abspath(os.path.join('..', 'ssmtoybox'))) 19 | 20 | 21 | # -- Project information ----------------------------------------------------- 22 | 23 | project = 'SSM Toybox' 24 | copyright = '2018, Jakub Prüher' 25 | author = 'Jakub Prüher' 26 | 27 | # The short X.Y version 28 | version = '' 29 | # The full version, including alpha/beta/rc tags 30 | release = '' 31 | 32 | 33 | # -- General configuration --------------------------------------------------- 34 | 35 | # If your documentation needs a minimal Sphinx version, state it here. 36 | # 37 | # needs_sphinx = '1.0' 38 | 39 | # Add any Sphinx extension module names here, as strings. They can be 40 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 41 | # ones. 42 | extensions = [ 43 | 'sphinx.ext.autodoc', 44 | 'sphinx.ext.napoleon', 45 | 'sphinx.ext.coverage', 46 | 'sphinx.ext.mathjax', 47 | 'sphinx.ext.viewcode', 48 | 'sphinx.ext.githubpages', 49 | ] 50 | 51 | napoleon_google_docstring = False 52 | napoleon_use_param = False 53 | napoleon_use_ivar = True 54 | 55 | # The suffix(es) of source filenames. 56 | # You can specify multiple suffix as a list of string: 57 | # 58 | # source_suffix = ['.rst', '.md'] 59 | source_suffix = '.rst' 60 | 61 | # The master toctree document. 62 | master_doc = 'index' 63 | 64 | # The language for content autogenerated by Sphinx. Refer to documentation 65 | # for a list of supported languages. 66 | # 67 | # This is also used if you do content translation via gettext catalogs. 68 | # Usually you set "language" from the command line for these cases. 69 | language = None 70 | 71 | # List of patterns, relative to source directory, that match files and 72 | # directories to ignore when looking for source files. 73 | # This pattern also affects html_static_path and html_extra_path . 74 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 75 | 76 | # The name of the Pygments (syntax highlighting) style to use. 77 | pygments_style = 'sphinx' 78 | 79 | 80 | # -- Options for HTML output ------------------------------------------------- 81 | 82 | # The theme to use for HTML and HTML Help pages. See the documentation for 83 | # a list of builtin themes. 84 | # 85 | html_theme = 'sphinx_rtd_theme' 86 | 87 | # Theme options are theme-specific and customize the look and feel of a theme 88 | # further. For a list of options available for each theme, see the 89 | # documentation. 90 | # 91 | # html_theme_options = {} 92 | 93 | # Add any paths that contain custom static files (such as style sheets) here, 94 | # relative to this directory. They are copied after the builtin static files, 95 | # so a file named "default.css" will overwrite the builtin "default.css". 96 | html_static_path = ['_static'] 97 | 98 | # Custom sidebar templates, must be a dictionary that maps document names 99 | # to template names. 100 | # 101 | # The default sidebars (for documents that don't match any pattern) are 102 | # defined by theme itself. Builtin themes are using these templates by 103 | # default: ``['localtoc.html', 'relations.html', 'sourcelink.html', 104 | # 'searchbox.html']``. 105 | # 106 | # html_sidebars = {} 107 | 108 | 109 | # -- Options for HTMLHelp output --------------------------------------------- 110 | 111 | # Output file base name for HTML help builder. 112 | htmlhelp_basename = 'SSMToyboxdoc' 113 | 114 | 115 | # -- Options for LaTeX output ------------------------------------------------ 116 | 117 | latex_elements = { 118 | # The paper size ('letterpaper' or 'a4paper'). 119 | # 120 | # 'papersize': 'letterpaper', 121 | 122 | # The font size ('10pt', '11pt' or '12pt'). 123 | # 124 | # 'pointsize': '10pt', 125 | 126 | # Additional stuff for the LaTeX preamble. 127 | # 128 | # 'preamble': '', 129 | 130 | # Latex figure (float) alignment 131 | # 132 | # 'figure_align': 'htbp', 133 | } 134 | 135 | # Grouping the document tree into LaTeX files. List of tuples 136 | # (source start file, target name, title, 137 | # author, documentclass [howto, manual, or own class]). 138 | latex_documents = [ 139 | (master_doc, 'SSMToybox.tex', 'SSM Toybox Documentation', 140 | 'Jakub Prüher', 'manual'), 141 | ] 142 | 143 | 144 | # -- Options for manual page output ------------------------------------------ 145 | 146 | # One entry per manual page. List of tuples 147 | # (source start file, name, description, authors, manual section). 148 | man_pages = [ 149 | (master_doc, 'ssmtoybox', 'SSM Toybox Documentation', 150 | [author], 1) 151 | ] 152 | 153 | 154 | # -- Options for Texinfo output ---------------------------------------------- 155 | 156 | # Grouping the document tree into Texinfo files. List of tuples 157 | # (source start file, target name, title, author, 158 | # dir menu entry, description, category) 159 | texinfo_documents = [ 160 | (master_doc, 'SSMToybox', 'SSM Toybox Documentation', 161 | author, 'SSMToybox', 'One line description of project.', 162 | 'Miscellaneous'), 163 | ] 164 | 165 | 166 | # -- Extension configuration ------------------------------------------------- 167 | 168 | # -- Options for todo extension ---------------------------------------------- 169 | 170 | # If true, `todo` and `todoList` produce output, else they produce nothing. 171 | todo_include_todos = True -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | .. SSM Toybox documentation master file, created by 2 | sphinx-quickstart on Mon Jun 25 16:12:46 2018. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to SSM Toybox's documentation! 7 | ====================================== 8 | 9 | .. toctree:: 10 | :maxdepth: 2 11 | :caption: Contents: 12 | 13 | modules 14 | 15 | Indices and tables 16 | ================== 17 | 18 | * :ref:`genindex` 19 | * :ref:`modindex` 20 | * :ref:`search` 21 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=. 11 | set BUILDDIR=_build 12 | set SPHINXPROJ=SSMToybox 13 | 14 | if "%1" == "" goto help 15 | 16 | %SPHINXBUILD% >NUL 2>NUL 17 | if errorlevel 9009 ( 18 | echo. 19 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 20 | echo.installed, then set the SPHINXBUILD environment variable to point 21 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 22 | echo.may add the Sphinx directory to PATH. 23 | echo. 24 | echo.If you don't have Sphinx installed, grab it from 25 | echo.http://sphinx-doc.org/ 26 | exit /b 1 27 | ) 28 | 29 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 30 | goto end 31 | 32 | :help 33 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 34 | 35 | :end 36 | popd 37 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy>=1.15.4 2 | numba>=0.41.0 3 | scipy>=1.1.0 4 | scikit-learn>=0.20.1 5 | pandas>=0.23.4 6 | matplotlib>=3.0.1 -------------------------------------------------------------------------------- /research/bsq/bsq_mtran.py: -------------------------------------------------------------------------------- 1 | import os 2 | # import numpy as np 3 | # import matplotlib.pyplot as plt 4 | from journal_figure import * 5 | from collections import OrderedDict 6 | 7 | from ssmtoybox.mtran import UnscentedTransform, SphericalRadialTransform, MonteCarloTransform 8 | from ssmtoybox.bq.bqmtran import BayesSardTransform, GaussianProcessTransform 9 | from ssmtoybox.utils import symmetrized_kl_divergence 10 | 11 | 12 | def sos(x, pars, dx=False): 13 | """Sum of squares function. 14 | 15 | Parameters 16 | ---------- 17 | x : numpy.ndarray 1D-array 18 | Returns 19 | ------- 20 | """ 21 | x = np.atleast_1d(x) 22 | if not dx: 23 | return np.atleast_1d(np.sum(x ** 2, axis=0)) 24 | else: 25 | return np.atleast_1d(2 * x).T.flatten() 26 | 27 | 28 | def toa(x, pars, dx=False): 29 | """Time of arrival. 30 | 31 | Parameters 32 | ---------- 33 | x 34 | Returns 35 | ------- 36 | """ 37 | x = np.atleast_1d(x) 38 | if not dx: 39 | return np.atleast_1d(np.sum(x ** 2, axis=0) ** 0.5) 40 | else: 41 | return np.atleast_1d(x * np.sum(x ** 2, axis=0) ** (-0.5)).T.flatten() 42 | 43 | 44 | def rss(x, pars, dx=False): 45 | """Received signal strength in dB scale. 46 | 47 | Parameters 48 | ---------- 49 | x : N-D ndarray 50 | Returns 51 | ------- 52 | """ 53 | c = 10 54 | b = 2 55 | x = np.atleast_1d(x) 56 | if not dx: 57 | return np.atleast_1d(c - b * 10 * np.log10(np.sum(x ** 2, axis=0))) 58 | else: 59 | return np.atleast_1d(-b * 20 / (x * np.log(10))).T.flatten() 60 | 61 | 62 | def doa(x, pars, dx=False): 63 | """Direction of arrival in 2D. 64 | 65 | Parameters 66 | ---------- 67 | x : 2-D ndarray 68 | Returns 69 | ------- 70 | """ 71 | if not dx: 72 | return np.atleast_1d(np.arctan2(x[1], x[0])) 73 | else: 74 | return np.array([-x[1], x[0]]) / (x[0] ** 2 + x[1] ** 2).T.flatten() 75 | 76 | 77 | def sum_of_squares_demo(plot_fit=False): 78 | dims = [1, 2, 3, 5, 10, 15, 25] 79 | 80 | sos_mean_data = np.zeros((2, len(dims))) 81 | sos_var_data = sos_mean_data.copy() 82 | for d, dim_in in enumerate(dims): 83 | alpha_ut = np.hstack((np.zeros((dim_in, 1)), np.eye(dim_in), 2*np.eye(dim_in))).astype(np.int) 84 | kpar = np.array([[1.0] + dim_in*[2.0]]) 85 | tforms = OrderedDict({ 86 | 'bsq': BayesSardTransform(dim_in, 1, kpar, alpha_ut, point_str='ut', point_par={'kappa': 0.0}), 87 | 'ut': UnscentedTransform(dim_in, kappa=0.0, beta=0.0) 88 | }) 89 | 90 | # print('EMV (dim = {:d}): {:.2e}'.format(dim_in, tforms['bsq'].model.model_var)) 91 | 92 | mean_in = np.zeros((dim_in, )) 93 | cov_in = np.eye(dim_in) 94 | 95 | for t, tf_key in enumerate(tforms): 96 | mean_tf, cov_tf, cc = tforms[tf_key].apply(sos, mean_in, cov_in, None) 97 | sos_mean_data[t, d] = np.asscalar(mean_tf) 98 | sos_var_data[t, d] = np.asscalar(cov_tf) 99 | 100 | import pandas as pd 101 | row_labels = [tstr for tstr in tforms.keys()] 102 | col_labels = [str(d) for d in dims] 103 | 104 | table_mean = pd.DataFrame(sos_mean_data, index=row_labels, columns=col_labels) 105 | table_var = pd.DataFrame(sos_var_data, index=row_labels, columns=col_labels) 106 | 107 | pd.set_option('precision', 2) 108 | print(table_mean) 109 | print(table_var) 110 | 111 | if plot_fit: 112 | bsqmt = BayesSardTransform(1, 1, np.array([[1.0, 3.0]]), np.array([[0, 1, 2]], dtype=np.int), point_str='ut') 113 | xtest = np.linspace(-5, 5, 50) 114 | ytrue = np.zeros((len(xtest))) 115 | for i in range(len(xtest)): 116 | ytrue[i] = sos(xtest[i], None) 117 | y = np.zeros((bsqmt.model.points.shape[1], )) 118 | for i in range(bsqmt.model.points.shape[1]): 119 | y[i] = sos(bsqmt.model.points[:, i], None) 120 | bsqmt.model.plot_model(xtest[None, :], fcn_obs=y, fcn_true=ytrue) 121 | 122 | 123 | def polar2cartesian(x, pars): 124 | return x[0] * np.array([np.cos(x[1]), np.sin(x[1])]) 125 | 126 | 127 | def polar2cartesian_skl_demo(): 128 | dim = 2 129 | 130 | # create spiral in polar domain 131 | r_spiral = lambda x: 10 * x 132 | theta_min, theta_max = 0.25 * np.pi, 2.25 * np.pi 133 | 134 | # equidistant points on a spiral 135 | num_mean = 10 136 | theta_pt = np.linspace(theta_min, theta_max, num_mean) 137 | r_pt = r_spiral(theta_pt) 138 | 139 | # setup input moments: means are placed on the points of the spiral 140 | num_cov = 10 # num_cov covariances are considered for each mean 141 | r_std = 0.5 142 | theta_std = np.deg2rad(np.linspace(6, 36, num_cov)) 143 | mean = np.array([r_pt, theta_pt]) 144 | cov = np.zeros((dim, dim, num_cov)) 145 | for i in range(num_cov): 146 | cov[..., i] = np.diag([r_std**2, theta_std[i]**2]) 147 | 148 | # COMPARE moment transforms 149 | ker_par = np.array([[1.0, 60, 6]]) 150 | mul_ind = np.hstack((np.zeros((dim, 1)), np.eye(dim), 2*np.eye(dim))).astype(np.int) 151 | tforms = OrderedDict([ 152 | ('bsq-ut', BayesSardTransform(dim, dim, ker_par, mul_ind, point_str='ut', point_par={'kappa': 2, 'alpha': 1})), 153 | ('gpq-ut', GaussianProcessTransform(dim, dim, ker_par, point_str='ut', point_par={'kappa': 2, 'alpha': 1})), 154 | ('ut', UnscentedTransform(dim, kappa=2, alpha=1, beta=0)), 155 | ]) 156 | baseline_mtf = MonteCarloTransform(dim, n=10000) 157 | num_tforms = len(tforms) 158 | 159 | # initialize storage of SKL scores 160 | skl_dict = dict([(mt_str, np.zeros((num_mean, num_cov))) for mt_str in tforms.keys()]) 161 | 162 | # for each mean 163 | for i in range(num_mean): 164 | 165 | # for each covariance 166 | for j in range(num_cov): 167 | mean_in, cov_in = mean[..., i], cov[..., j] 168 | 169 | # calculate baseline using Monte Carlo 170 | mean_out_mc, cov_out_mc, cc = baseline_mtf.apply(polar2cartesian, mean_in, cov_in, None) 171 | 172 | # for each moment transform 173 | for mt_str in tforms.keys(): 174 | 175 | # calculate the transformed moments 176 | mean_out, cov_out, cc = tforms[mt_str].apply(polar2cartesian, mean_in, cov_in, None) 177 | 178 | # compute SKL 179 | skl_dict[mt_str][i, j] = symmetrized_kl_divergence(mean_out_mc, cov_out_mc, mean_out, cov_out) 180 | 181 | # PLOT the SKL score for each MT and position on the spiral 182 | plt.style.use('seaborn-deep') 183 | printfig = FigurePrint() 184 | fig = plt.figure() 185 | 186 | # Average over mean indexes 187 | ax1 = fig.add_subplot(121) 188 | index = np.arange(num_mean)+1 189 | for mt_str in tforms.keys(): 190 | ax1.plot(index, skl_dict[mt_str].mean(axis=1), marker='o', label=mt_str.upper()) 191 | ax1.set_xlabel('Position index') 192 | ax1.set_ylabel('SKL') 193 | 194 | # Average over azimuth variances 195 | ax2 = fig.add_subplot(122, sharey=ax1) 196 | for mt_str in tforms.keys(): 197 | ax2.plot(np.rad2deg(theta_std), skl_dict[mt_str].mean(axis=0), marker='o', label=mt_str.upper()) 198 | ax2.set_xlabel('Azimuth STD [$ \circ $]') 199 | ax2.legend() 200 | fig.tight_layout(pad=0) 201 | plt.show() 202 | 203 | # save figure 204 | printfig.savefig('polar2cartesian_skl') 205 | 206 | 207 | if __name__ == '__main__': 208 | polar2cartesian_skl_demo() 209 | # sum_of_squares_demo() 210 | -------------------------------------------------------------------------------- /research/bsq/bsq_ungm.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import numpy as np 4 | import pandas as pd 5 | # import matplotlib.pyplot as plt 6 | from journal_figure import * 7 | from numpy import newaxis as na 8 | from scipy.stats import multivariate_normal 9 | from scipy.linalg import cho_factor, cho_solve 10 | from numpy.linalg import cholesky 11 | 12 | from ssmtoybox.ssinf import ExtendedKalman, CubatureKalman, UnscentedKalman, GaussHermiteKalman 13 | from ssmtoybox.ssinf import GaussianProcessKalman, BayesSardKalman, BayesSardTransform 14 | from ssmtoybox.ssmod import UNGMTransition, UNGMMeasurement 15 | from ssmtoybox.utils import bootstrap_var, squared_error, neg_log_likelihood, log_cred_ratio, mse_matrix, GaussRV 16 | 17 | alg_label_dict = { 18 | 'GaussianProcessKalman': 'GPQKF', 19 | 'BayesSardKalman': 'BSQKF', 20 | 'ExtendedKalman': 'EKF', 21 | 'CubatureKalman': 'CKF', 22 | 'UnscentedKalman': 'UKF', 23 | 'GaussHermiteKalman': 'GHKF', 24 | } 25 | 26 | 27 | def evaluate_performance(x, mean_f, cov_f, mean_s, cov_s, bootstrap_variance=True): 28 | num_dim, num_step, num_sim, num_alg = mean_f.shape 29 | 30 | # simulation-average of time-averaged RMSE 31 | print('RMSE...') 32 | rmseData_f = np.sqrt(np.mean(squared_error(x[..., na], mean_f), axis=1)) 33 | rmseData_s = np.sqrt(np.mean(squared_error(x[..., na], mean_s), axis=1)) 34 | rmseMean_f = rmseData_f.mean(axis=1).T 35 | rmseMean_s = rmseData_s.mean(axis=1).T 36 | 37 | print('NLL and NCI...') 38 | nllData_f = np.zeros((1, num_step, num_sim, num_alg)) 39 | nciData_f = nllData_f.copy() 40 | nllData_s = nllData_f.copy() 41 | nciData_s = nllData_f.copy() 42 | for k in range(1, num_step): 43 | for fi in range(num_alg): 44 | mse_mat_f = mse_matrix(x[:, k, :], mean_f[:, k, :, fi]) 45 | mse_mat_s = mse_matrix(x[:, k, :], mean_f[:, k, :, fi]) 46 | for s in range(num_sim): 47 | # filter scores 48 | nllData_f[:, k, s, fi] = neg_log_likelihood(x[:, k, s], mean_f[:, k, s, fi], cov_f[:, :, k, s, fi]) 49 | nciData_f[:, k, s, fi] = log_cred_ratio(x[:, k, s], mean_f[:, k, s, fi], cov_f[:, :, k, s, fi], 50 | mse_mat_f) 51 | 52 | # smoother scores 53 | nllData_s[:, k, s, fi] = neg_log_likelihood(x[:, k, s], mean_s[:, k, s, fi], cov_s[:, :, k, s, fi]) 54 | nciData_s[:, k, s, fi] = log_cred_ratio(x[:, k, s], mean_s[:, k, s, fi], cov_s[:, :, k, s, fi], 55 | mse_mat_s) 56 | 57 | nciData_f, nciData_s = nciData_f.mean(axis=1), nciData_s.mean(axis=1) 58 | nllData_f, nllData_s = nllData_f.mean(axis=1), nllData_s.mean(axis=1) 59 | 60 | # average scores (over time and MC simulations) 61 | nciMean_f, nciMean_s = nciData_f.mean(axis=1).T, nciData_s.mean(axis=1).T 62 | nllMean_f, nllMean_s = nllData_f.mean(axis=1).T, nllData_s.mean(axis=1).T 63 | 64 | if bootstrap_variance: 65 | print('Bootstrapping variance ...') 66 | num_bs_samples = 10000 67 | rmseStd_f, rmseStd_s = np.zeros((num_alg, 1)), np.zeros((num_alg, 1)) 68 | nciStd_f, nciStd_s = rmseStd_f.copy(), rmseStd_f.copy() 69 | nllStd_f, nllStd_s = rmseStd_f.copy(), rmseStd_f.copy() 70 | for f in range(num_alg): 71 | rmseStd_f[f] = 2 * np.sqrt(bootstrap_var(rmseData_f[..., f], num_bs_samples)) 72 | rmseStd_s[f] = 2 * np.sqrt(bootstrap_var(rmseData_s[..., f], num_bs_samples)) 73 | nciStd_f[f] = 2 * np.sqrt(bootstrap_var(nciData_f[..., f], num_bs_samples)) 74 | nciStd_s[f] = 2 * np.sqrt(bootstrap_var(nciData_s[..., f], num_bs_samples)) 75 | nllStd_f[f] = 2 * np.sqrt(bootstrap_var(nllData_f[..., f], num_bs_samples)) 76 | nllStd_s[f] = 2 * np.sqrt(bootstrap_var(nllData_s[..., f], num_bs_samples)) 77 | 78 | return rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s, \ 79 | rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s 80 | else: 81 | return rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s 82 | 83 | 84 | def print_table(data, row_labels=None, col_labels=None, latex=False): 85 | pd.DataFrame(data, index=row_labels, columns=col_labels) 86 | print(pd) 87 | if latex: 88 | pd.to_latex() 89 | 90 | 91 | def tables(): 92 | steps, mc = 500, 100 93 | # initialize UNGM model 94 | dyn = UNGMTransition(GaussRV(1, cov=5.0), GaussRV(1, cov=10.0)) 95 | obs = UNGMMeasurement(GaussRV(1, cov=1.0), 1) 96 | # generate some data 97 | np.random.seed(0) 98 | x = dyn.simulate_discrete(steps, mc) 99 | z = obs.simulate_measurements(x) 100 | 101 | par_ut = np.array([[3.0, 0.3]]) 102 | par_gh5 = np.array([[5.0, 0.6]]) 103 | par_gh7 = np.array([[3.0, 0.4]]) 104 | 105 | mulind_ut = np.array([[0, 1, 2]]) 106 | mulind_gh = lambda degree: np.atleast_2d(np.arange(degree)) 107 | 108 | # initialize filters/smoothers 109 | algorithms = ( 110 | # Classical filters 111 | UnscentedKalman(dyn, obs, alpha=1.0, beta=0.0), 112 | GaussHermiteKalman(dyn, obs, deg=5), 113 | GaussHermiteKalman(dyn, obs, deg=7), 114 | # GPQ filters 115 | GaussianProcessKalman(dyn, obs, par_ut, par_ut, kernel='rbf', points='ut', point_hyp={'alpha': 1.0}), 116 | GaussianProcessKalman(dyn, obs, par_gh5, par_gh5, kernel='rbf', points='gh', point_hyp={'degree': 5}), 117 | GaussianProcessKalman(dyn, obs, par_gh7, par_gh7, kernel='rbf', points='gh', point_hyp={'degree': 7}), 118 | # BSQ filters 119 | BayesSardKalman(dyn, obs, par_ut, par_ut, mulind_ut, mulind_ut, points='ut', point_hyp={'alpha': 1.0}), 120 | BayesSardKalman(dyn, obs, par_gh5, par_gh5, mulind_gh(5), mulind_gh(5), points='gh', point_hyp={'degree': 5}), 121 | BayesSardKalman(dyn, obs, par_gh7, par_gh7, mulind_gh(7), mulind_gh(7), points='gh', point_hyp={'degree': 7}), 122 | ) 123 | num_algs = len(algorithms) 124 | 125 | # space for estimates 126 | dim = dyn.dim_state 127 | mean_f, cov_f = np.zeros((dim, steps, mc, num_algs)), np.zeros((dim, dim, steps, mc, num_algs)) 128 | mean_s, cov_s = np.zeros((dim, steps, mc, num_algs)), np.zeros((dim, dim, steps, mc, num_algs)) 129 | # do filtering/smoothing 130 | t0 = time.time() # measure execution time 131 | print('Running filters/smoothers ...') 132 | for a, alg in enumerate(algorithms): 133 | print('{}'.format(alg.__class__.__name__)) # print filter/smoother name 134 | for sim in range(mc): 135 | mean_f[..., sim, a], cov_f[..., sim, a] = alg.forward_pass(z[..., sim]) 136 | mean_s[..., sim, a], cov_s[..., sim, a] = alg.backward_pass() 137 | alg.reset() 138 | print('Done in {0:.4f} [sec]'.format(time.time() - t0)) 139 | 140 | # evaluate perfomance 141 | scores = evaluate_performance(x, mean_f, cov_f, mean_s, cov_s) 142 | rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s = scores[:6] 143 | rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s = scores[6:] 144 | 145 | # put data into Pandas DataFrame for fancy printing and latex export 146 | # row_labels = ['SR', 'UT', 'GH-5', 'GH-7', 'GH-10', 'GH-15', 'GH-20'] 147 | row_labels = ['UT', 'GH-5', 'GH-7'] 148 | num_labels = len(row_labels) 149 | col_labels = ['Classical', 'GPQ', 'BSQ', 'Classical (2std)', 'GPQ (2std)', 'BSQ (2std)'] 150 | rmse_table_f = pd.DataFrame(np.hstack((rmseMean_f.reshape(3, num_labels).T, rmseStd_f.reshape(3, num_labels).T)), 151 | index=row_labels, columns=col_labels) 152 | nci_table_f = pd.DataFrame(np.hstack((nciMean_f.reshape(3, num_labels).T, nciStd_f.reshape(3, num_labels).T)), 153 | index=row_labels, columns=col_labels) 154 | nll_table_f = pd.DataFrame(np.hstack((nllMean_f.reshape(3, num_labels).T, nllStd_f.reshape(3, num_labels).T)), 155 | index=row_labels, columns=col_labels) 156 | rmse_table_s = pd.DataFrame(np.hstack((rmseMean_s.reshape(3, num_labels).T, rmseStd_s.reshape(3, num_labels).T)), 157 | index=row_labels, columns=col_labels) 158 | nci_table_s = pd.DataFrame(np.hstack((nciMean_s.reshape(3, num_labels).T, nciStd_s.reshape(3, num_labels).T)), 159 | index=row_labels, columns=col_labels) 160 | nll_table_s = pd.DataFrame(np.hstack((nllMean_s.reshape(3, num_labels).T, nllStd_s.reshape(3, num_labels).T)), 161 | index=row_labels, columns=col_labels) 162 | 163 | # print kernel parameters 164 | print('Kernel parameters') 165 | print('{:5}: {}'.format('UT', par_ut)) 166 | print('{:5}: {}'.format('GH-5', par_gh5)) 167 | print('{:5}: {}'.format('GH-7', par_gh7)) 168 | print() 169 | 170 | # print tables 171 | pd.set_option('precision', 2, 'display.max_columns', 6) 172 | print('Filter RMSE') 173 | print(rmse_table_f) 174 | print('Filter NCI') 175 | print(nci_table_f) 176 | print('Filter NLL') 177 | print(nll_table_f) 178 | print('Smoother RMSE') 179 | print(rmse_table_s) 180 | print('Smoother NCI') 181 | print(nci_table_s) 182 | print('Smoother NLL') 183 | print(nll_table_s) 184 | # return computed metrics for filters and smoothers 185 | return {'filter_RMSE': rmse_table_f, 'filter_NCI': nci_table_f, 'filter_NLL': nll_table_f, 186 | 'smoother_RMSE': rmse_table_s, 'smoother_NCI': nci_table_s, 'smoother_NLL': nll_table_s} 187 | 188 | 189 | # TODO: plot EMV vs. ell on lower dimensional problem 190 | def lengthscale_filter_demo(lscale): 191 | steps, mc = 500, 20 192 | # initialize UNGM model 193 | dyn = UNGMTransition(GaussRV(1, cov=5.0), GaussRV(1, cov=10.0)) 194 | obs = UNGMMeasurement(GaussRV(1, cov=1.0), 1) 195 | # generate some data 196 | x = dyn.simulate_discrete(steps, mc) 197 | z = obs.simulate_measurements(x) 198 | 199 | dim = dyn.dim_state 200 | num_el = len(lscale) 201 | # lscale = [1e-3, 3e-3, 1e-2, 3e-2, 1e-1, 3e-1, 1, 3, 1e1, 3e1] # , 1e2, 3e2] 202 | mean_f, cov_f = np.zeros((dim, steps, mc, num_el)), np.zeros((dim, dim, steps, mc, num_el)) 203 | for iel, el in enumerate(lscale): 204 | 205 | # kernel parameters 206 | ker_par = np.array([[1.0, el * dim]]) 207 | 208 | # initialize BHKF with current lenghtscale 209 | f = GaussianProcessKalman(dyn, obs, ker_par, ker_par, kernel='rbf', points='ut') 210 | # filtering 211 | for s in range(mc): 212 | mean_f[..., s, iel], cov_f[..., s, iel] = f.forward_pass(z[..., s]) 213 | 214 | # evaluate RMSE, NCI and NLL 215 | rmseVsEl = squared_error(x[..., na], mean_f) 216 | nciVsEl = rmseVsEl.copy() 217 | nllVsEl = rmseVsEl.copy() 218 | for k in range(steps): 219 | for iel in range(num_el): 220 | mse_mat = mse_matrix(x[:, k, :], mean_f[:, k, :, iel]) 221 | for s in range(mc): 222 | nciVsEl[:, k, s, iel] = log_cred_ratio(x[:, k, s], mean_f[:, k, s, iel], cov_f[:, :, k, s, iel], 223 | mse_mat) 224 | nllVsEl[:, k, s, iel] = neg_log_likelihood(x[:, k, s], mean_f[:, k, s, iel], cov_f[:, :, k, s, iel]) 225 | 226 | # average out time and MC simulations 227 | rmseVsEl = np.sqrt(np.mean(rmseVsEl, axis=1)).mean(axis=1) 228 | nciVsEl = nciVsEl.mean(axis=(1, 2)) 229 | nllVsEl = nllVsEl.mean(axis=(1, 2)) 230 | 231 | # plot influence of changing lengthscale on the RMSE and NCI and NLL filter performance 232 | plt.figure() 233 | plt.semilogx(lscale, rmseVsEl.squeeze(), color='k', ls='-', lw=2, marker='o', label='RMSE') 234 | plt.semilogx(lscale, nciVsEl.squeeze(), color='k', ls='--', lw=2, marker='o', label='NCI') 235 | plt.semilogx(lscale, nllVsEl.squeeze(), color='k', ls='-.', lw=2, marker='o', label='NLL') 236 | plt.grid(True) 237 | plt.legend() 238 | plt.show() 239 | 240 | plot_data = {'el': lscale, 'rmse': rmseVsEl, 'nci': nciVsEl, 'neg_log_likelihood': nllVsEl} 241 | return plot_data 242 | 243 | 244 | def lengthscale_demo(lscale, two_dim=False): 245 | alpha_ut = np.array([[0, 1, 2]]) 246 | tf = BayesSardTransform(1, 1, np.array([[1, 1]]), alpha_ut, point_str='ut') 247 | 248 | emv = np.zeros((len(lscale))) 249 | for i, ell in enumerate(lscale): 250 | par = np.array([[1.0, ell]]) 251 | emv[i] = tf.model.exp_model_variance(par, alpha_ut) 252 | 253 | plt.style.use('seaborn-deep') 254 | pf = FigurePrint() 255 | plt.figure() 256 | plt.semilogx(lscale, emv) 257 | plt.xlabel('$\ell$') 258 | plt.ylabel('EMV') 259 | plt.tight_layout(pad=0) 260 | plt.show() 261 | pf.savefig('emv_lengthscale_sensitivity') 262 | 263 | # 2D case 264 | if two_dim: 265 | alpha_ut = np.hstack((np.zeros((2, 1)), np.eye(2), 2*np.eye(2))).astype(np.int) 266 | tf = BayesSardTransform(2, 1, np.array([[1, 1, 1]]), alpha_ut, point_str='ut') 267 | emv = np.zeros((len(lscale), len(lscale))) 268 | for i, ell_0 in enumerate(lscale): 269 | for j, ell_1 in enumerate(lscale): 270 | par = np.array([[1.0, ell_0, ell_1]]) 271 | emv[i, j] = tf.model.exp_model_variance(par, alpha_ut) 272 | 273 | fig = plt.figure() 274 | from mpl_toolkits.mplot3d.axes3d import Axes3D 275 | ax = Axes3D(fig) 276 | X, Y = np.meshgrid(np.log10(lscale), np.log10(lscale)) 277 | ax.plot_surface(X, Y, emv) 278 | 279 | ax.set_xlabel('$\log_{10}(\ell_1)$') 280 | ax.set_ylabel('$\log_{10}(\ell_2)$') 281 | ax.set_zlabel('EMV') 282 | plt.show() 283 | 284 | 285 | if __name__ == '__main__': 286 | # TODO: use argsparse to create nice command line interface 287 | tables_dict = tables() 288 | # # save tables in LaTeX format 289 | pd.set_option('precision', 2) 290 | with open('ungm_rmse.tex', 'w') as file: 291 | tables_dict['filter_RMSE'].to_latex(file, float_format=lambda s: '{:.3f}'.format(s)) 292 | with open('ungm_inc.tex', 'w') as file: 293 | tables_dict['filter_NCI'].to_latex(file, float_format=lambda s: '{:.3f}'.format(s)) 294 | with open('ungm_nll.tex', 'w') as file: 295 | tables_dict['filter_NLL'].to_latex(file, float_format=lambda s: '{:.3f}'.format(s)) 296 | 297 | lscales = np.logspace(-3, 3, 100) 298 | # plot_data = lengthscale_filter_demo(lscales) 299 | 300 | lengthscale_demo(lscales) 301 | -------------------------------------------------------------------------------- /research/bsq/journal_figure.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib as mpl 3 | mpl.use('pgf') 4 | 5 | 6 | class FigurePrint: 7 | 8 | INCH_PER_PT = 1.0 / 72.27 # Convert pt to inch 9 | PHI = (np.sqrt(5.0) - 1.0) / 2.0 # Aesthetic ratio (you could change this) 10 | 11 | def __init__(self, fig_width_pt=252): 12 | """ 13 | Parameters 14 | ---------- 15 | fig_width_pt : float 16 | Width of the figure in points, usually obtained from the journal specs or using the LaTeX command 17 | ``\the\columnwidth``. Default is ``fig_width_pt=252`` (3.5 inches). 18 | """ 19 | self.fig_width_pt = fig_width_pt 20 | pgf_with_latex = { # setup matplotlib to use latex for output 21 | "pgf.texsystem": "pdflatex", # change this if using xetex or lautex 22 | "text.usetex": True, # use LaTeX to write all text 23 | "font.family": "serif", 24 | "font.serif": [], # blank entries should cause plots to inherit fonts from the document 25 | "font.sans-serif": [], 26 | "font.monospace": [], 27 | "font.size": 10, 28 | "axes.labelsize": 10, # LaTeX default is 10pt font. 29 | "legend.fontsize": 8, # Make the legend/label fonts a little smaller 30 | "xtick.labelsize": 8, 31 | "ytick.labelsize": 8, 32 | # "axes.prop_cycle": ['#5DA5DA', '#FAA43A', '#60BD68', 33 | # '#F17CB0', '#B2912F', '#B276B2', 34 | # '#DECF3F', '#F15854', '#4D4D4D'], 35 | "figure.figsize": self.figsize(), # default fig size 36 | "pgf.preamble": [ # plots will be generated using this preamble 37 | r"\usepackage[utf8]{inputenc}", # use utf8 fonts 38 | r"\usepackage[T1]{fontenc}", 39 | r"\usepackage{siunitx}", 40 | ] 41 | } 42 | mpl.rcParams.update(pgf_with_latex) 43 | 44 | def figsize(self, w_scale=1.0, h_scale=1.0): 45 | """ 46 | Calculates figure width and height given the width and height scale. 47 | Parameters 48 | ---------- 49 | w_scale: float 50 | Figure width scale. 51 | h_scale: float 52 | Figure height scale. 53 | Returns 54 | ------- 55 | list 56 | Figure width and height in inches. 57 | """ 58 | 59 | fig_width = self.fig_width_pt * self.INCH_PER_PT * w_scale # width in inches 60 | fig_height = fig_width * self.PHI * h_scale # height in inches 61 | return [fig_width, fig_height] 62 | 63 | def update_default_figsize(self, fig_width_pt): 64 | """ 65 | Updates default figure size used for saving. 66 | Parameters 67 | ---------- 68 | fig_width_pt : float 69 | Width of the figure in points, usually obtained from the journal specs or using the LaTeX command 70 | ``\the\columnwidth``. 71 | Returns 72 | ------- 73 | """ 74 | self.fig_width_pt = fig_width_pt 75 | mpl.rcParams.update({"figure.figsize": self.figsize()}) 76 | 77 | @staticmethod 78 | def savefig(filename): 79 | """ 80 | Save figure to PGF. PDF copy created for viewing convenience. 81 | Parameters 82 | ---------- 83 | filename 84 | Returns 85 | ------- 86 | """ 87 | plt.savefig('{}.pgf'.format(filename)) 88 | plt.savefig('{}.pdf'.format(filename)) 89 | 90 | 91 | import matplotlib.pyplot as plt 92 | -------------------------------------------------------------------------------- /research/gpq/icinco_demo.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import time 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import pandas as pd 7 | from numpy import newaxis as na 8 | from tqdm import trange 9 | 10 | from ssmtoybox.ssinf import CubatureKalman, UnscentedKalman, GaussHermiteKalman, GaussianProcessKalman 11 | from ssmtoybox.ssmod import UNGMTransition, UNGMMeasurement 12 | from ssmtoybox.utils import bootstrap_var, squared_error, neg_log_likelihood, log_cred_ratio, mse_matrix, GaussRV 13 | 14 | np.random.seed(42) # ensure reproducibility of results 15 | 16 | 17 | def evaluate_performance(x, mean_f, cov_f, mean_s, cov_s, bootstrap_variance=True): 18 | num_dim, num_step, num_sim, num_alg = mean_f.shape 19 | 20 | # simulation-average of time-averaged RMSE 21 | print('RMSE...') 22 | rmseData_f = np.sqrt(np.mean(squared_error(x[..., na], mean_f), axis=1)) 23 | rmseData_s = np.sqrt(np.mean(squared_error(x[..., na], mean_s), axis=1)) 24 | rmseMean_f = rmseData_f.mean(axis=1).T 25 | rmseMean_s = rmseData_s.mean(axis=1).T 26 | 27 | print('NLL and NCI...') 28 | nllData_f = np.zeros((1, num_step, num_sim, num_alg)) 29 | nciData_f = nllData_f.copy() 30 | nllData_s = nllData_f.copy() 31 | nciData_s = nllData_f.copy() 32 | for k in range(1, num_step): 33 | for fi in range(num_alg): 34 | mse_mat_f = mse_matrix(x[:, k, :], mean_f[:, k, :, fi]) 35 | mse_mat_s = mse_matrix(x[:, k, :], mean_f[:, k, :, fi]) 36 | for s in range(num_sim): 37 | # filter scores 38 | nllData_f[:, k, s, fi] = neg_log_likelihood(x[:, k, s], mean_f[:, k, s, fi], cov_f[:, :, k, s, fi]) 39 | nciData_f[:, k, s, fi] = log_cred_ratio(x[:, k, s], mean_f[:, k, s, fi], cov_f[:, :, k, s, fi], 40 | mse_mat_f) 41 | 42 | # smoother scores 43 | nllData_s[:, k, s, fi] = neg_log_likelihood(x[:, k, s], mean_s[:, k, s, fi], cov_s[:, :, k, s, fi]) 44 | nciData_s[:, k, s, fi] = log_cred_ratio(x[:, k, s], mean_s[:, k, s, fi], cov_s[:, :, k, s, fi], 45 | mse_mat_s) 46 | 47 | nciData_f, nciData_s = nciData_f.mean(axis=1), nciData_s.mean(axis=1) 48 | nllData_f, nllData_s = nllData_f.mean(axis=1), nllData_s.mean(axis=1) 49 | 50 | # average scores (over time and MC simulations) 51 | nciMean_f, nciMean_s = nciData_f.mean(axis=1).T, nciData_s.mean(axis=1).T 52 | nllMean_f, nllMean_s = nllData_f.mean(axis=1).T, nllData_s.mean(axis=1).T 53 | 54 | if bootstrap_variance: 55 | print('Bootstrapping variance ...') 56 | num_bs_samples = 10000 57 | rmseStd_f, rmseStd_s = np.zeros((num_alg, 1)), np.zeros((num_alg, 1)) 58 | nciStd_f, nciStd_s = rmseStd_f.copy(), rmseStd_f.copy() 59 | nllStd_f, nllStd_s = rmseStd_f.copy(), rmseStd_f.copy() 60 | for f in range(num_alg): 61 | rmseStd_f[f] = 2 * np.sqrt(bootstrap_var(rmseData_f[..., f], num_bs_samples)) 62 | rmseStd_s[f] = 2 * np.sqrt(bootstrap_var(rmseData_s[..., f], num_bs_samples)) 63 | nciStd_f[f] = 2 * np.sqrt(bootstrap_var(nciData_f[..., f], num_bs_samples)) 64 | nciStd_s[f] = 2 * np.sqrt(bootstrap_var(nciData_s[..., f], num_bs_samples)) 65 | nllStd_f[f] = 2 * np.sqrt(bootstrap_var(nllData_f[..., f], num_bs_samples)) 66 | nllStd_s[f] = 2 * np.sqrt(bootstrap_var(nllData_s[..., f], num_bs_samples)) 67 | 68 | return rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s, \ 69 | rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s 70 | else: 71 | return rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s 72 | 73 | 74 | def print_table(data, row_labels=None, col_labels=None, latex=False): 75 | pd.DataFrame(data, index=row_labels, columns=col_labels) 76 | print(pd) 77 | if latex: 78 | pd.to_latex() 79 | 80 | 81 | def tables(steps=500, sims=100): 82 | # setup univariate non-stationary growth model 83 | x0 = GaussRV(1, cov=np.atleast_2d(5.0)) 84 | q = GaussRV(1, cov=np.atleast_2d(10.0)) 85 | dyn = UNGMTransition(x0, q) # dynamics 86 | r = GaussRV(1) 87 | obs = UNGMMeasurement(r, 1) # observation model 88 | x = dyn.simulate_discrete(steps, mc_sims=sims) # generate some data 89 | z = obs.simulate_measurements(x) 90 | 91 | kern_par_sr = np.array([[1.0, 0.3 * dyn.dim_in]]) 92 | kern_par_ut = np.array([[1.0, 3.0 * dyn.dim_in]]) 93 | kern_par_gh = np.array([[1.0, 0.1 * dyn.dim_in]]) 94 | 95 | # initialize filters/smoothers 96 | algorithms = ( 97 | CubatureKalman(dyn, obs), 98 | UnscentedKalman(dyn, obs), 99 | GaussHermiteKalman(dyn, obs), 100 | GaussHermiteKalman(dyn, obs), 101 | GaussHermiteKalman(dyn, obs), 102 | GaussHermiteKalman(dyn, obs), 103 | GaussHermiteKalman(dyn, obs), 104 | GaussianProcessKalman(dyn, obs, kern_par_sr, kern_par_sr, points='sr'), 105 | GaussianProcessKalman(dyn, obs, kern_par_ut, kern_par_ut, points='ut'), 106 | GaussianProcessKalman(dyn, obs, kern_par_sr, kern_par_sr, points='gh', point_hyp={'degree': 5}), 107 | GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 7}), 108 | GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 10}), 109 | GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 15}), 110 | GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 20}), 111 | ) 112 | num_algs = len(algorithms) 113 | 114 | # space for estimates 115 | mean_f, cov_f = np.zeros((dyn.dim_in, steps, sims, num_algs)), np.zeros((dyn.dim_in, dyn.dim_in, steps, sims, num_algs)) 116 | mean_s, cov_s = np.zeros((dyn.dim_in, steps, sims, num_algs)), np.zeros((dyn.dim_in, dyn.dim_in, steps, sims, num_algs)) 117 | # do filtering/smoothing 118 | t0 = time.time() # measure execution time 119 | print('Running filters/smoothers ...', flush=True) 120 | for a, alg in enumerate(algorithms): 121 | for sim in trange(sims, desc='{:25}'.format(alg.__class__.__name__), file=sys.stdout): 122 | mean_f[..., sim, a], cov_f[..., sim, a] = alg.forward_pass(z[..., sim]) 123 | mean_s[..., sim, a], cov_s[..., sim, a] = alg.backward_pass() 124 | alg.reset() 125 | print('Done in {0:.4f} [sec]'.format(time.time() - t0)) 126 | 127 | # evaluate perfomance 128 | scores = evaluate_performance(x, mean_f, cov_f, mean_s, cov_s) 129 | rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s = scores[:6] 130 | rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s = scores[6:] 131 | 132 | # put data into Pandas DataFrame for fancy printing and latex export 133 | row_labels = ['SR', 'UT', 'GH-5', 'GH-7', 'GH-10', 'GH-15', 134 | 'GH-20'] # [alg.__class__.__name__ for alg in algorithms] 135 | col_labels = ['Classical', 'Bayesian', 'Classical (2std)', 'Bayesian (2std)'] 136 | rmse_table_f = pd.DataFrame(np.hstack((rmseMean_f.reshape(2, 7).T, rmseStd_f.reshape(2, 7).T)), 137 | index=row_labels, columns=col_labels) 138 | nci_table_f = pd.DataFrame(np.hstack((nciMean_f.reshape(2, 7).T, nciStd_f.reshape(2, 7).T)), 139 | index=row_labels, columns=col_labels) 140 | nll_table_f = pd.DataFrame(np.hstack((nllMean_f.reshape(2, 7).T, nllStd_f.reshape(2, 7).T)), 141 | index=row_labels, columns=col_labels) 142 | rmse_table_s = pd.DataFrame(np.hstack((rmseMean_s.reshape(2, 7).T, rmseStd_s.reshape(2, 7).T)), 143 | index=row_labels, columns=col_labels) 144 | nci_table_s = pd.DataFrame(np.hstack((nciMean_s.reshape(2, 7).T, nciStd_s.reshape(2, 7).T)), 145 | index=row_labels, columns=col_labels) 146 | nll_table_s = pd.DataFrame(np.hstack((nllMean_s.reshape(2, 7).T, nllStd_s.reshape(2, 7).T)), 147 | index=row_labels, columns=col_labels) 148 | # print tables 149 | print('Filter RMSE') 150 | print(rmse_table_f) 151 | print('Filter NCI') 152 | print(nci_table_f) 153 | print('Filter NLL') 154 | print(nll_table_f) 155 | print('Smoother RMSE') 156 | print(rmse_table_s) 157 | print('Smoother NCI') 158 | print(nci_table_s) 159 | print('Smoother NLL') 160 | print(nll_table_s) 161 | # return computed metrics for filters and smoothers 162 | return {'filter_RMSE': rmse_table_f, 'filter_NCI': nci_table_f, 'filter_NLL': nll_table_f, 163 | 'smoother_RMSE': rmse_table_s, 'smoother_NCI': nci_table_s, 'smoother_NLL': nll_table_s} 164 | 165 | 166 | def hypers_demo(lscale=None): 167 | 168 | print(f"Seed = {np.random.get_state()[1][0]}") 169 | 170 | # set default lengthscales if unspecified 171 | if lscale is None: 172 | lscale = [1e-3, 3e-3, 1e-2, 3e-2, 1e-1, 3e-1, 1, 3, 1e1, 3e1, 1e2, ] 173 | 174 | steps, mc = 500, 100 175 | 176 | # setup univariate non-stationary growth model 177 | x0 = GaussRV(1, cov=np.atleast_2d(5.0)) 178 | q = GaussRV(1, cov=np.atleast_2d(10.0)) 179 | dyn = UNGMTransition(x0, q) # dynamics 180 | r = GaussRV(1) 181 | obs = UNGMMeasurement(r, 1) # observation model 182 | x = dyn.simulate_discrete(steps, mc_sims=mc) # generate some data 183 | z = obs.simulate_measurements(x) 184 | 185 | num_el = len(lscale) 186 | mean_f, cov_f = np.zeros((dyn.dim_in, steps, mc, num_el)), np.zeros((dyn.dim_in, dyn.dim_in, steps, mc, num_el)) 187 | for iel, el in enumerate(lscale): 188 | 189 | # kernel parameters 190 | ker_par = np.array([[1.0, el * dyn.dim_in]]) 191 | 192 | # initialize BHKF with current lenghtscale 193 | f = GaussianProcessKalman(dyn, obs, ker_par, ker_par, points='ut', point_hyp={'kappa': 0.0}) 194 | # filtering 195 | for s in range(mc): 196 | mean_f[..., s, iel], cov_f[..., s, iel] = f.forward_pass(z[..., s]) 197 | 198 | # evaluate RMSE, NCI and NLL 199 | rmseVsEl = squared_error(x[..., na], mean_f) 200 | nciVsEl = rmseVsEl.copy() 201 | nllVsEl = rmseVsEl.copy() 202 | for k in range(steps): 203 | for iel in range(num_el): 204 | mse_mat = mse_matrix(x[:, k, :], mean_f[:, k, :, iel]) 205 | for s in range(mc): 206 | nciVsEl[:, k, s, iel] = log_cred_ratio(x[:, k, s], mean_f[:, k, s, iel], cov_f[:, :, k, s, iel], 207 | mse_mat) 208 | nllVsEl[:, k, s, iel] = neg_log_likelihood(x[:, k, s], mean_f[:, k, s, iel], cov_f[:, :, k, s, iel]) 209 | 210 | # average out time and MC simulations 211 | rmseVsEl = np.sqrt(np.mean(rmseVsEl, axis=1)).mean(axis=1) 212 | nciVsEl = nciVsEl.mean(axis=(1, 2)) 213 | nllVsEl = nllVsEl.mean(axis=(1, 2)) 214 | 215 | # plot influence of changing lengthscale on the RMSE and NCI and NLL filter performance 216 | plt.figure() 217 | plt.semilogx(lscale, rmseVsEl.squeeze(), color='k', ls='-', lw=2, marker='o', label='RMSE') 218 | plt.semilogx(lscale, nciVsEl.squeeze(), color='k', ls='--', lw=2, marker='o', label='NCI') 219 | plt.semilogx(lscale, nllVsEl.squeeze(), color='k', ls='-.', lw=2, marker='o', label='NLL') 220 | plt.grid(True) 221 | plt.legend() 222 | plt.show() 223 | 224 | return {'el': lscale, 'rmse': rmseVsEl, 'nci': nciVsEl, 'neg_log_likelihood': nllVsEl} 225 | 226 | 227 | if __name__ == '__main__': 228 | tables_dict = tables(steps=500, sims=100) 229 | # plot_data = hypers_demo() 230 | -------------------------------------------------------------------------------- /research/gpq/journal_figure.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib as mpl 3 | mpl.use('pgf') 4 | 5 | # TODO: turn into a class, so that all the properties coould be set manually, multiple instances means multiple setups 6 | 7 | 8 | class FigurePrint: 9 | 10 | INCH_PER_PT = 1.0 / 72.27 # Convert pt to inch 11 | PHI = (np.sqrt(5.0) - 1.0) / 2.0 # Aesthetic ratio (you could change this) 12 | 13 | def __init__(self, fig_width_pt=252): 14 | """ 15 | 16 | Parameters 17 | ---------- 18 | fig_width_pt : float 19 | Width of the figure in points, usually obtained from the journal specs or using the LaTeX command 20 | ``\the\columnwidth``. Default is ``fig_width_pt=252`` (3.5 inches). 21 | """ 22 | self.fig_width_pt = fig_width_pt 23 | pgf_with_latex = { # setup matplotlib to use latex for output 24 | "pgf.texsystem": "pdflatex", # change this if using xetex or lautex 25 | "text.usetex": True, # use LaTeX to write all text 26 | "font.family": "serif", 27 | "font.serif": [], # blank entries should cause plots to inherit fonts from the document 28 | "font.sans-serif": [], 29 | "font.monospace": [], 30 | "font.size": 10, 31 | "axes.labelsize": 10, # LaTeX default is 10pt font. 32 | "legend.fontsize": 8, # Make the legend/label fonts a little smaller 33 | "xtick.labelsize": 8, 34 | "ytick.labelsize": 8, 35 | # "axes.prop_cycle": ['#5DA5DA', '#FAA43A', '#60BD68', 36 | # '#F17CB0', '#B2912F', '#B276B2', 37 | # '#DECF3F', '#F15854', '#4D4D4D'], 38 | "figure.figsize": self.figsize(), # default fig size 39 | "pgf.preamble": [ # plots will be generated using this preamble 40 | r"\usepackage[utf8x]{inputenc}", # use utf8 fonts 41 | r"\usepackage[T1]{fontenc}", 42 | ] 43 | } 44 | mpl.rcParams.update(pgf_with_latex) 45 | 46 | def figsize(self, w_scale=1.0, h_scale=1.0): 47 | """ 48 | Calculates figure width and height given the width and height scale. 49 | 50 | Parameters 51 | ---------- 52 | w_scale: float 53 | Figure width scale. 54 | 55 | h_scale: float 56 | Figure height scale. 57 | 58 | Returns 59 | ------- 60 | list 61 | Figure width and height in inches. 62 | """ 63 | 64 | fig_width = self.fig_width_pt * self.INCH_PER_PT * w_scale # width in inches 65 | fig_height = fig_width * self.PHI * h_scale # height in inches 66 | return [fig_width, fig_height] 67 | 68 | def update_default_figsize(self, fig_width_pt): 69 | """ 70 | Updates default figure size used for saving. 71 | 72 | Parameters 73 | ---------- 74 | fig_width_pt : float 75 | Width of the figure in points, usually obtained from the journal specs or using the LaTeX command 76 | ``\the\columnwidth``. 77 | 78 | 79 | Returns 80 | ------- 81 | 82 | """ 83 | self.fig_width_pt = fig_width_pt 84 | mpl.rcParams.update({"figure.figsize": self.figsize()}) 85 | 86 | @staticmethod 87 | def savefig(filename): 88 | """ 89 | Save figure to PGF. PDF copy created for viewing convenience. 90 | 91 | Parameters 92 | ---------- 93 | filename 94 | 95 | Returns 96 | ------- 97 | 98 | """ 99 | plt.savefig('{}.pgf'.format(filename)) 100 | plt.savefig('{}.pdf'.format(filename)) 101 | 102 | import matplotlib.pyplot as plt 103 | 104 | -------------------------------------------------------------------------------- /research/gpq/polar2cartesian.py: -------------------------------------------------------------------------------- 1 | # import matplotlib as mpl 2 | # import matplotlib.pyplot as plt 3 | from journal_figure import * 4 | from ssmtoybox.bq.bqmtran import GaussianProcessTransform 5 | from ssmtoybox.mtran import MonteCarloTransform, SphericalRadialTransform, GaussHermiteTransform 6 | from ssmtoybox.utils import * 7 | import numpy.linalg as la 8 | from collections import OrderedDict 9 | 10 | 11 | """ 12 | Gaussian Process Quadrature moment transformation tested on a mapping from polar to cartesian coordinates. 13 | """ 14 | 15 | 16 | def no_par(f): 17 | def wrapper(x): 18 | return f(x, None) 19 | return wrapper 20 | 21 | 22 | def polar2cartesian(x, pars): 23 | return x[0] * np.array([np.cos(x[1]), np.sin(x[1])]) 24 | 25 | 26 | def cartesian2polar(x, pars): 27 | r = np.sqrt(x[0]**2 + x[1]**2) 28 | theta = np.arctan2(x[1], x[0]) 29 | return np.array([r, theta]) 30 | 31 | 32 | def gpq_polar2cartesian_demo(): 33 | dim = 2 34 | 35 | # Initialize transforms 36 | # high el[0], because the function is linear given x[1] 37 | kpar = np.array([[1.0, 600, 6]]) 38 | tf_gpq = GaussianProcessTransform(dim, 1, kpar, kern_str='rbf', point_str='sr') 39 | tf_sr = SphericalRadialTransform(dim) 40 | tf_mc = MonteCarloTransform(dim, n=1e4) # 10k samples 41 | 42 | # Input mean and covariance 43 | mean_in = np.array([1, np.pi / 2]) 44 | cov_in = np.diag([0.05 ** 2, (np.pi / 10) ** 2]) 45 | # mean_in = np.array([10, 0]) 46 | # cov_in = np.diag([0.5**2, (5*np.pi/180)**2]) 47 | 48 | # Mapped samples 49 | x = np.random.multivariate_normal(mean_in, cov_in, size=int(1e3)).T 50 | fx = np.apply_along_axis(polar2cartesian, 0, x, None) 51 | 52 | # MC transformed moments 53 | mean_mc, cov_mc, cc_mc = tf_mc.apply(polar2cartesian, mean_in, cov_in, None) 54 | ellipse_mc = ellipse_points(mean_mc, cov_mc) 55 | 56 | # GPQ transformed moments with ellipse points 57 | mean_gpq, cov_gpq, cc = tf_gpq.apply(polar2cartesian, mean_in, cov_in, None) 58 | ellipse_gpq = ellipse_points(mean_gpq, cov_gpq) 59 | 60 | # SR transformed moments with ellipse points 61 | mean_sr, cov_sr, cc = tf_sr.apply(polar2cartesian, mean_in, cov_in, None) 62 | ellipse_sr = ellipse_points(mean_sr, cov_sr) 63 | 64 | # Plots 65 | plt.figure() 66 | 67 | # MC ground truth mean w/ covariance ellipse 68 | plt.plot(mean_mc[0], mean_mc[1], 'ro', markersize=6, lw=2) 69 | plt.plot(ellipse_mc[0, :], ellipse_mc[1, :], 'r--', lw=2, label='MC') 70 | 71 | # GPQ transformed mean w/ covariance ellipse 72 | plt.plot(mean_gpq[0], mean_gpq[1], 'go', markersize=6) 73 | plt.plot(ellipse_gpq[0, :], ellipse_gpq[1, :], color='g', label='GPQ') 74 | 75 | # SR transformed mean w/ covariance ellipse 76 | plt.plot(mean_sr[0], mean_sr[1], 'bo', markersize=6) 77 | plt.plot(ellipse_sr[0, :], ellipse_sr[1, :], color='b', label='SR') 78 | 79 | # Transformed samples of the input random variable 80 | plt.plot(fx[0, :], fx[1, :], 'k.', alpha=0.15) 81 | plt.axes().set_aspect('equal') 82 | plt.legend() 83 | plt.show() 84 | 85 | np.set_printoptions(precision=2) 86 | print("GPQ") 87 | print("Mean weights: {}".format(tf_gpq.wm)) 88 | print("Cov weight matrix eigvals: {}".format(la.eigvals(tf_gpq.Wc))) 89 | print("Integral variance: {:.2e}".format(tf_gpq.model.integral_variance(None))) 90 | print("Expected model variance: {:.2e}".format(tf_gpq.model.exp_model_variance(None))) 91 | print("SKL Score:") 92 | print("SR: {:.2e}".format(symmetrized_kl_divergence(mean_mc, cov_mc, mean_sr, cov_sr))) 93 | print("GPQ: {:.2e}".format(symmetrized_kl_divergence(mean_mc, cov_mc, mean_gpq, cov_gpq))) 94 | 95 | 96 | def polar2cartesian_skl_demo(): 97 | num_dim = 2 98 | 99 | # create spiral in polar domain 100 | r_spiral = lambda x: 10 * x 101 | theta_min, theta_max = 0.25 * np.pi, 2.25 * np.pi 102 | 103 | # equidistant points on a spiral 104 | num_mean = 10 105 | theta_pt = np.linspace(theta_min, theta_max, num_mean) 106 | r_pt = r_spiral(theta_pt) 107 | 108 | # samples from normal RVs centered on the points of the spiral 109 | mean = np.array([r_pt, theta_pt]) 110 | r_std = 0.5 111 | 112 | # multiple azimuth covariances in increasing order 113 | num_cov = 10 114 | theta_std = np.deg2rad(np.linspace(6, 36, num_cov)) 115 | cov = np.zeros((num_dim, num_dim, num_cov)) 116 | for i in range(num_cov): 117 | cov[..., i] = np.diag([r_std**2, theta_std[i]**2]) 118 | 119 | # COMPARE moment transforms 120 | ker_par = np.array([[1.0, 60, 6]]) 121 | moment_tforms = OrderedDict([ 122 | ('gpq-sr', GaussianProcessTransform(num_dim, 1, ker_par, kern_str='rbf', point_str='sr')), 123 | ('sr', SphericalRadialTransform(num_dim)), 124 | ]) 125 | baseline_mtf = MonteCarloTransform(num_dim, n=10000) 126 | num_tforms = len(moment_tforms) 127 | 128 | # initialize storage of SKL scores 129 | skl_dict = dict([(mt_str, np.zeros((num_mean, num_cov))) for mt_str in moment_tforms.keys()]) 130 | 131 | # for each mean 132 | for i in range(num_mean): 133 | 134 | # for each covariance 135 | for j in range(num_cov): 136 | mean_in, cov_in = mean[..., i], cov[..., j] 137 | 138 | # calculate baseline using Monte Carlo 139 | mean_out_mc, cov_out_mc, cc = baseline_mtf.apply(polar2cartesian, mean_in, cov_in, None) 140 | 141 | # for each MT 142 | for mt_str in moment_tforms.keys(): 143 | 144 | # calculate the transformed moments 145 | mean_out, cov_out, cc = moment_tforms[mt_str].apply(polar2cartesian, mean_in, cov_in, None) 146 | 147 | # compute SKL 148 | skl_dict[mt_str][i, j] = symmetrized_kl_divergence(mean_out_mc, cov_out_mc, mean_out, cov_out) 149 | 150 | # PLOT the SKL score for each MT and position on the spiral 151 | plt.style.use('seaborn-deep') 152 | printfig = FigurePrint() 153 | fig = plt.figure() 154 | 155 | # Average over mean indexes 156 | ax1 = fig.add_subplot(121) 157 | index = np.arange(num_mean)+1 158 | for mt_str in moment_tforms.keys(): 159 | ax1.plot(index, skl_dict[mt_str].mean(axis=1), marker='o', label=mt_str.upper()) 160 | ax1.set_xlabel('Position index') 161 | ax1.set_ylabel('SKL') 162 | 163 | # Average over azimuth variances 164 | ax2 = fig.add_subplot(122, sharey=ax1) 165 | for mt_str in moment_tforms.keys(): 166 | ax2.plot(np.rad2deg(theta_std), skl_dict[mt_str].mean(axis=0), marker='o', label=mt_str.upper()) 167 | ax2.set_xlabel(r'Azimuth STD [$ \circ $]') 168 | ax2.legend() 169 | fig.tight_layout(pad=0) 170 | 171 | # save figure 172 | printfig.savefig('polar2cartesian_skl') 173 | 174 | 175 | def polar2cartesian_spiral_demo(): 176 | num_dim = 2 177 | 178 | # create spiral in polar domain 179 | r_spiral = lambda x: 10 * x 180 | theta_min, theta_max = 0.25 * np.pi, 2.25 * np.pi 181 | theta = np.linspace(theta_min, theta_max, 100) 182 | r = r_spiral(theta) 183 | 184 | # equidistant points on a spiral 185 | num_mean = 10 186 | theta_pt = np.linspace(theta_min, theta_max, num_mean) 187 | r_pt = r_spiral(theta_pt) 188 | 189 | # samples from normal RVs centered on the points of the spiral 190 | mean = np.array([r_pt, theta_pt]) 191 | r_std = 0.5 192 | 193 | # multiple azimuth covariances in increasing order 194 | num_cov = 10 195 | theta_std = np.deg2rad(np.linspace(6, 36, num_cov)) 196 | cov = np.zeros((num_dim, num_dim, num_cov)) 197 | for i in range(num_cov): 198 | cov[..., i] = np.diag([r_std ** 2, theta_std[i] ** 2]) 199 | 200 | pol_spiral = np.array([r, theta]) 201 | pol_spiral_pt = np.array([r_pt, theta_pt]) 202 | 203 | printfig = FigurePrint() 204 | fig = plt.figure() 205 | # PLOTS: Input moments in polar coordinates 206 | # ax = fig.add_subplot(121, projection='polar') 207 | # # ax.set_aspect('equal') 208 | # 209 | # # origin 210 | # ax.plot(0, 0, 'r+', ms=4) 211 | # 212 | # # spiral 213 | # ax.plot(pol_spiral[1, :], pol_spiral[0, :], color='r', lw=0.5, ls='--', alpha=0.5) 214 | # 215 | # # points on a spiral, i.e. input means 216 | # ax.plot(pol_spiral_pt[1, :], pol_spiral_pt[0, :], 'o', color='k', ms=1) 217 | # 218 | # # for every input mean and covariance 219 | # for i in range(num_mean): 220 | # for j in range(num_cov): 221 | # 222 | # # plot covariance ellipse 223 | # car_ellipse = ellipse_points(mean[..., i], cov[..., 5]) 224 | # ax.plot(car_ellipse[1, :], car_ellipse[0, :], color='k', lw=0.5) 225 | 226 | # transform spiral to Cartesian coordinates 227 | car_spiral = np.apply_along_axis(polar2cartesian, 0, pol_spiral, None) 228 | car_spiral_pt = np.apply_along_axis(polar2cartesian, 0, pol_spiral_pt, None) 229 | 230 | # PLOTS: Transformed moments in Cartesian coordinates 231 | ax = fig.add_subplot(111, projection='polar') 232 | ax.set_aspect('equal') 233 | 234 | # origin 235 | ax.plot(0, 0, 'r+', ms=10) 236 | 237 | # spiral 238 | # ax.plot(car_spiral[0, :], car_spiral[1, :], color='r', lw=0.5, ls='--', alpha=0.5) 239 | 240 | # points on a spiral, i.e. input means 241 | ax.plot(pol_spiral_pt[0, :], pol_spiral_pt[1, :], 'o', color='k', ms=4) 242 | ax.text(pol_spiral_pt[0, 5]-0.15, pol_spiral_pt[1, 5]+1.25, r'$[r_i, \theta_i]$') 243 | rgr = [2, 4, 6] 244 | plt.rgrids(rgr, [str(r) for r in rgr]) 245 | ax.grid(True, linestyle=':', lw=1, alpha=0.5) 246 | 247 | # for every input mean and covariance 248 | # for i in range(num_mean): 249 | # for j in range(num_cov): 250 | # 251 | # # plot covariance ellipse 252 | # car_ellipse = np.apply_along_axis(polar2cartesian, 0, ellipse_points(mean[..., i], cov[..., 5]), None) 253 | # # car_ellipse = ellipse_points(mean[..., i], cov[..., -1]) 254 | # ax.plot(car_ellipse[0, :], car_ellipse[1, :], color='k', lw=0.5) 255 | 256 | fig.tight_layout(pad=0.1) 257 | 258 | printfig.savefig('polar2cartesian_spiral') 259 | 260 | 261 | if __name__ == '__main__': 262 | polar2cartesian_skl_demo() 263 | polar2cartesian_spiral_demo() 264 | -------------------------------------------------------------------------------- /research/gpqd/gpqd_base.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy import newaxis as na, linalg as la 3 | from scipy.linalg import cho_solve, cho_factor 4 | 5 | from ssmtoybox.bq.bqkern import RBFGauss 6 | from ssmtoybox.bq.bqmod import GaussianProcessModel 7 | from ssmtoybox.bq.bqmtran import BQTransform 8 | from ssmtoybox.utils import maha 9 | 10 | 11 | class GaussianProcessDerTransform(BQTransform): 12 | 13 | def __init__(self, dim_in, dim_out, kern_par, 14 | point_str='ut', point_par=None, estimate_par=False, which_der=None): 15 | self.model = GaussianProcessDerModel(dim_in, kern_par, point_str, point_par, estimate_par, which_der) 16 | self.I_out = np.eye(dim_out) # pre-allocation for later computations 17 | # BQ transform weights for the mean, covariance and cross-covariance 18 | self.wm, self.Wc, self.Wcc = self.weights(kern_par) 19 | 20 | def _fcn_eval(self, fcn, x, fcn_par): 21 | """ 22 | Evaluations of the integrand, which can comprise function observations as well as derivative observations. 23 | 24 | Parameters 25 | ---------- 26 | fcn : func 27 | Integrand as a function handle, which is expected to behave certain way. 28 | 29 | x : ndarray 30 | Argument (input) of the integrand. 31 | 32 | fcn_par : 33 | Parameters of the integrand. 34 | 35 | Returns 36 | ------- 37 | : ndarray 38 | Function evaluations of shape (out_dim, num_pts). 39 | 40 | Notes 41 | ----- 42 | Methods in derived subclasses decides whether to return derivatives also 43 | """ 44 | # should return as many columns as output dims, one column includes function and derivative evaluations 45 | # for every sigma-point, thus it is (n + n*d,); n = # sigma-points, d = sigma-point dimensionality 46 | # returned array should be (n + n*d, e); e = output dimensionality 47 | # evaluate function at sigmas (e, n) 48 | fx = np.apply_along_axis(fcn, 0, x, fcn_par) 49 | # Jacobians evaluated only at sigmas specified by which_der array (e * d, n) 50 | dfx = np.apply_along_axis(fcn, 0, x[:, self.model.which_der], fcn_par, dx=True) 51 | # stack function values and derivative values into one column 52 | return np.vstack((fx.T, dfx.T.reshape(self.model.dim_in * len(self.model.which_der), -1))).T 53 | 54 | 55 | class GaussianProcessDerModel(GaussianProcessModel): 56 | """Gaussian Process Model with Derivative Observations""" 57 | 58 | _supported_kernels_ = ['rbf-d'] 59 | 60 | def __init__(self, dim, kern_par, point_str, point_par=None, estimate_par=False, which_der=None): 61 | super(GaussianProcessDerModel, self).__init__(dim, kern_par, 'rbf', point_str, point_par, estimate_par) 62 | self.kernel = RBFGaussDer(dim, kern_par) 63 | # assume derivatives evaluated at all sigmas if unspecified 64 | self.which_der = which_der if which_der is not None else np.arange(self.num_pts) 65 | 66 | def bq_weights(self, par, *args): 67 | par = self.kernel.get_parameters(par) 68 | x = self.points 69 | 70 | # inverse kernel matrix 71 | iK = self.kernel.eval_inv_dot(par, x, scaling=False) 72 | 73 | # kernel expectations 74 | q = self.kernel.exp_x_kx(par, x) 75 | Q = self.kernel.exp_x_kxkx(par, par, x) 76 | R = self.kernel.exp_x_xkx(par, x) 77 | 78 | # derivative kernel expectations 79 | qd = self.kernel.exp_x_dkx(par, x, which_der=self.which_der) 80 | Qfd = self.kernel.exp_x_kxdkx(par, x) 81 | Qdd = self.kernel.exp_x_dkxdkx(par, x) 82 | Rd = self.kernel.exp_x_xdkx(par, x) 83 | 84 | # form the "joint" (function value and derivative) kernel expectations 85 | q_tilde = np.hstack((q.T, qd.T.ravel())) 86 | Q_tilde = np.vstack((np.hstack((Q, Qfd)), np.hstack((Qfd.T, Qdd)))) 87 | R_tilde = np.hstack((R, Rd)) 88 | 89 | # BQ weights in terms of kernel expectations 90 | w_m = q_tilde.dot(iK) 91 | w_c = iK.dot(Q_tilde).dot(iK) 92 | w_cc = R_tilde.dot(iK) 93 | 94 | # save the kernel expectations for later 95 | self.q, self.Q, self.iK = q_tilde, Q_tilde, iK 96 | # expected model variance 97 | self.model_var = self.kernel.exp_x_kxx(par) * (1 - np.trace(Q_tilde.dot(iK))) 98 | # integral variance 99 | self.integral_var = self.kernel.exp_xy_kxy(par) - q_tilde.T.dot(iK).dot(q_tilde) 100 | 101 | # covariance weights should be symmetric 102 | if not np.array_equal(w_c, w_c.T): 103 | w_c = 0.5 * (w_c + w_c.T) 104 | 105 | return w_m, w_c, w_cc, self.model_var, self.integral_var 106 | 107 | def exp_model_variance(self, par, *args): 108 | iK = self.kernel.eval_inv_dot(par, self.points) 109 | 110 | Q = self.kernel.exp_x_kxkx(par, par, self.points) 111 | Qfd = self.kernel.exp_x_kxdkx(par, par, self.points) 112 | Qdd = self.kernel.exp_x_dkxdkx(par, par, self.points) 113 | Q_tilde = np.vstack((np.hstack((Q, Qfd)), np.hstack((Qfd.T, Qdd)))) 114 | 115 | return self.kernel.exp_x_kxx(par) * (1 - np.trace(Q_tilde.dot(iK))) 116 | 117 | def integral_variance(self, par, *args): 118 | par = self.kernel.get_parameters(par) # if par None returns default kernel parameters 119 | 120 | q = self.kernel.exp_x_kx(par, self.points) 121 | qd = self.kernel.exp_x_dkx(par, self.points) 122 | q_tilde = np.hstack((q.T, qd.T.ravel())) 123 | 124 | iK = self.kernel.eval_inv_dot(par, self.points, scaling=False) 125 | kbar = self.kernel.exp_xy_kxy(par) 126 | return kbar - q_tilde.T.dot(iK).dot(q_tilde) 127 | 128 | 129 | class RBFGaussDer(RBFGauss): 130 | """RBF kernel "with derivatives". Kernel expectations are w.r.t. Gaussian density.""" 131 | 132 | def __init__(self, dim, par, jitter=1e-8): 133 | super(RBFGaussDer, self).__init__(dim, par, jitter) 134 | 135 | def eval(self, par, x1, x2=None, diag=False, scaling=True, which_der=None): 136 | 137 | if x2 is None: 138 | x2 = x1.copy() 139 | 140 | alpha, sqrt_inv_lam = RBFGauss._unpack_parameters(par) 141 | alpha = 1.0 if not scaling else alpha 142 | 143 | x1 = sqrt_inv_lam.dot(x1) # sqrt(Lam^-1) * x 144 | x2 = sqrt_inv_lam.dot(x2) 145 | if diag: # only diagonal of kernel matrix 146 | assert x1.shape == x2.shape 147 | dx = x1 - x2 148 | Kff = np.exp(2 * np.log(alpha) - 0.5 * np.sum(dx * dx, axis=0)) 149 | else: 150 | Kff = np.exp(2 * np.log(alpha) - 0.5 * maha(x1.T, x2.T)) 151 | 152 | x1, x2 = np.atleast_2d(x1), np.atleast_2d(x2) 153 | D, N = x1.shape 154 | Ds, Ns = x2.shape 155 | assert Ds == D 156 | which_der = np.arange(N) if which_der is None else which_der 157 | Nd = len(which_der) # points w/ derivative observations 158 | # iLam = np.diag(el ** -1 * np.ones(D)) # sqrt(Lam^-1) 159 | # iiLam = np.diag(el ** -2 * np.ones(D)) # Lam^-1 160 | 161 | # x1 = iLam.dot(x1) # sqrt(Lambda^-1) * X 162 | # x2 = iLam.dot(x2) 163 | # Kff = np.exp(2 * np.log(alpha) - 0.5 * maha(x2.T, x1.T)) # cov(f(xi), f(xj)) 164 | x1 = sqrt_inv_lam.dot(x1) # Lambda^-1 * X 165 | x2 = sqrt_inv_lam.dot(x2) 166 | inv_lam = sqrt_inv_lam ** 2 167 | XmX = x2[..., na] - x1[:, na, :] # pair-wise differences 168 | 169 | # NOTE: benchmark vs. np.kron(), replace with np.kron() if possible, but which_der complicates the matter 170 | Kfd = np.zeros((Ns, D * Nd)) # cov(f(xi), df(xj)) 171 | for i in range(Ns): 172 | for j in range(Nd): 173 | jstart, jend = j * D, j * D + D 174 | j_d = which_der[j] 175 | Kfd[i, jstart:jend] = Kff[i, j_d] * XmX[:, i, j_d] 176 | 177 | Kdd = np.zeros((D * Nd, D * Nd)) # cov(df(xi), df(xj)) 178 | for i in range(Nd): 179 | for j in range(Nd): 180 | istart, iend = i * D, i * D + D 181 | jstart, jend = j * D, j * D + D 182 | i_d, j_d = which_der[i], which_der[j] # indices of points with derivatives 183 | Kdd[istart:iend, jstart:jend] = Kff[i_d, j_d] * (inv_lam - np.outer(XmX[:, i_d, j_d], XmX[:, i_d, j_d])) 184 | if Ns == N: 185 | return np.vstack((np.hstack((Kff, Kfd)), np.hstack((Kfd.T, Kdd)))) 186 | else: 187 | return np.hstack((Kff, Kfd)) 188 | 189 | def eval_inv_dot(self, par, x, b=None, scaling=True, which_der=None): 190 | """ 191 | Compute the product of kernel matrix inverse and a vector `b`. 192 | 193 | Parameters 194 | ---------- 195 | par : ndarray 196 | Kernel parameters. 197 | 198 | x : ndarray 199 | Data set. 200 | 201 | b : None or ndarray, optional 202 | If `None`, inverse kernel matrix is computed (i.e. `b=np.eye(N)`). 203 | 204 | scaling : bool, optional 205 | Use scaling parameter of the kernel matrix. 206 | 207 | which_der : ndarray 208 | Indicates for which points are the derivatives available. 209 | 210 | Returns 211 | ------- 212 | : (N, N) ndarray 213 | Product of kernel matrix inverse and vector `b`. 214 | """ 215 | # if b=None returns inverse of K 216 | dim, num_pts = x.shape 217 | which_der = np.arange(num_pts) if which_der is None else which_der 218 | num_der = len(which_der) # number of points with derivatives 219 | K = self.eval(par, x, scaling=scaling, which_der=which_der) 220 | return self._cho_inv(K + self.jitter * np.eye(num_pts + num_der*dim), b) 221 | 222 | def eval_chol(self, par, x, scaling=True, which_der=None): 223 | """ 224 | Compute of Cholesky factor of the kernel matrix. 225 | 226 | Parameters 227 | ---------- 228 | par : (dim+1, ) ndarray 229 | Kernel parameters. 230 | 231 | x : (dim, N) ndarray 232 | Data set. 233 | 234 | scaling : bool, optional 235 | Use scaling parameter of the kernel. 236 | 237 | which_der : ndarray 238 | Indicates for which points are the derivatives available. 239 | 240 | Returns 241 | ------- 242 | : (N, N) ndarray 243 | Cholesky factor of the kernel matrix. 244 | """ 245 | dim, num_pts = x.shape 246 | which_der = np.arange(num_pts) if which_der is None else which_der 247 | num_der = len(which_der) # number of points with derivatives 248 | K = self.eval(par, x, scaling=scaling, which_der=which_der) 249 | return la.cholesky(K + self.jitter * np.eye(num_pts + num_der*dim)) 250 | 251 | def exp_x_dkx(self, par, x, scaling=False, which_der=None): 252 | """Expectation E_x[k_fd(x, x_n)]""" 253 | 254 | dim, num_pts = x.shape 255 | alpha, sqrt_inv_lam = RBFGauss._unpack_parameters(par) 256 | # alpha = 1.0 if not scaling else alpha 257 | inv_lam = sqrt_inv_lam ** 2 258 | lam = np.diag(inv_lam.diagonal() ** -1) 259 | which_der = np.arange(num_pts) if which_der is None else which_der 260 | 261 | q = self.exp_x_kx(par, x, scaling) # kernel mean E_x[k_ff(x, x_n)] 262 | 263 | eye_d = np.eye(dim) 264 | Sig_q = cho_solve(cho_factor(inv_lam + eye_d), eye_d) # B^-1*I 265 | eta = Sig_q.dot(x) # (D,N) Sig_q*x 266 | mu_q = inv_lam.dot(eta) # (D,N) 267 | r = q[na, which_der] * inv_lam.dot(mu_q[:, which_der] - x[:, which_der]) # -t.dot(iLam) * q # (D, N) 268 | 269 | return r.T.ravel() # (1, n_der*D) 270 | 271 | def exp_x_xdkx(self, par, x, scaling=False, which_der=None): 272 | """Expectation E_x[x k_fd(x, x_m)]""" 273 | dim, num_pts = x.shape 274 | which_der = np.arange(num_pts) if which_der is None else which_der 275 | num_der = len(which_der) 276 | _, sqrt_inv_lam = RBFGauss._unpack_parameters(par) 277 | 278 | inv_lam = sqrt_inv_lam ** 2 279 | eye_d = np.eye(dim) 280 | 281 | q = self.exp_x_kx(par, x, scaling) 282 | Sig_q = cho_solve(cho_factor(inv_lam + eye_d), eye_d) # B^-1*I 283 | eta = Sig_q.dot(x) # (D,N) Sig_q*x 284 | mu_q = inv_lam.dot(eta) # (D,N) 285 | r = q[na, which_der] * inv_lam.dot(mu_q[:, which_der] - x[:, which_der]) # -t.dot(iLam) * q # (D, N) 286 | 287 | # quantities for cross-covariance "weights" 288 | iLamSig = inv_lam.dot(Sig_q) # (D,D) 289 | r_tilde = np.empty((dim, num_der * dim)) 290 | for i in range(num_der): 291 | i_d = which_der[i] 292 | r_tilde[:, i * dim:i * dim + dim] = q[i_d] * iLamSig + np.outer(mu_q[:, i_d], r[:, i].T) 293 | 294 | return r_tilde # (dim, num_pts*dim) 295 | 296 | def exp_x_kxdkx(self, par, x, scaling=False, which_der=None): 297 | """Expectation E_x[k_ff(x_n, x) k_fd(x, x_m)]""" 298 | dim, num_pts = x.shape 299 | which_der = np.arange(num_pts) if which_der is None else which_der 300 | num_der = len(which_der) 301 | 302 | _, sqrt_inv_lam = RBFGauss._unpack_parameters(par) 303 | inv_lam = sqrt_inv_lam ** 2 304 | lam = np.diag(inv_lam.diagonal() ** -1) 305 | eye_d = np.eye(dim) 306 | 307 | # quantities for covariance weights 308 | Sig_q = cho_solve(cho_factor(inv_lam + eye_d), eye_d) # B^-1*I 309 | eta = Sig_q.dot(x) # (D,N) Sig_q*x 310 | inn = inv_lam.dot(x) # inp / el[:, na]**2 311 | Q = self.exp_x_kxkx(par, par, x, scaling) # (N,N) 312 | 313 | cho_LamSig = cho_factor(lam + Sig_q) 314 | eta_tilde = inv_lam.dot(cho_solve(cho_LamSig, eta)) # Lambda^-1(Lambda+Sig_q)^-1*eta 315 | mu_Q = eta_tilde[..., na] + eta_tilde[:, na, :] # (D,N_der,N) pairwise sum of pre-multiplied eta's 316 | 317 | E_dfff = np.empty((num_der * dim, num_pts)) 318 | for i in range(num_der): 319 | for j in range(num_pts): 320 | istart, iend = i * dim, i * dim + dim 321 | i_d = which_der[i] 322 | E_dfff[istart:iend, j] = Q[i_d, j] * (mu_Q[:, i_d, j] - inn[:, i_d]) 323 | 324 | return E_dfff.T # (num_pts, num_der*dim) 325 | 326 | def exp_x_dkxdkx(self, par, x, scaling=False, which_der=None): 327 | """Expectation E_x[k_df(x_n, x) k_fd(x, x_m)]""" 328 | dim, num_pts = x.shape 329 | which_der = np.arange(num_pts) if which_der is None else which_der 330 | num_der = len(which_der) 331 | 332 | _, sqrt_inv_lam = RBFGauss._unpack_parameters(par) 333 | inv_lam = sqrt_inv_lam ** 2 334 | lam = np.diag(inv_lam.diagonal() ** -1) 335 | eye_d = np.eye(dim) 336 | 337 | # quantities for covariance weights 338 | Sig_q = cho_solve(cho_factor(inv_lam + eye_d), eye_d) # B^-1*I 339 | eta = Sig_q.dot(x) # (D,N) Sig_q*x 340 | inn = inv_lam.dot(x) # inp / el[:, na]**2 341 | Q = self.exp_x_kxkx(par, par, x, scaling) # (N,N) 342 | 343 | cho_LamSig = cho_factor(lam + Sig_q) 344 | Sig_Q = cho_solve(cho_LamSig, Sig_q).dot(inv_lam) # (D,D) Lambda^-1 (Lambda*(Lambda+Sig_q)^-1*Sig_q) Lambda^-1 345 | eta_tilde = inv_lam.dot(cho_solve(cho_LamSig, eta)) # Lambda^-1(Lambda+Sig_q)^-1*eta 346 | mu_Q = eta_tilde[..., na] + eta_tilde[:, na, :] # (D,N_der,N) pairwise sum of pre-multiplied eta's 347 | 348 | E_dffd = np.empty((num_der * dim, num_der * dim)) 349 | for i in range(num_der): 350 | for j in range(num_der): 351 | istart, iend = i * dim, i * dim + dim 352 | jstart, jend = j * dim, j * dim + dim 353 | i_d, j_d = which_der[i], which_der[j] 354 | T = np.outer((inn[:, i_d] - mu_Q[:, i_d, j_d]), (inn[:, j_d] - mu_Q[:, i_d, j_d]).T) + Sig_Q 355 | E_dffd[istart:iend, jstart:jend] = Q[i_d, j_d] * T 356 | 357 | return E_dffd # (num_der*dim, num_der*dim) 358 | 359 | -------------------------------------------------------------------------------- /research/gpqd/hybrid_demo.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | import numpy as np 4 | import pandas as pd 5 | 6 | from research.gpq.icinco_demo import evaluate_performance 7 | from ssmtoybox.mtran import UnscentedTransform 8 | from ssmtoybox.ssinf import ExtendedKalman, ExtendedKalmanGPQD 9 | from ssmtoybox.ssmod import UNGMTransition, UNGMMeasurement 10 | from ssmtoybox.utils import GaussRV 11 | 12 | steps, mc = 50, 10 # time steps, mc simulations 13 | 14 | # setup univariate non-stationary growth model 15 | x0 = GaussRV(1, cov=np.atleast_2d(5.0)) 16 | q = GaussRV(1, cov=np.atleast_2d(10.0)) 17 | dyn = UNGMTransition(x0, q) # dynamics 18 | r = GaussRV(1) 19 | obs = UNGMMeasurement(r, 1) # observation model 20 | 21 | x = dyn.simulate_discrete(steps, mc) 22 | z = obs.simulate_measurements(x) 23 | 24 | # use only the central sigma-point 25 | usp_0 = np.zeros((dyn.dim_in, 1)) 26 | usp_ut = UnscentedTransform.unit_sigma_points(dyn.dim_in) 27 | 28 | # set the RBF kernel hyperparameters 29 | hyp_rbf = np.array([[1.0] + dyn.dim_in*[3.0]]) 30 | hyp_rbf_ut = np.array([[8.0] + dyn.dim_in*[0.5]]) 31 | 32 | # derivative observations only at the central point 33 | der_mask = np.array([0]) 34 | 35 | # filters/smoothers to test 36 | algorithms = ( 37 | # EKF, GPQ+D w/ affine kernel, GPQ+D w/ RBF kernel (el --> infty) 38 | ExtendedKalman(dyn, obs), 39 | # GPQ+D RBF kernel w/ single sigma-point, becomes EKF for el --> infinity 40 | ExtendedKalmanGPQD(dyn, obs, hyp_rbf, hyp_rbf), 41 | ) 42 | num_alg = len(algorithms) 43 | 44 | # space for estimates 45 | mean_f, cov_f = np.zeros((dyn.dim_in, steps, mc, num_alg)), np.zeros((dyn.dim_in, dyn.dim_in, steps, mc, num_alg)) 46 | mean_s, cov_s = np.zeros((dyn.dim_in, steps, mc, num_alg)), np.zeros((dyn.dim_in, dyn.dim_in, steps, mc, num_alg)) 47 | 48 | # do filtering/smoothing 49 | t0 = time.time() # measure execution time 50 | print('Running filters/smoothers ...') 51 | for a, alg in enumerate(algorithms): 52 | print('{}'.format(alg.__class__.__name__)) # print filter/smoother name 53 | for sim in range(mc): 54 | mean_f[..., sim, a], cov_f[..., sim, a] = alg.forward_pass(z[..., sim]) 55 | mean_s[..., sim, a], cov_s[..., sim, a] = alg.backward_pass() 56 | alg.reset() 57 | print('Done in {0:.4f} [sec]'.format(time.time() - t0)) 58 | 59 | # evaluate perfomance 60 | scores = evaluate_performance(x, mean_f, cov_f, mean_s, cov_s) 61 | rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s = scores[:6] 62 | rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s = scores[6:] 63 | 64 | # rmseMean_f, rmseMean_s = rmseMean_f.squeeze(), rmseMean_s.squeeze() 65 | # nciMean_f, nciMean_s = nciMean_f.squeeze(), nciMean_s.squeeze() 66 | # nllMean_f, nllMean_s = nllMean_f.squeeze(), nllMean_s.squeeze() 67 | 68 | # put data into Pandas DataFrame for fancy printing and latex export 69 | row_labels = ['EKF', 'EKF-GPQD'] # ['EKF', 'GPQD-RBF', 'GPQD-AFFINE', 'UKF', 'GPQD-UT-RBF'] 70 | col_labels = ['RMSE', '2STD', 'NCI', '2STD', 'NLL', '2STD'] 71 | pd.set_option('precision', 4, 'max_columns', 6) 72 | table_f = pd.DataFrame(np.hstack((rmseMean_f, rmseStd_f, nciMean_f, nciStd_f, nllMean_f, nllStd_f)), 73 | index=row_labels, columns=col_labels) 74 | table_s = pd.DataFrame(np.hstack((rmseMean_s, rmseStd_s, nciMean_s, nciStd_s, nllMean_s, nllStd_s)), 75 | index=row_labels, columns=col_labels) 76 | # print tables 77 | print('Filter metrics') 78 | print(table_f) 79 | print('Smoother metrics') 80 | print(table_s) 81 | -------------------------------------------------------------------------------- /research/gpqd/tests/test_gpqd.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from scipy.linalg import cho_factor, cho_solve, solve 4 | from numpy.linalg import det 5 | from numpy import newaxis as na 6 | 7 | from unittest import TestCase 8 | 9 | from research.gpqd.gpqd_base import GaussianProcessDerModel, RBFGaussDer 10 | from ssmtoybox.utils import maha 11 | from ssmtoybox.mtran import UnscentedTransform 12 | 13 | 14 | class RBFGaussDerKernelTest(TestCase): 15 | 16 | @staticmethod 17 | def expected_eval(xs, x, alpha=10.0, el=0.7, which_der=None): 18 | """RBF kernel w/ derivatives.""" 19 | x, xs = np.atleast_2d(x), np.atleast_2d(xs) 20 | D, N = x.shape 21 | Ds, Ns = xs.shape 22 | assert Ds == D 23 | which_der = np.arange(N) if which_der is None else which_der 24 | Nd = len(which_der) # points w/ derivative observations 25 | # extract hypers 26 | # alpha, el, jitter = hypers['sig_var'], hypers['lengthscale'], hypers['noise_var'] 27 | iLam = np.diag(el ** -1 * np.ones(D)) 28 | iiLam = np.diag(el ** -2 * np.ones(D)) 29 | 30 | x = iLam.dot(x) # sqrt(Lambda^-1) * X 31 | xs = iLam.dot(xs) 32 | Kff = np.exp(2 * np.log(alpha) - 0.5 * maha(xs.T, x.T)) # cov(f(xi), f(xj)) 33 | x = iLam.dot(x) # Lambda^-1 * X 34 | xs = iLam.dot(xs) 35 | XmX = xs[..., na] - x[:, na, :] # pair-wise differences 36 | Kfd = np.zeros((Ns, D * Nd)) # cov(f(xi), df(xj)) 37 | Kdd = np.zeros((D * Nd, D * Nd)) # cov(df(xi), df(xj)) 38 | for i in range(Ns): 39 | for j in range(Nd): 40 | jstart, jend = j * D, j * D + D 41 | j_d = which_der[j] 42 | Kfd[i, jstart:jend] = Kff[i, j_d] * XmX[:, i, j_d] 43 | for i in range(Nd): 44 | for j in range(Nd): 45 | istart, iend = i * D, i * D + D 46 | jstart, jend = j * D, j * D + D 47 | i_d, j_d = which_der[i], which_der[j] # indices of points with derivatives 48 | Kdd[istart:iend, jstart:jend] = Kff[i_d, j_d] * (iiLam - np.outer(XmX[:, i_d, j_d], XmX[:, i_d, j_d])) 49 | if Ns == N: 50 | return np.vstack((np.hstack((Kff, Kfd)), np.hstack((Kfd.T, Kdd)))) 51 | else: 52 | return np.hstack((Kff, Kfd)) 53 | 54 | @staticmethod 55 | def expected_exp_x_dkx(x, alpha=1.0, el=1.0, which_der=None): 56 | dim, num_pts = x.shape 57 | which_der = which_der if which_der is not None else np.arange(num_pts) 58 | eye_d = np.eye(dim) 59 | el = np.asarray(dim * [el]) 60 | iLam = np.diag(el ** -1) # sqrt(Lambda^-1) 61 | iiLam = np.diag(el ** -2) # Lambda^-1 62 | inn = iLam.dot(x) # (x-m)^T*iLam # (N, D) 63 | B = iiLam + eye_d # P*Lambda^-1+I, (P+Lam)^-1 = Lam^-1*(P*Lam^-1+I)^-1 # (D, D) 64 | cho_B = cho_factor(B) 65 | t = cho_solve(cho_B, inn) # dot(inn, inv(B)) # (x-m)^T*iLam*(P+Lambda)^-1 # (D, N) 66 | l = np.exp(-0.5 * np.sum(inn * t, 0)) # (N, 1) 67 | q = (alpha ** 2 / np.sqrt(det(B))) * l # (N, 1) 68 | Sig_q = cho_solve(cho_B, eye_d) # B^-1*I 69 | eta = Sig_q.dot(x) # (D,N) Sig_q*x 70 | mu_q = iiLam.dot(eta) # (D,N) 71 | r = q[na, which_der] * iiLam.dot(mu_q[:, which_der] - x[:, which_der]) # -t.dot(iLam) * q # (D, N) 72 | 73 | return r.T.ravel() # (1, num_der*dim) 74 | 75 | @staticmethod 76 | def expected_exp_x_xdkx(x, alpha=1.0, el=1.0, which_der=None): 77 | dim, num_pts = x.shape 78 | which_der = np.arange(num_pts) if which_der is None else which_der 79 | num_der = len(which_der) 80 | el = np.asarray(dim * [el]) 81 | eye_d = np.eye(dim) 82 | iLam = np.diag(el ** -1) # sqrt(Lambda^-1) 83 | iiLam = np.diag(el ** -2) # Lambda^-1 84 | 85 | inn = iLam.dot(x) # (x-m)^T*iLam # (N, D) 86 | B = iiLam + eye_d 87 | cho_B = cho_factor(B) 88 | t = cho_solve(cho_B, inn) 89 | l = np.exp(-0.5 * np.sum(inn * t, 0)) # (N, 1) 90 | q = (alpha ** 2 / np.sqrt(np.linalg.det(B))) * l # (N, 1) 91 | Sig_q = cho_solve(cho_B, eye_d) # B^-1*I 92 | eta = Sig_q.dot(x) # (D,N) Sig_q*x 93 | mu_q = iiLam.dot(eta) # (D,N) 94 | r = q[na, which_der] * iiLam.dot(mu_q[:, which_der] - x[:, which_der]) # -t.dot(iLam) * q # (D, N) 95 | 96 | # quantities for cross-covariance "weights" 97 | iLamSig = iiLam.dot(Sig_q) # (D,D) 98 | r_tilde = np.empty((dim, num_der * dim)) 99 | for i in range(num_der): 100 | i_d = which_der[i] 101 | r_tilde[:, i * dim:i * dim + dim] = q[i_d] * iLamSig + np.outer(mu_q[:, i_d], r[:, i].T) 102 | 103 | return r_tilde # (dim, num_der*dim) 104 | 105 | @staticmethod 106 | def expected_exp_x_kxdkx(x, alpha=1.0, el=1.0, which_der=None): 107 | dim, num_pts = x.shape 108 | which_der = which_der if which_der is not None else np.arange(num_pts) 109 | num_der = len(which_der) 110 | eye_d = np.eye(dim) 111 | el = np.asarray(dim * [el]) 112 | Lam = np.diag(el ** 2) 113 | iLam = np.diag(el ** -1) # sqrt(Lambda^-1) 114 | iiLam = np.diag(el ** -2) # Lambda^-1 115 | 116 | inn = iLam.dot(x) # (x-m)^T*iLam # (N, D) 117 | B = iiLam + eye_d # P*Lambda^-1+I, (P+Lam)^-1 = Lam^-1*(P*Lam^-1+I)^-1 # (D, D) 118 | cho_B = cho_factor(B) 119 | Sig_q = cho_solve(cho_B, eye_d) # B^-1*I 120 | eta = Sig_q.dot(x) # (D,N) Sig_q*x 121 | # quantities for covariance weights 122 | zet = 2 * np.log(alpha) - 0.5 * np.sum(inn * inn, 0) # (D,N) 2log(alpha) - 0.5*(x-m)^T*Lambda^-1*(x-m) 123 | inn = iiLam.dot(x) # inp / el[:, na]**2 124 | R = 2 * iiLam + eye_d # 2P*Lambda^-1 + I 125 | Q = (1.0 / np.sqrt(det(R))) * np.exp((zet[:, na] + zet[:, na].T) + 126 | maha(inn.T, -inn.T, V=0.5 * solve(R, eye_d))) # (N,N) 127 | cho_LamSig = cho_factor(Lam + Sig_q) 128 | eta_tilde = iiLam.dot(cho_solve(cho_LamSig, eta)) # Lambda^-1(Lambda+Sig_q)^-1*eta 129 | mu_Q = eta_tilde[..., na] + eta_tilde[:, na, :] # (D,N_der,N) pairwise sum of pre-multiplied eta's 130 | 131 | E_dfff = np.empty((num_der * dim, num_pts)) 132 | for i in range(num_der): 133 | for j in range(num_pts): 134 | istart, iend = i * dim, i * dim + dim 135 | i_d = which_der[i] 136 | E_dfff[istart:iend, j] = Q[i_d, j] * (mu_Q[:, i_d, j] - inn[:, i_d]) 137 | 138 | return E_dfff.T # (num_der*dim, num_pts) 139 | 140 | @staticmethod 141 | def expected_exp_x_dkxdkx(x, alpha=1.0, el=1.0, which_der=None): 142 | dim, num_pts = x.shape 143 | which_der = which_der if which_der is not None else np.arange(num_pts) 144 | num_der = len(which_der) 145 | eye_d = np.eye(dim) 146 | el = np.asarray(dim * [el]) 147 | Lam = np.diag(el ** 2) 148 | iLam = np.diag(el ** -1) # sqrt(Lambda^-1) 149 | iiLam = np.diag(el ** -2) # Lambda^-1 150 | 151 | inn = iLam.dot(x) # (x-m)^T*iLam # (N, D) 152 | B = iiLam + eye_d # P*Lambda^-1+I, (P+Lam)^-1 = Lam^-1*(P*Lam^-1+I)^-1 # (D, D) 153 | cho_B = cho_factor(B) 154 | Sig_q = cho_solve(cho_B, eye_d) # B^-1*I 155 | eta = Sig_q.dot(x) # (D,N) Sig_q*x 156 | # quantities for covariance weights 157 | zet = 2 * np.log(alpha) - 0.5 * np.sum(inn * inn, 0) # (D,N) 2log(alpha) - 0.5*(x-m)^T*Lambda^-1*(x-m) 158 | inn = iiLam.dot(x) # inp / el[:, na]**2 159 | R = 2 * iiLam + eye_d # 2P*Lambda^-1 + I 160 | Q = (1.0 / np.sqrt(det(R))) * np.exp((zet[:, na] + zet[:, na].T) + 161 | maha(inn.T, -inn.T, V=0.5 * solve(R, eye_d))) # (N,N) 162 | cho_LamSig = cho_factor(Lam + Sig_q) 163 | Sig_Q = cho_solve(cho_LamSig, Sig_q).dot(iiLam) # (D,D) Lambda^-1 (Lambda*(Lambda+Sig_q)^-1*Sig_q) Lambda^-1 164 | eta_tilde = iiLam.dot(cho_solve(cho_LamSig, eta)) # Lambda^-1(Lambda+Sig_q)^-1*eta 165 | mu_Q = eta_tilde[..., na] + eta_tilde[:, na, :] # (D,N_der,N) pairwise sum of pre-multiplied eta's 166 | 167 | E_dffd = np.empty((num_der * dim, num_der * dim)) 168 | for i in range(num_der): 169 | for j in range(num_der): 170 | istart, iend = i * dim, i * dim + dim 171 | jstart, jend = j * dim, j * dim + dim 172 | i_d, j_d = which_der[i], which_der[j] 173 | T = np.outer((inn[:, i_d] - mu_Q[:, i_d, j_d]), (inn[:, j_d] - mu_Q[:, i_d, j_d]).T) + Sig_Q 174 | E_dffd[istart:iend, jstart:jend] = Q[i_d, j_d] * T 175 | 176 | return E_dffd # (num_der*dim, num_der*dim) 177 | 178 | def setUp(self) -> None: 179 | self.dim = 2 180 | self.x = UnscentedTransform.unit_sigma_points(self.dim) 181 | self.kernel_par = np.array([[1.0] + self.dim * [2.0]]) 182 | self.kernel = RBFGaussDer(self.dim, self.kernel_par) 183 | 184 | def test_eval(self): 185 | kernel, kernel_par, x = self.kernel, self.kernel_par, self.x 186 | 187 | out = kernel.eval(kernel_par, x) 188 | exp = RBFGaussDerKernelTest.expected_eval(x, x, alpha=1.0, el=2.0) 189 | 190 | self.assertEqual(out.shape, exp.shape) 191 | self.assertTrue(np.allclose(out, exp)) 192 | 193 | def test_exp_x_dkx(self): 194 | kernel, kernel_par, x = self.kernel, self.kernel_par, self.x 195 | 196 | out = kernel.exp_x_dkx(kernel_par, x) 197 | exp = RBFGaussDerKernelTest.expected_exp_x_dkx(x, alpha=kernel_par[0, 0], el=kernel_par[0, 1]) 198 | 199 | self.assertEqual(out.shape, exp.shape) 200 | self.assertTrue(np.allclose(out, exp)) 201 | 202 | def test_exp_x_xdkx(self): 203 | kernel, kernel_par, x = self.kernel, self.kernel_par, self.x 204 | 205 | out = kernel.exp_x_xdkx(kernel_par, x) 206 | exp = RBFGaussDerKernelTest.expected_exp_x_xdkx(x, alpha=kernel_par[0, 0], el=kernel_par[0, 1]) 207 | 208 | self.assertEqual(out.shape, exp.shape) 209 | self.assertTrue(np.allclose(out, exp)) 210 | 211 | def test_exp_x_kxdkx(self): 212 | kernel, kernel_par, x = self.kernel, self.kernel_par, self.x 213 | 214 | out = kernel.exp_x_kxdkx(kernel_par, x) 215 | exp = RBFGaussDerKernelTest.expected_exp_x_kxdkx(x, alpha=kernel_par[0, 0], el=kernel_par[0, 1]) 216 | 217 | self.assertEqual(out.shape, exp.shape) 218 | self.assertTrue(np.allclose(out, exp)) 219 | 220 | def test_exp_x_dkxdkx(self): 221 | kernel, kernel_par, x = self.kernel, self.kernel_par, self.x 222 | 223 | out = kernel.exp_x_dkxdkx(kernel_par, x) 224 | exp = RBFGaussDerKernelTest.expected_exp_x_dkxdkx(x, alpha=kernel_par[0, 0], el=kernel_par[0, 1]) 225 | 226 | self.assertEqual(out.shape, exp.shape) 227 | self.assertTrue(np.allclose(out, exp)) 228 | 229 | 230 | class GaussianProcessDerModelTest(TestCase): 231 | 232 | @staticmethod 233 | def weights_rbf_der(unit_sp, alpha=1.0, el=1.0, which_der=None): 234 | d, n = unit_sp.shape 235 | which_der = which_der if which_der is not None else np.arange(n) 236 | 237 | el = np.asarray(d * [el]) 238 | assert len(el) == d 239 | i_der = which_der # shorthand for indexes of points with derivatives 240 | n_der = len(i_der) # # points w/ derivatives 241 | assert n_der <= n # # points w/ derivatives must be <= # points 242 | # pre-allocation for convenience 243 | eye_d, eye_n, eye_y = np.eye(d), np.eye(n), np.eye(n + d * n_der) 244 | 245 | K = RBFGaussDerKernelTest.expected_eval(unit_sp, unit_sp, alpha=alpha, el=el, which_der=i_der) 246 | iK = cho_solve(cho_factor(K + 1e-8 * eye_y), eye_y) # invert kernel matrix BOTTLENECK 247 | Lam = np.diag(el ** 2) 248 | iLam = np.diag(el ** -1) # sqrt(Lambda^-1) 249 | iiLam = np.diag(el ** -2) # Lambda^-1 250 | inn = iLam.dot(unit_sp) # (x-m)^T*iLam # (N, D) 251 | B = iiLam + eye_d # P*Lambda^-1+I, (P+Lam)^-1 = Lam^-1*(P*Lam^-1+I)^-1 # (D, D) 252 | cho_B = cho_factor(B) 253 | t = cho_solve(cho_B, inn) # dot(inn, inv(B)) # (x-m)^T*iLam*(P+Lambda)^-1 # (D, N) 254 | l = np.exp(-0.5 * np.sum(inn * t, 0)) # (N, 1) 255 | q = (alpha ** 2 / np.sqrt(np.linalg.det(B))) * l # (N, 1) 256 | Sig_q = cho_solve(cho_B, eye_d) # B^-1*I 257 | eta = Sig_q.dot(unit_sp) # (D,N) Sig_q*x 258 | mu_q = iiLam.dot(eta) # (D,N) 259 | r = q[na, i_der] * iiLam.dot(mu_q[:, i_der] - unit_sp[:, i_der]) # -t.dot(iLam) * q # (D, N) 260 | q_tilde = np.hstack((q.T, r.T.ravel())) # (1, N + n_der*D) 261 | 262 | # weights for mean 263 | wm = q_tilde.dot(iK) 264 | 265 | # quantities for cross-covariance "weights" 266 | iLamSig = iiLam.dot(Sig_q) # (D,D) 267 | r_tilde = np.empty((d, n_der * d)) 268 | for i in range(n_der): 269 | i_d = i_der[i] 270 | r_tilde[:, i * d:i * d + d] = q[i_d] * iLamSig + np.outer(mu_q[:, i_d], r[:, i].T) 271 | R_tilde = np.hstack((q[na, :] * mu_q, r_tilde)) # (D, N+N*D) 272 | 273 | # input-output covariance (cross-covariance) "weights" 274 | Wcc = R_tilde.dot(iK) # (D, N+N*D) 275 | 276 | # quantities for covariance weights 277 | zet = 2 * np.log(alpha) - 0.5 * np.sum(inn * inn, 0) # (D,N) 2log(alpha) - 0.5*(x-m)^T*Lambda^-1*(x-m) 278 | inn = iiLam.dot(unit_sp) # inp / el[:, na]**2 279 | R = 2 * iiLam + eye_d # 2P*Lambda^-1 + I 280 | Q = (1.0 / np.sqrt(det(R))) * np.exp((zet[:, na] + zet[:, na].T) + 281 | maha(inn.T, -inn.T, V=0.5 * solve(R, eye_d))) # (N,N) 282 | cho_LamSig = cho_factor(Lam + Sig_q) 283 | Sig_Q = cho_solve(cho_LamSig, Sig_q).dot(iiLam) # (D,D) Lambda^-1 (Lambda*(Lambda+Sig_q)^-1*Sig_q) Lambda^-1 284 | eta_tilde = iiLam.dot(cho_solve(cho_LamSig, eta)) # Lambda^-1(Lambda+Sig_q)^-1*eta 285 | mu_Q = eta_tilde[..., na] + eta_tilde[:, na, :] # (D,N_der,N) pairwise sum of pre-multiplied eta's 286 | 287 | E_dfff = np.empty((n_der * d, n)) 288 | for i in range(n_der): 289 | for j in range(n): 290 | istart, iend = i * d, i * d + d 291 | i_d = i_der[i] 292 | E_dfff[istart:iend, j] = Q[i_d, j] * (mu_Q[:, i_d, j] - inn[:, i_d]) 293 | 294 | E_dffd = np.empty((n_der * d, n_der * d)) 295 | for i in range(n_der): 296 | for j in range(n_der): 297 | istart, iend = i * d, i * d + d 298 | jstart, jend = j * d, j * d + d 299 | i_d, j_d = i_der[i], i_der[j] 300 | T = np.outer((inn[:, i_d] - mu_Q[:, i_d, j_d]), (inn[:, j_d] - mu_Q[:, i_d, j_d]).T) + Sig_Q 301 | E_dffd[istart:iend, jstart:jend] = Q[i_d, j_d] * T 302 | Q_tilde = np.vstack((np.hstack((Q, E_dfff.T)), np.hstack((E_dfff, E_dffd)))) # (N + N_der*D, N + N_der*D) 303 | 304 | # weights for covariance 305 | iKQ = iK.dot(Q_tilde) 306 | Wc = iKQ.dot(iK) 307 | 308 | return wm, Wc, Wcc 309 | 310 | def test_weights(self): 311 | dim = 2 312 | kernel_par = np.array([[1.0] + dim*[2.0]]) 313 | model = GaussianProcessDerModel(dim, kernel_par, 'ut') 314 | 315 | out_wm, out_wc, out_wcc, _, _ = model.bq_weights(kernel_par) 316 | exp_wm, exp_wc, exp_wcc = GaussianProcessDerModelTest.weights_rbf_der(model.points, 317 | alpha=kernel_par[0, 0], 318 | el=kernel_par[0, 1]) 319 | # check shapes 320 | self.assertEqual(out_wm.shape, exp_wm.shape) 321 | self.assertEqual(out_wc.shape, exp_wc.shape) 322 | self.assertEqual(out_wcc.shape, exp_wcc.shape) 323 | 324 | # check values 325 | self.assertTrue(np.allclose(out_wm, out_wm)) 326 | self.assertTrue(np.allclose(out_wc, out_wc)) 327 | self.assertTrue(np.allclose(out_wcc, out_wcc)) 328 | -------------------------------------------------------------------------------- /research/tpq/figprint.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib as mpl 3 | mpl.use('pgf') 4 | 5 | 6 | class FigurePrint: 7 | 8 | INCH_PER_PT = 1.0 / 72.27 # Convert pt to inch 9 | PHI = (np.sqrt(5.0) - 1.0) / 2.0 # Aesthetic ratio (you could change this) 10 | 11 | def __init__(self, fig_width_pt=252): 12 | """ 13 | 14 | Parameters 15 | ---------- 16 | fig_width_pt : float 17 | Width of the figure in points, usually obtained from the journal specs or using the LaTeX command 18 | ``\the\columnwidth``. Default is ``fig_width_pt=252`` (3.5 inches). 19 | """ 20 | self.fig_width_pt = fig_width_pt 21 | pgf_with_latex = { # setup matplotlib to use latex for output 22 | "pgf.texsystem": "pdflatex", # change this if using xetex or lautex 23 | "text.usetex": True, # use LaTeX to write all text 24 | "font.family": "serif", 25 | "font.serif": [], # blank entries should cause plots to inherit fonts from the document 26 | "font.sans-serif": [], 27 | "font.monospace": [], 28 | "font.size": 10, 29 | "axes.labelsize": 10, # LaTeX default is 10pt font. 30 | "legend.fontsize": 8, # Make the legend/label fonts a little smaller 31 | "xtick.labelsize": 8, 32 | "ytick.labelsize": 8, 33 | # "axes.prop_cycle": ['#5DA5DA', '#FAA43A', '#60BD68', 34 | # '#F17CB0', '#B2912F', '#B276B2', 35 | # '#DECF3F', '#F15854', '#4D4D4D'], 36 | "figure.figsize": self.figsize(), # default fig size 37 | "pgf.preamble": [ # plots will be generated using this preamble 38 | r"\usepackage[utf8]{inputenc}", # use utf8 fonts 39 | r"\usepackage[T1]{fontenc}", 40 | ] 41 | } 42 | mpl.rcParams.update(pgf_with_latex) 43 | 44 | def figsize(self, w_scale=1.0, h_scale=1.0): 45 | """ 46 | Calculates figure width and height given the width and height scale. 47 | 48 | Parameters 49 | ---------- 50 | w_scale: float 51 | Figure width scale. 52 | 53 | h_scale: float 54 | Figure height scale. 55 | 56 | Returns 57 | ------- 58 | list 59 | Figure width and height in inches. 60 | """ 61 | 62 | fig_width = self.fig_width_pt * self.INCH_PER_PT * w_scale # width in inches 63 | fig_height = fig_width * self.PHI * h_scale # height in inches 64 | return [fig_width, fig_height] 65 | 66 | def update_default_figsize(self, fig_width_pt): 67 | """ 68 | Updates default figure size used for saving. 69 | 70 | Parameters 71 | ---------- 72 | fig_width_pt : float 73 | Width of the figure in points, usually obtained from the journal specs or using the LaTeX command 74 | ``\the\columnwidth``. 75 | 76 | 77 | Returns 78 | ------- 79 | 80 | """ 81 | self.fig_width_pt = fig_width_pt 82 | mpl.rcParams.update({"figure.figsize": self.figsize()}) 83 | 84 | @staticmethod 85 | def savefig(filename): 86 | """ 87 | Save figure to PGF. PDF copy created for viewing convenience. 88 | 89 | Parameters 90 | ---------- 91 | filename 92 | 93 | Returns 94 | ------- 95 | 96 | """ 97 | plt.savefig('{}.pgf'.format(filename)) 98 | plt.savefig('{}.pdf'.format(filename)) 99 | 100 | import matplotlib.pyplot as plt 101 | -------------------------------------------------------------------------------- /research/tpq/gpr_vs_tpr.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | parser = argparse.ArgumentParser(description='Generate figure comparing predictions of GP and TP regression models.') 3 | parser.add_argument('--show-plots', help='Shows plots instead of printing them into PDF/PGF.', action='store_true') 4 | args = parser.parse_args() 5 | 6 | if args.show_plots: 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | else: 10 | from figprint import * 11 | from numpy import newaxis as na 12 | from ssmtoybox.bq.bqmod import GaussianProcessModel, StudentTProcessModel 13 | 14 | dim = 1 15 | par_kernel = np.array([[0.8, 0.7]]) 16 | 17 | # init models 18 | gp = GaussianProcessModel(dim, par_kernel, kern_str='rbf', point_str='ut') 19 | tp = StudentTProcessModel(dim, par_kernel, kern_str='rbf', point_str='ut', nu=10.0) 20 | 21 | # some nonlinear function 22 | f = lambda x: np.sin(np.sin(x)*x**2)*np.exp(x) 23 | expit = lambda x: 5/(1+np.exp(-20*(x+1)))+0.01 + 5/(1+np.exp(20*(x-2)))+0.01 24 | 25 | # setup some test data 26 | num_test = 100 27 | x_test = np.linspace(-5, 5, num_test)[na, :] 28 | 29 | # draw from a GP 30 | K = gp.kernel.eval(np.array([[0.1, 0.7]]), x_test) + 1e-8*np.eye(num_test) 31 | gp_sample = np.random.multivariate_normal(np.zeros(num_test), K) 32 | # amplitude modulation of the gp sample 33 | gp_sample *= expit(np.linspace(-5, 5, num_test)) 34 | gp_sample += expit(np.linspace(-5, 5, num_test)) 35 | 36 | 37 | i_train = [10, 20, 40, 52, 55, 80] 38 | x_train = x_test[:, i_train] 39 | y_train = gp_sample[i_train] # + multivariate_t(np.zeros((1,)), 0.5*np.eye(1), 3.0, size=len(i_train)).squeeze() 40 | # noise = multivariate_t(np.zeros((1,)), np.eye(1), 3.0, size=gp.num_pts).T 41 | y_test = gp_sample 42 | 43 | gp_mean, gp_var = gp.predict(x_test, y_train, x_train, par_kernel) 44 | gp_std = np.sqrt(gp_var) 45 | tp_mean, tp_var = tp.predict(x_test, y_train, x_train, par_kernel) 46 | tp_std = np.sqrt(tp_var) 47 | 48 | x_test = x_test.squeeze() 49 | y_test = y_test.squeeze() 50 | x_train = x_train.squeeze() 51 | y_train = y_train.squeeze() 52 | 53 | if args.show_plots is None: 54 | fp = FigurePrint() 55 | 56 | # plt.plot(np.linspace(-5, 5, num_test), expit(np.linspace(-5, 5, num_test))) 57 | # plot training data, predictive mean and variance 58 | ymin, ymax, ypad = gp_sample.min(), gp_sample.max(), 0.25*gp_sample.ptp() 59 | fig, ax = plt.subplots(2, 1, sharex=True) 60 | 61 | ax[0].fill_between(x_test, gp_mean - 2 * gp_std, gp_mean + 2 * gp_std, color='0.1', alpha=0.15) 62 | ax[0].plot(x_test, gp_mean, color='k', lw=2) 63 | ax[0].plot(x_train, y_train, 'ko', ms=6) 64 | ax[0].plot(x_test, y_test, lw=2, ls='--', color='tomato') 65 | ax[0].set_ylim([ymin-ypad, ymax+ypad]) 66 | ax[0].set_ylabel('g(x)') 67 | 68 | ax[1].fill_between(x_test, tp_mean - 2 * tp_std, tp_mean + 2 * tp_std, color='0.1', alpha=0.15) 69 | ax[1].plot(x_test, tp_mean, color='k', lw=2) 70 | ax[1].plot(x_train, y_train, 'ko', ms=6) 71 | ax[1].plot(x_test, y_test, lw=2, ls='--', color='tomato') 72 | ax[1].set_ylim([ymin-ypad, ymax+ypad]) 73 | ax[1].set_ylabel('g(x)') 74 | ax[1].set_xlabel('x') 75 | 76 | plt.tight_layout(pad=0.0) 77 | plt.show() 78 | 79 | if args.show_plots is None: 80 | fp.savefig('gp_vs_tp') 81 | -------------------------------------------------------------------------------- /research/tpq/tpq_base.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy import newaxis as na 3 | from ssmtoybox.mtran import LinearizationTransform, FullySymmetricStudentTransform 4 | from ssmtoybox.bq.bqmtran import GaussianProcessTransform, StudentTProcessTransform, BQTransform 5 | from ssmtoybox.bq.bqkern import RBFStudent 6 | from ssmtoybox.ssmod import TransitionModel, MeasurementModel, UNGMTransition, UNGMMeasurement 7 | from ssmtoybox.ssinf import StudentianInference 8 | from ssmtoybox.utils import log_cred_ratio, mse_matrix, gauss_mixture, multivariate_t, RandomVariable, GaussRV, \ 9 | StudentRV 10 | 11 | 12 | # Gaussian mixture random variable 13 | class GaussianMixtureRV(RandomVariable): 14 | 15 | def __init__(self, dim, means, covs, alphas): 16 | if len(means) != len(covs) != len(alphas): 17 | raise ValueError('Same number of means, covariances and mixture weights needs to be supplied!') 18 | 19 | if not np.isclose(np.sum(alphas), 1.0): 20 | ValueError('Mixture weights must sum to unity!') 21 | 22 | self.dim = dim 23 | self.means = means 24 | self.covs = covs 25 | self.alphas = alphas 26 | 27 | def sample(self, size): 28 | return np.moveaxis(gauss_mixture(self.means, self.covs, self.alphas, size), -1, 0) 29 | 30 | def get_stats(self): 31 | return self.means, self.covs, self.alphas 32 | 33 | 34 | # Student's t-filters 35 | class ExtendedStudent(StudentianInference): 36 | 37 | def __init__(self, dyn, obs, dof=4.0, fixed_dof=True): 38 | tf = LinearizationTransform(dyn.dim_in) 39 | th = LinearizationTransform(obs.dim_in) 40 | super(ExtendedStudent, self).__init__(dyn, obs, tf, th, dof, fixed_dof) 41 | 42 | 43 | class GPQStudent(StudentianInference): 44 | 45 | def __init__(self, dyn, obs, kern_par_dyn, kern_par_obs, point_hyp=None, dof=4.0, fixed_dof=True): 46 | """ 47 | Student filter with Gaussian Process quadrature moment transforms using fully-symmetric sigma-point set. 48 | 49 | Parameters 50 | ---------- 51 | dyn : TransitionModel 52 | 53 | obs : MeasurementModel 54 | 55 | kern_par_dyn : numpy.ndarray 56 | Kernel parameters for the GPQ moment transform of the dynamics. 57 | 58 | kern_par_obs : numpy.ndarray 59 | Kernel parameters for the GPQ moment transform of the measurement function. 60 | 61 | point_hyp : dict 62 | Point set parameters with keys: 63 | * `'degree'`: Degree (order) of the quadrature rule. 64 | * `'kappa'`: Tuning parameter of controlling spread of sigma-points around the center. 65 | 66 | dof : float 67 | Desired degree of freedom for the filtered density. 68 | 69 | fixed_dof : bool 70 | If `True`, DOF will be fixed for all time steps, which preserves the heavy-tailed behaviour of the filter. 71 | If `False`, DOF will be increasing after each measurement update, which means the heavy-tailed behaviour is 72 | not preserved and therefore converges to a Gaussian filter. 73 | """ 74 | 75 | # degrees of freedom for SSM noises 76 | _, _, q_dof = dyn.noise_rv.get_stats() 77 | _, _, r_dof = obs.noise_rv.get_stats() 78 | 79 | # add DOF of the noises to the sigma-point parameters 80 | if point_hyp is None: 81 | point_hyp = dict() 82 | point_hyp_dyn = point_hyp 83 | point_hyp_obs = point_hyp 84 | point_hyp_dyn.update({'dof': q_dof}) 85 | point_hyp_obs.update({'dof': r_dof}) 86 | 87 | # init moment transforms 88 | t_dyn = GaussianProcessTransform(dyn.dim_in, kern_par_dyn, 'rbf-student', 'fs', point_hyp_dyn) 89 | t_obs = GaussianProcessTransform(obs.dim_in, kern_par_obs, 'rbf-student', 'fs', point_hyp_obs) 90 | super(GPQStudent, self).__init__(dyn, obs, t_dyn, t_obs, dof, fixed_dof) 91 | 92 | 93 | class FSQStudent(StudentianInference): 94 | """Filter based on fully symmetric quadrature rules.""" 95 | 96 | def __init__(self, dyn, obs, degree=3, kappa=None, dof=4.0, fixed_dof=True): 97 | 98 | # degrees of freedom for SSM noises 99 | _, _, q_dof = dyn.noise_rv.get_stats() 100 | _, _, r_dof = obs.noise_rv.get_stats() 101 | 102 | # init moment transforms 103 | t_dyn = FullySymmetricStudentTransform(dyn.dim_in, degree, kappa, q_dof) 104 | t_obs = FullySymmetricStudentTransform(obs.dim_in, degree, kappa, r_dof) 105 | super(FSQStudent, self).__init__(dyn, obs, t_dyn, t_obs, dof, fixed_dof) 106 | 107 | 108 | def rbf_student_mc_weights(x, kern, num_samples, num_batch): 109 | # MC approximated BQ weights using RBF kernel and Student density 110 | # MC computed by batches, because without batches we would run out of memory for large sample sizes 111 | 112 | assert isinstance(kern, RBFStudent) 113 | # kernel parameters and input dimensionality 114 | par = kern.par 115 | dim, num_pts = x.shape 116 | 117 | # inverse kernel matrix 118 | iK = kern.eval_inv_dot(kern.par, x, scaling=False) 119 | mean, scale, dof = np.zeros((dim, )), np.eye(dim), kern.dof 120 | 121 | # compute MC estimates by batches 122 | num_samples_batch = num_samples // num_batch 123 | q_batch = np.zeros((num_pts, num_batch, )) 124 | Q_batch = np.zeros((num_pts, num_pts, num_batch)) 125 | R_batch = np.zeros((dim, num_pts, num_batch)) 126 | for ib in range(num_batch): 127 | 128 | # multivariate t samples 129 | x_samples = multivariate_t(mean, scale, dof, num_samples_batch).T 130 | 131 | # evaluate kernel 132 | k_samples = kern.eval(par, x_samples, x, scaling=False) 133 | kk_samples = k_samples[:, na, :] * k_samples[..., na] 134 | xk_samples = x_samples[..., na] * k_samples[na, ...] 135 | 136 | # intermediate sums 137 | q_batch[..., ib] = k_samples.sum(axis=0) 138 | Q_batch[..., ib] = kk_samples.sum(axis=0) 139 | R_batch[..., ib] = xk_samples.sum(axis=1) 140 | 141 | # MC approximations == sum the sums divide by num_samples 142 | c = 1/num_samples 143 | q = c * q_batch.sum(axis=-1) 144 | Q = c * Q_batch.sum(axis=-1) 145 | R = c * R_batch.sum(axis=-1) 146 | 147 | # BQ moment transform weights 148 | wm = q.dot(iK) 149 | wc = iK.dot(Q).dot(iK) 150 | wcc = R.dot(iK) 151 | return wm, wc, wcc, Q 152 | 153 | 154 | def eval_perf_scores(x, mf, Pf): 155 | xD, steps, mc_sims, num_filt = mf.shape 156 | 157 | # average RMSE over simulations 158 | rmse = np.sqrt(((x[..., na] - mf) ** 2).sum(axis=0)) 159 | rmse_avg = rmse.mean(axis=1) 160 | 161 | reg = 1e-6 * np.eye(xD) 162 | 163 | # average inclination indicator over simulations 164 | lcr = np.empty((steps, mc_sims, num_filt)) 165 | for f in range(num_filt): 166 | for k in range(steps): 167 | mse = mse_matrix(x[:, k, :], mf[:, k, :, f]) + reg 168 | for imc in range(mc_sims): 169 | lcr[k, imc, f] = log_cred_ratio(x[:, k, imc], mf[:, k, imc, f], Pf[..., k, imc, f], mse) 170 | lcr_avg = lcr.mean(axis=1) 171 | 172 | return rmse_avg, lcr_avg 173 | 174 | 175 | def run_filters(filters, z): 176 | num_filt = len(filters) 177 | zD, steps, mc_sims = z.shape 178 | xD = filters[0].mod_dyn.dim_state 179 | 180 | # init space for filtered mean and covariance 181 | mf = np.zeros((xD, steps, mc_sims, num_filt)) 182 | Pf = np.zeros((xD, xD, steps, mc_sims, num_filt)) 183 | 184 | # run filters 185 | for i, f in enumerate(filters): 186 | print('Running {} ...'.format(f.__class__.__name__)) 187 | for imc in range(mc_sims): 188 | mf[..., imc, i], Pf[..., imc, i] = f.forward_pass(z[..., imc]) 189 | f.reset() 190 | 191 | # return filtered mean and covariance 192 | return mf, Pf 193 | 194 | -------------------------------------------------------------------------------- /research/tpq/tpq_constant_velocity.py: -------------------------------------------------------------------------------- 1 | from tpq_base import * 2 | from scipy.io import savemat 3 | from ssmtoybox.ssmod import ConstantVelocity, Radar2DMeasurement 4 | from ssmtoybox.ssinf import StudentProcessKalman, StudentProcessStudent, GaussianProcessKalman, UnscentedKalman, \ 5 | CubatureKalman, FullySymmetricStudent 6 | 7 | """ 8 | Tracking of an object behaving according to Constant Velocity model based on radar measurements with glint noise. 9 | """ 10 | 11 | 12 | def constant_velocity_radar_demo(steps=100, mc_sims=100): 13 | print('Constant Velocity Radar Tracking with Glint Noise') 14 | print('K = {:d}, MC = {:d}'.format(steps, mc_sims)) 15 | 16 | # SYSTEM 17 | m0 = np.array([10000, 300, 1000, -40], dtype=np.float) 18 | P0 = np.diag([100**2, 10**2, 100**2, 10**2]) 19 | x0 = GaussRV(4, m0, P0) 20 | dt = 0.5 # discretization period 21 | # process noise and noise gain 22 | Q = np.diag([50, 5]) 23 | G = np.array([[dt ** 2 / 2, 0], 24 | [dt, 0], 25 | [0, dt ** 2 / 2], 26 | [0, dt]]) 27 | q = GaussRV(4, cov=G.T.dot(Q).dot(G)) 28 | dyn = ConstantVelocity(x0, q, dt) 29 | 30 | R0 = np.diag([50, 0.4e-6]) 31 | R1 = np.diag([5000, 1.6e-5]) # glint (outlier) RV covariance 32 | glint_prob = 0.15 33 | r = GaussianMixtureRV(2, covs=(R0, R1), alphas=(1-glint_prob, glint_prob)) 34 | obs = Radar2DMeasurement(r, dyn.dim_state, state_index=[0, 2, 1, 3]) 35 | 36 | # SIMULATE DATA 37 | x = dyn.simulate_discrete(steps, mc_sims) 38 | z = obs.simulate_measurements(x) 39 | 40 | # STATE SPACE MODEL 41 | m0 = np.array([10175, 295, 980, -35], dtype=np.float) 42 | P0 = np.diag([100 ** 2, 10 ** 2, 100 ** 2, 10 ** 2]) 43 | x0_dof = 1000.0 44 | x0 = StudentRV(4, m0, ((x0_dof-2)/x0_dof)*P0, x0_dof) 45 | dt = 0.5 # discretization period 46 | # process noise and noise gain 47 | Q = np.diag([50, 5]) 48 | q = StudentRV(4, scale=((x0_dof-2)/x0_dof)*G.T.dot(Q).dot(G), dof=x0_dof) 49 | dyn = ConstantVelocity(x0, q, dt) 50 | 51 | r_dof = 4.0 52 | r = StudentRV(2, scale=((r_dof-2)/r_dof)*R0, dof=r_dof) 53 | obs = Radar2DMeasurement(r, dyn.dim_state) 54 | 55 | # import matplotlib.pyplot as plt 56 | # for i in range(mc_sims): 57 | # plt.plot(x[0, :, i], x[2, :, i], 'b', alpha=0.15) 58 | # plt.show() 59 | 60 | # kernel parameters for TPQ and GPQ filters 61 | # TPQ Student 62 | par_dyn_tp = np.array([[0.05, 100, 100, 100, 100]], dtype=float) 63 | par_obs_tp = np.array([[0.005, 10, 100, 10, 100]], dtype=float) 64 | # parameters of the point-set 65 | kappa = 0.0 66 | par_pt = {'kappa': kappa} 67 | 68 | # print kernel parameters 69 | import pandas as pd 70 | parlab = ['alpha'] + ['ell_{}'.format(d + 1) for d in range(x.shape[0])] 71 | partable = pd.DataFrame(np.vstack((par_dyn_tp, par_obs_tp)), columns=parlab, index=['dyn', 'obs']) 72 | print() 73 | print(partable) 74 | 75 | # TODO: less TPQSFs, max boxplot y-range = 2000, try to get convergent RMSE semilogy 76 | # init filters 77 | filters = ( 78 | # ExtendedStudent(dyn, obs), 79 | # UnscentedKalman(dyn, obs, kappa=kappa), 80 | FullySymmetricStudent(dyn, obs, kappa=kappa, dof=4.0), 81 | StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=4.0, dof_tp=4.0, point_par=par_pt), 82 | # StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=4.0, dof_tp=10.0, point_par=par_pt), 83 | # StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=4.0, dof_tp=20.0, point_par=par_pt), 84 | # GaussianProcessKalman(dyn, obs, par_dyn_tp, par_obs_tp, dof=4.0, point_hyp=par_pt), 85 | ) 86 | itpq = np.argwhere([isinstance(filters[i], StudentProcessStudent) for i in range(len(filters))]).squeeze(axis=1)[0] 87 | 88 | # assign weights approximated by MC with lots of samples 89 | # very dirty code 90 | pts = filters[itpq].tf_dyn.model.points 91 | kern = filters[itpq].tf_dyn.model.kernel 92 | wm, wc, wcc, Q = rbf_student_mc_weights(pts, kern, int(2e6), 1000) 93 | for f in filters: 94 | if isinstance(f.tf_dyn, BQTransform): 95 | f.tf_dyn.wm, f.tf_dyn.Wc, f.tf_dyn.Wcc = wm, wc, wcc 96 | f.tf_dyn.Q = Q 97 | pts = filters[itpq].tf_meas.model.points 98 | kern = filters[itpq].tf_meas.model.kernel 99 | wm, wc, wcc, Q = rbf_student_mc_weights(pts, kern, int(2e6), 1000) 100 | for f in filters: 101 | if isinstance(f.tf_meas, BQTransform): 102 | f.tf_meas.wm, f.tf_meas.Wc, f.tf_meas.Wcc = wm, wc, wcc 103 | f.tf_meas.Q = Q 104 | 105 | # run all filters 106 | mf, Pf = run_filters(filters, z) 107 | 108 | # evaluate scores 109 | pos_x, pos_mf, pos_Pf = x[[0, 2], ...], mf[[0, 2], ...], Pf[np.ix_([0, 2], [0, 2])] 110 | vel_x, vel_mf, vel_Pf = x[[1, 3], ...], mf[[1, 3], ...], Pf[np.ix_([1, 3], [1, 3])] 111 | pos_rmse, pos_lcr = eval_perf_scores(pos_x, pos_mf, pos_Pf) 112 | vel_rmse, vel_lcr = eval_perf_scores(vel_x, vel_mf, vel_Pf) 113 | rmse_avg, lcr_avg = eval_perf_scores(x, mf, Pf) 114 | 115 | # variance of average metrics 116 | from ssmtoybox.utils import bootstrap_var 117 | var_rmse_avg = np.zeros((len(filters),)) 118 | var_lcr_avg = np.zeros((len(filters),)) 119 | for fi in range(len(filters)): 120 | var_rmse_avg[fi] = bootstrap_var(rmse_avg[:, fi], int(1e4)) 121 | var_lcr_avg[fi] = bootstrap_var(lcr_avg[:, fi], int(1e4)) 122 | 123 | # save trajectories, measurements and metrics to file for later processing (tables, plots) 124 | data_dict = { 125 | 'x': x, 126 | 'z': z, 127 | 'mf': mf, 128 | 'Pf': Pf, 129 | 'rmse_avg': rmse_avg, 130 | 'lcr_avg': lcr_avg, 131 | 'var_rmse_avg': var_rmse_avg, 132 | 'var_lcr_avg': var_lcr_avg, 133 | 'pos_rmse': pos_rmse, 134 | 'pos_lcr': pos_lcr, 135 | 'vel_rmse': vel_rmse, 136 | 'vel_lcr': vel_lcr, 137 | 'steps': steps, 138 | 'mc_sims': mc_sims, 139 | 'par_dyn_tp': par_dyn_tp, 140 | 'par_obs_tp': par_obs_tp, 141 | 'f_label': ['UKF', 'SF', r'TPQSF($\nu$=20)', 'GPQSF'] 142 | } 143 | savemat('cv_radar_simdata_{:d}k_{:d}mc'.format(steps, mc_sims), data_dict) 144 | 145 | # print out table 146 | # mean overall RMSE and INC with bootstrapped variances 147 | f_label = [f.__class__.__name__ for f in filters] 148 | m_label = ['MEAN_RMSE', 'STD(MEAN_RMSE)', 'MEAN_INC', 'STD(MEAN_INC)'] 149 | data = np.array([rmse_avg.mean(axis=0), np.sqrt(var_rmse_avg), lcr_avg.mean(axis=0), np.sqrt(var_lcr_avg)]).T 150 | table = pd.DataFrame(data, f_label, m_label) 151 | print(table) 152 | 153 | # mean/max RMSE and INC 154 | m_label = ['MEAN_RMSE', 'MAX_RMSE', 'MEAN_INC', 'MAX_INC'] 155 | pos_data = np.array([pos_rmse.mean(axis=0), pos_rmse.max(axis=0), pos_lcr.mean(axis=0), pos_lcr.max(axis=0)]).T 156 | vel_data = np.array([vel_rmse.mean(axis=0), vel_rmse.max(axis=0), vel_lcr.mean(axis=0), vel_lcr.max(axis=0)]).T 157 | pos_table = pd.DataFrame(pos_data, f_label, m_label) 158 | pos_table.index.name = 'Position' 159 | vel_table = pd.DataFrame(vel_data, f_label, m_label) 160 | vel_table.index.name = 'Velocity' 161 | print(pos_table) 162 | print(vel_table) 163 | 164 | # plot metrics 165 | import matplotlib.pyplot as plt 166 | time = np.arange(1, steps + 1) 167 | fig, ax = plt.subplots(2, 1, sharex=True) 168 | for fi, f in enumerate(filters): 169 | ax[0].semilogy(time, pos_rmse[..., fi], label=f.__class__.__name__) 170 | ax[1].semilogy(time, vel_rmse[..., fi], label=f.__class__.__name__) 171 | plt.legend() 172 | plt.show() 173 | 174 | 175 | def constant_velocity_radar_plots_tables(datafile): 176 | 177 | # extract true/filtered state trajectories, measurements and evaluated metrics from *.mat data file 178 | d = loadmat(datafile) 179 | # x, z, mf, Pf = d['x'], d['z'], d['mf'], d['Pf'] 180 | rmse_avg, lcr_avg = d['rmse_avg'], d['lcr_avg'] 181 | var_rmse_avg, var_lcr_avg = d['var_rmse_avg'].squeeze(), d['var_lcr_avg'].squeeze() 182 | pos_rmse, pos_lcr = d['pos_rmse'], d['pos_lcr'] 183 | vel_rmse, vel_lcr = d['vel_rmse'], d['vel_lcr'] 184 | steps, mc_sims = d['steps'], d['mc_sims'] 185 | 186 | # TABLES 187 | import pandas as pd 188 | 189 | # limit display of decimal places 190 | pd.set_option('display.precision', 4) 191 | 192 | # filter/metric labels 193 | # f_label = d['f_label'] 194 | f_label = ['UKF', 'SF', 'TPQSF\n' + r'$(\nu_g=4)$', 'TPQSF\n' + r'$(\nu_g=10)$', 195 | 'TPQSF\n' + r'$(\nu_g=20)$', 'GPQSF'] 196 | m_label = ['MEAN_RMSE', 'STD(MEAN_RMSE)', 'MEAN_INC', 'STD(MEAN_INC)'] 197 | 198 | # form data array, put in DataFrame and print 199 | data = np.array([rmse_avg.mean(axis=0), np.sqrt(var_rmse_avg), lcr_avg.mean(axis=0), np.sqrt(var_lcr_avg)]).T 200 | table = pd.DataFrame(data, f_label, m_label) 201 | print(table) 202 | 203 | # save table to latex 204 | with open('cv_radar_rmse_inc.tex', 'w') as f: 205 | table.to_latex(f) 206 | 207 | # plots 208 | # import matplotlib.pyplot as plt 209 | # from fusion_paper.figprint import FigurePrint 210 | fp = FigurePrint() 211 | 212 | # position and velocity RMSE plots 213 | time = np.arange(1, steps+1) 214 | fig, ax = plt.subplots(2, 1, sharex=True) 215 | 216 | for fi, f in enumerate(f_label): 217 | ax[0].semilogy(time, pos_rmse[..., fi], label=f) 218 | ax[1].semilogy(time, vel_rmse[..., fi], label=f) 219 | ax[0].set_ylabel('Position') 220 | ax[1].set_ylabel('Velocity') 221 | ax[1].set_xlabel('time step [k]') 222 | plt.legend() 223 | plt.tight_layout(pad=0) 224 | fp.savefig('cv_radar_rmse_semilogy') 225 | 226 | # RMSE and INC box plots 227 | fig, ax = plt.subplots() 228 | ax.boxplot(rmse_avg, showfliers=True) 229 | ax.set_ylabel('Average RMSE') 230 | ax.set_ylim(0, 200) 231 | xtickNames = plt.setp(ax, xticklabels=f_label) 232 | # plt.setp(xtickNames, rotation=45, fontsize=8) 233 | plt.tight_layout(pad=0.1) 234 | fp.savefig('cv_radar_rmse_boxplot') 235 | 236 | fig, ax = plt.subplots() 237 | ax.boxplot(lcr_avg) 238 | ax.set_ylabel('Average INC') 239 | xtickNames = plt.setp(ax, xticklabels=f_label) 240 | # plt.setp(xtickNames, rotation=45, fontsize=8) 241 | plt.tight_layout(pad=0.1) 242 | fp.savefig('cv_radar_inc_boxplot') 243 | 244 | -------------------------------------------------------------------------------- /research/tpq/tpq_ungm.py: -------------------------------------------------------------------------------- 1 | from tpq_base import * 2 | from scipy.io import loadmat 3 | from ssmtoybox.ssinf import StudentProcessKalman, StudentProcessStudent, GaussianProcessKalman, UnscentedKalman, \ 4 | CubatureKalman, FullySymmetricStudent 5 | import matplotlib.pyplot as plt 6 | # from fusion_paper.figprint import * 7 | 8 | # def __init__(self): 9 | # pars = { 10 | # 'x0_mean': np.atleast_1d(0.0), 11 | # 'x0_cov': np.atleast_2d(5.0), 12 | # 'q_mean_0': np.zeros(self.qD), 13 | # 'q_mean_1': np.zeros(self.qD), 14 | # 'q_cov_0': 10 * np.eye(self.qD), 15 | # 'q_cov_1': 100 * np.eye(self.qD), 16 | # 'r_mean_0': np.zeros(self.rD), 17 | # 'r_mean_1': np.zeros(self.rD), 18 | # 'r_cov_0': 0.01 * np.eye(self.rD), 19 | # 'r_cov_1': 1 * np.eye(self.rD), 20 | # } 21 | 22 | # 23 | # def __init__(self, x0_mean=0.0, x0_cov=1.0, q_mean=0.0, q_cov=10.0, r_mean=0.0, r_cov=1.0, **kwargs): 24 | # super(UNGM, self).__init__(**kwargs) 25 | # kwargs = { 26 | # 'x0_mean': np.atleast_1d(x0_mean), 27 | # 'x0_cov': np.atleast_2d(x0_cov), 28 | # 'x0_dof': 4.0, 29 | # 'q_mean': np.atleast_1d(q_mean), 30 | # 'q_cov': np.atleast_2d(q_cov), 31 | # 'q_dof': 4.0, 32 | # 'r_mean': np.atleast_1d(r_mean), 33 | # 'r_cov': np.atleast_2d(r_cov), 34 | # 'r_dof': 4.0, 35 | # } 36 | 37 | 38 | def ungm_demo(steps=250, mc_sims=100): 39 | # SYSTEM (data generator): dynamics and measurement 40 | x0_cov = 1.0 41 | q_cov_0, q_cov_1 = 10.0, 100.0 42 | r_cov_0, r_cov_1 = 0.01, 1.0 43 | x0 = GaussRV(1, cov=x0_cov) 44 | zero_means = (np.zeros((1,)), np.zeros((1,))) 45 | gm_weights = np.array([0.8, 0.2]) 46 | q_covs = (np.atleast_2d(q_cov_0), np.atleast_2d(q_cov_1)) 47 | q = GaussianMixtureRV(1, zero_means, q_covs, gm_weights) 48 | dyn = UNGMTransition(x0, q) 49 | 50 | r_covs = (np.atleast_2d(r_cov_0), np.atleast_2d(r_cov_1)) 51 | r = GaussianMixtureRV(1, zero_means, r_covs, gm_weights) 52 | obs = UNGMMeasurement(r, dyn.dim_state) 53 | 54 | # simulate data 55 | x = dyn.simulate_discrete(steps, mc_sims) 56 | z = obs.simulate_measurements(x) 57 | 58 | # STUDENT STATE SPACE MODEL: dynamics and measurement 59 | nu = 4.0 60 | x0 = StudentRV(1, scale=(nu-2)/nu*x0_cov, dof=nu) 61 | q = StudentRV(1, scale=((nu-2)/nu)*q_cov_0, dof=nu) 62 | dyn = UNGMTransition(x0, q) 63 | r = StudentRV(1, scale=((nu-2)/nu)*r_cov_0, dof=nu) 64 | obs = UNGMMeasurement(r, dyn.dim_state) 65 | 66 | # GAUSSIAN SSM for UKF 67 | x0 = GaussRV(1, cov=x0_cov) 68 | q = GaussRV(1, cov=q_cov_0) 69 | dyn_gauss = UNGMTransition(x0, q) 70 | r = GaussRV(1, cov=r_cov_0) 71 | obs_gauss = UNGMMeasurement(r, dyn.dim_state) 72 | 73 | # kernel parameters for TPQ and GPQ filters 74 | # TPQ Student 75 | # par_dyn_tp = np.array([[1.8, 3.0]]) 76 | # par_obs_tp = np.array([[0.4, 1.0, 1.0]]) 77 | par_dyn_tp = np.array([[3.0, 1.0]]) 78 | par_obs_tp = np.array([[3.0, 3.0]]) 79 | # GPQ Student 80 | par_dyn_gpqs = par_dyn_tp 81 | par_obs_gpqs = par_obs_tp 82 | # GPQ Kalman 83 | par_dyn_gpqk = np.array([[1.0, 0.5]]) 84 | par_obs_gpqk = np.array([[1.0, 1, 10]]) 85 | # parameters of the point-set 86 | kappa = 0.0 87 | par_pt = {'kappa': kappa} 88 | 89 | # FIXME: TPQ filters give too similar results, unlike in the paper, likely because I fiddled with DoF choice in TPQ 90 | # init filters 91 | filters = ( 92 | # ExtendedStudent(dyn, obs), 93 | UnscentedKalman(dyn_gauss, obs_gauss, kappa=kappa), 94 | # FullySymmetricStudent(dyn, obs, kappa=kappa, dof=3.0), 95 | FullySymmetricStudent(dyn, obs, kappa=kappa, dof=4.0), 96 | # FullySymmetricStudent(dyn, obs, kappa=kappa, dof=8.0), 97 | # FullySymmetricStudent(dyn, obs, kappa=kappa, dof=100.0), 98 | StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=4.0, dof_tp=3.0, point_par=par_pt), 99 | # StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=3.0, dof_tp=4.0, point_par=par_pt), 100 | StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=4.0, dof_tp=10.0, point_par=par_pt), 101 | # StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=4.0, dof_tp=100.0, point_par=par_pt), 102 | StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=4.0, dof_tp=500.0, point_par=par_pt), 103 | # GaussianProcessStudent(dyn, obs, par_dyn_gpqs, par_obs_gpqs, dof=10.0, point_hyp=par_pt), 104 | # StudentProcessKalman(dyn, obs, par_dyn_gpqk, par_obs_gpqk, points='fs', point_hyp=par_pt), 105 | # GaussianProcessKalman(dyn, obs, par_dyn_tp, par_obs_tp, point_hyp=par_pt), 106 | ) 107 | itpq = np.argwhere([isinstance(filters[i], StudentProcessStudent) for i in range(len(filters))]).squeeze(axis=1)[0] 108 | 109 | # assign weights approximated by MC with lots of samples 110 | # very dirty code 111 | pts = filters[itpq].tf_dyn.model.points 112 | kern = filters[itpq].tf_dyn.model.kernel 113 | wm, wc, wcc, Q = rbf_student_mc_weights(pts, kern, int(1e6), 1000) 114 | for f in filters: 115 | if isinstance(f.tf_dyn, BQTransform): 116 | f.tf_dyn.wm, f.tf_dyn.Wc, f.tf_dyn.Wcc = wm, wc, wcc 117 | f.tf_dyn.Q = Q 118 | pts = filters[itpq].tf_obs.model.points 119 | kern = filters[itpq].tf_obs.model.kernel 120 | wm, wc, wcc, Q = rbf_student_mc_weights(pts, kern, int(1e6), 1000) 121 | for f in filters: 122 | if isinstance(f.tf_obs, BQTransform): 123 | f.tf_obs.wm, f.tf_obs.Wc, f.tf_obs.Wcc = wm, wc, wcc 124 | f.tf_obs.Q = Q 125 | 126 | # print kernel parameters 127 | import pandas as pd 128 | parlab = ['alpha'] + ['ell_{}'.format(d + 1) for d in range(x.shape[0])] 129 | partable = pd.DataFrame(np.vstack((par_dyn_tp, par_obs_tp)), columns=parlab, index=['dyn', 'obs']) 130 | print() 131 | print(partable) 132 | 133 | # run all filters 134 | mf, Pf = run_filters(filters, z) 135 | 136 | # compute average RMSE and INC from filtered trajectories 137 | rmse_avg, lcr_avg = eval_perf_scores(x, mf, Pf) 138 | 139 | # variance of average metrics 140 | from ssmtoybox.utils import bootstrap_var 141 | var_rmse_avg = np.zeros((len(filters),)) 142 | var_lcr_avg = np.zeros((len(filters),)) 143 | for fi in range(len(filters)): 144 | var_rmse_avg[fi] = bootstrap_var(rmse_avg[:, fi], int(1e4)) 145 | var_lcr_avg[fi] = bootstrap_var(lcr_avg[:, fi], int(1e4)) 146 | 147 | # save trajectories, measurements and metrics to file for later processing (tables, plots) 148 | # data_dict = { 149 | # 'x': x, 150 | # 'z': z, 151 | # 'mf': mf, 152 | # 'Pf': Pf, 153 | # 'rmse_avg': rmse_avg, 154 | # 'lcr_avg': lcr_avg, 155 | # 'var_rmse_avg': var_rmse_avg, 156 | # 'var_lcr_avg': var_lcr_avg, 157 | # 'steps': steps, 158 | # 'mc_sims': mc_sims, 159 | # 'par_dyn_tp': par_dyn_tp, 160 | # 'par_obs_tp': par_obs_tp, 161 | # } 162 | # savemat('ungm_simdata_{:d}k_{:d}mc'.format(steps, mc_sims), data_dict) 163 | 164 | f_label = [f.__class__.__name__ for f in filters] 165 | m_label = ['MEAN_RMSE', 'STD(MEAN_RMSE)', 'MEAN_INC', 'STD(MEAN_INC)'] 166 | data = np.array([rmse_avg.mean(axis=0), np.sqrt(var_rmse_avg), lcr_avg.mean(axis=0), np.sqrt(var_lcr_avg)]).T 167 | table = pd.DataFrame(data, f_label, m_label) 168 | pd.set_option('display.max_columns', 6) 169 | print(table) 170 | 171 | 172 | def ungm_plots_tables(datafile): 173 | 174 | # extract true/filtered state trajectories, measurements and evaluated metrics from *.mat data file 175 | d = loadmat(datafile) 176 | x, z, mf, Pf = d['x'], d['z'], d['mf'], d['Pf'] 177 | rmse_avg, lcr_avg = d['rmse_avg'], d['lcr_avg'] 178 | var_rmse_avg, var_lcr_avg = d['var_rmse_avg'].squeeze(), d['var_lcr_avg'].squeeze() 179 | steps, mc_sims = d['steps'], d['mc_sims'] 180 | 181 | # TABLES 182 | import pandas as pd 183 | 184 | # limit display of decimal places 185 | pd.set_option('display.precision', 4) 186 | 187 | # filter/metric labels 188 | f_label = ['UKF', 'SF', r'TPQSF($\nu$=3)', r'TPQSF($\nu$=4)', 189 | r'TPQSF($\nu$=6)', r'TPQSF($\nu$=8)', r'TPQSF($\nu$=10)', 'GPQSF'] 190 | m_label = ['MEAN_RMSE', 'VAR(MEAN_RMSE)', 'MEAN_INC', 'VAR(MEAN_INC)'] 191 | 192 | # form data array, put in DataFrame and print 193 | data = np.array([rmse_avg.mean(axis=0), var_rmse_avg, lcr_avg.mean(axis=0), var_lcr_avg]).T 194 | table = pd.DataFrame(data, f_label, m_label) 195 | print(table) 196 | 197 | # save table to latex 198 | with open('ungm_rmse_inc.tex', 'w') as f: 199 | table.to_latex(f) 200 | 201 | # plots 202 | fp = FigurePrint() 203 | 204 | # RMSE and INC box plots 205 | fig, ax = plt.subplots() 206 | ax.boxplot(rmse_avg) 207 | ax.set_ylabel('Average RMSE') 208 | ax.set_ylim(0, 80) 209 | xtickNames = plt.setp(ax, xticklabels=f_label) 210 | plt.setp(xtickNames, rotation=45, fontsize=8) 211 | plt.tight_layout(pad=0.1) 212 | fp.savefig('ungm_rmse_boxplot') 213 | 214 | fig, ax = plt.subplots() 215 | ax.boxplot(lcr_avg) 216 | ax.set_ylabel('Average INC') 217 | xtickNames = plt.setp(ax, xticklabels=f_label) 218 | plt.setp(xtickNames, rotation=45, fontsize=8) 219 | plt.tight_layout(pad=0.1) 220 | fp.savefig('ungm_inc_boxplot') 221 | 222 | # filtered state and covariance 223 | # fig, ax = plt.subplots(3, 1, sharex=True) 224 | # time = np.arange(1, steps + 1) 225 | # for fi, f in enumerate(filters): 226 | # # true state 227 | # ax[fi].plot(time, x[0, :, 0], 'r--', alpha=0.5) 228 | # 229 | # # measurements 230 | # ax[fi].plot(time, z[0, :, 0], 'k.') 231 | # 232 | # xhat = mf[0, :, 0, fi] 233 | # std = np.sqrt(Pf[0, 0, :, 0, fi]) 234 | # ax[fi].plot(time, xhat, label=f.__class__.__name__) 235 | # ax[fi].fill_between(time, xhat - 2 * std, xhat + 2 * std, alpha=0.15) 236 | # ax[fi].axis([None, None, -50, 50]) 237 | # ax[fi].legend() 238 | # plt.show() 239 | # 240 | # # compare posterior variances with outliers 241 | # plt.figure() 242 | # plt.plot(time, z[0, :, 0], 'k.') 243 | # for fi, f in enumerate(filters): 244 | # plt.plot(time, 2 * np.sqrt(Pf[0, 0, :, 0, fi]), label=f.__class__.__name__) 245 | # plt.legend() 246 | # plt.show() 247 | 248 | 249 | if __name__ == '__main__': 250 | np.set_printoptions(precision=4) 251 | # synthetic_demo(mc_sims=50) 252 | # lotka_volterra_demo() 253 | ungm_demo() 254 | # ungm_plots_tables('ungm_simdata_250k_500mc.mat') 255 | # reentry_tracking_demo() 256 | # constant_velocity_bot_demo() 257 | # constant_velocity_radar_demo() 258 | # constant_velocity_radar_plots_tables('cv_radar_simdata_100k_500mc') 259 | # coordinated_bot_demo(steps=40, mc_sims=100) 260 | # coordinated_radar_demo(steps=100, mc_sims=100, plots=False) 261 | -------------------------------------------------------------------------------- /research/truncated_mt_demo.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from matplotlib.gridspec import GridSpec 4 | 5 | from ssmtoybox.mtran import MomentTransform 6 | from ssmtoybox.mtran import MonteCarloTransform, UnscentedTransform, TruncatedUnscentedTransform 7 | from ssmtoybox.ssinf import TruncatedUnscentedKalman, UnscentedKalman 8 | from ssmtoybox.ssmod import ReentryVehicle2DTransition, Radar2DMeasurement 9 | from ssmtoybox.utils import GaussRV 10 | from ssmtoybox.utils import ellipse_points, symmetrized_kl_divergence, squared_error, log_cred_ratio, mse_matrix 11 | 12 | 13 | def cartesian2polar(x, pars, dx=False): 14 | return np.array([np.sqrt(x[0] ** 2 + x[1] ** 2), np.arctan2(x[1], x[0])]) 15 | 16 | 17 | def polar2cartesian(x, pars, dx=False): 18 | return x[0] * np.array([np.cos(x[1]), np.sin(x[1])]) 19 | 20 | 21 | def mt_trunc_demo(mt_trunc, mt, dim=None, full_input_cov=True, **kwargs): 22 | """ 23 | Comparison of truncated MT and vanilla MT on polar2cartesian transform for increasing state dimensions. The 24 | truncated MT is aware of the effective dimension, so we expect it to be closer to the true covariance 25 | 26 | Observation: Output covariance of the Truncated UT stays closer to the MC baseline than the covariance 27 | produced by the vanilla UT. 28 | 29 | There's some merit to the idea, but the problem is with computing the input-output cross-covariance. 30 | 31 | 32 | Parameters 33 | ---------- 34 | mt_trunc 35 | mt 36 | dim 37 | full_input_cov : boolean 38 | If `False`, a diagonal input covariance is used, otherwise a full covariance is used. 39 | kwargs 40 | 41 | Returns 42 | ------- 43 | 44 | """ 45 | 46 | assert issubclass(mt_trunc, MomentTransform) and issubclass(mt, MomentTransform) 47 | 48 | # state dimensions and effective dimension 49 | dim = [2, 3, 4, 5] if dim is None else dim 50 | d_eff = 2 51 | 52 | # nonlinear integrand 53 | f = polar2cartesian 54 | 55 | # input mean and covariance 56 | mean_eff, cov_eff = np.array([1, np.pi / 2]), np.diag([0.05 ** 2, (np.pi / 10) ** 2]) 57 | 58 | if full_input_cov: 59 | A = np.random.rand(d_eff, d_eff) 60 | cov_eff = A.dot(cov_eff).dot(A.T) 61 | 62 | # use MC transform with lots of samples to compute the true transformed moments 63 | tmc = MonteCarloTransform(d_eff, n=1e4) 64 | M_mc, C_mc, cc_mc = tmc.apply(f, mean_eff, cov_eff, None) 65 | # transformed samples 66 | x = np.random.multivariate_normal(mean_eff, cov_eff, size=int(1e3)).T 67 | fx = np.apply_along_axis(f, 0, x, None) 68 | X_mc = ellipse_points(M_mc, C_mc) 69 | 70 | M = np.zeros((2, len(dim), 2)) 71 | C = np.zeros((2, 2, len(dim), 2)) 72 | X = np.zeros((2, 50, len(dim), 2)) 73 | for i, d in enumerate(dim): 74 | t = mt_trunc(d, d_eff, **kwargs) 75 | s = mt(d, **kwargs) 76 | 77 | # input mean and covariance 78 | mean, cov = np.zeros(d), np.eye(d) 79 | mean[:d_eff], cov[:d_eff, :d_eff] = mean_eff, cov_eff 80 | 81 | # transformed moments (w/o cross-covariance) 82 | M[:, i, 0], C[..., i, 0], cc = t.apply(f, mean, cov, None) 83 | M[:, i, 1], C[..., i, 1], cc = s.apply(f, mean, cov, None) 84 | 85 | # points on the ellipse defined by the transformed mean and covariance for plotting 86 | X[..., i, 0] = ellipse_points(M[:, i, 0], C[..., i, 0]) 87 | X[..., i, 1] = ellipse_points(M[:, i, 1], C[..., i, 1]) 88 | 89 | # PLOTS: transformed samples, MC mean and covariance ground truth 90 | fig, ax = plt.subplots(1, 2) 91 | ax[0].plot(fx[0, :], fx[1, :], 'k.', alpha=0.15) 92 | ax[0].plot(M_mc[0], M_mc[1], 'ro', markersize=6, lw=2) 93 | ax[0].plot(X_mc[0, :], X_mc[1, :], 'r--', lw=2, label='MC') 94 | 95 | # SR and SR-T mean and covariance for various state dimensions 96 | # TODO: it's more effective to plot SKL between the transformed and baseline covariances. 97 | for i, d in enumerate(dim): 98 | ax[0].plot(M[0, i, 0], M[1, i, 0], 'b+', markersize=10, lw=2) 99 | ax[0].plot(X[0, :, i, 0], X[1, :, i, 0], color='b', label='mt-trunc (d={})'.format(d)) 100 | for i, d in enumerate(dim): 101 | ax[0].plot(M[0, i, 1], M[1, i, 1], 'go', markersize=6) 102 | ax[0].plot(X[0, :, i, 1], X[1, :, i, 1], color='g', label='mt (d={})'.format(d)) 103 | ax[0].set_aspect('equal') 104 | plt.legend() 105 | 106 | # symmetrized KL-divergence 107 | skl = np.zeros((len(dim), 2)) 108 | for i, d in enumerate(dim): 109 | skl[i, 0] = symmetrized_kl_divergence(M_mc, C_mc, M[:, i, 0], C[..., i, 0]) 110 | skl[i, 1] = symmetrized_kl_divergence(M_mc, C_mc, M[:, i, 1], C[..., i, 1]) 111 | plt_opt = {'lw': 2, 'marker': 'o'} 112 | ax[1].plot(dim, skl[:, 0], label='truncated', **plt_opt) 113 | ax[1].plot(dim, skl[:, 1], label='original', **plt_opt) 114 | ax[1].set_xticks(dim) 115 | ax[1].set_xlabel('Dimension') 116 | ax[1].set_ylabel('SKL') 117 | plt.legend() 118 | plt.show() 119 | 120 | 121 | def ukf_trunc_demo(mc_sims=50): 122 | disc_tau = 0.5 # discretization period in seconds 123 | duration = 200 124 | 125 | # define system 126 | m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932]) 127 | P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 0]) 128 | x0 = GaussRV(5, m0, P0) 129 | q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 0])) 130 | sys = ReentryVehicle2DTransition(x0, q, dt=disc_tau) 131 | # define radar measurement model 132 | r = GaussRV(2, cov=np.diag([1e-6, 0.17e-6])) 133 | obs = Radar2DMeasurement(r, sys.dim_state) 134 | 135 | # simulate reference state trajectory by SDE integration 136 | x = sys.simulate_continuous(duration, disc_tau, mc_sims) 137 | x_ref = x.mean(axis=2) 138 | # simulate corresponding radar measurements 139 | y = obs.simulate_measurements(x) 140 | 141 | # initialize state-space model; uses cartesian2polar as measurement (not polar2cartesian) 142 | P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1]) 143 | x0 = GaussRV(5, m0, P0) 144 | q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 1e-6])) 145 | dyn = ReentryVehicle2DTransition(x0, q, dt=disc_tau) 146 | 147 | # initialize UKF and UKF in truncated version 148 | alg = ( 149 | UnscentedKalman(dyn, obs), 150 | TruncatedUnscentedKalman(dyn, obs), 151 | ) 152 | num_alg = len(alg) 153 | 154 | # space for filtered mean and covariance 155 | steps = x.shape[1] 156 | x_mean = np.zeros((dyn.dim_in, steps, mc_sims, num_alg)) 157 | x_cov = np.zeros((dyn.dim_in, dyn.dim_in, steps, mc_sims, num_alg)) 158 | 159 | # filtering estimate of the state trajectory based on provided measurements 160 | from tqdm import trange 161 | for i_est, estimator in enumerate(alg): 162 | for i_mc in trange(mc_sims): 163 | x_mean[..., i_mc, i_est], x_cov[..., i_mc, i_est] = estimator.forward_pass(y[..., i_mc]) 164 | estimator.reset() 165 | 166 | # Plots 167 | plt.figure() 168 | g = GridSpec(2, 4) 169 | plt.subplot(g[:, :2]) 170 | 171 | # Earth surface w/ radar position 172 | radar_x, radar_y = dyn.R0, 0 173 | t = 0.02 * np.arange(-1, 4, 0.1) 174 | plt.plot(dyn.R0 * np.cos(t), dyn.R0 * np.sin(t), color='darkblue', lw=2) 175 | plt.plot(radar_x, radar_y, 'ko') 176 | 177 | plt.plot(x_ref[0, :], x_ref[1, :], color='r', ls='--') 178 | # Convert from polar to cartesian 179 | meas = np.stack(( + y[0, ...] * np.cos(y[1, ...]), radar_y + y[0, ...] * np.sin(y[1, ...])), axis=0) 180 | for i in range(mc_sims): 181 | # Vehicle trajectory 182 | # plt.plot(x[0, :, i], x[1, :, i], alpha=0.35, color='r', ls='--') 183 | 184 | # Plot measurements 185 | plt.plot(meas[0, :, i], meas[1, :, i], 'k.', alpha=0.3) 186 | 187 | # Filtered position estimate 188 | plt.plot(x_mean[0, 1:, i, 0], x_mean[1, 1:, i, 0], color='g', alpha=0.3) 189 | plt.plot(x_mean[0, 1:, i, 1], x_mean[1, 1:, i, 1], color='orange', alpha=0.3) 190 | 191 | # Performance score plots 192 | error2 = x_mean.copy() 193 | lcr = np.zeros((steps, mc_sims, num_alg)) 194 | for a in range(num_alg): 195 | for k in range(steps): 196 | mse = mse_matrix(x[:4, k, :], x_mean[:4, k, :, a]) 197 | for imc in range(mc_sims): 198 | error2[:, k, imc, a] = squared_error(x[:, k, imc], x_mean[:, k, imc, a]) 199 | lcr[k, imc, a] = log_cred_ratio(x[:4, k, imc], x_mean[:4, k, imc, a], x_cov[:4, :4, k, imc, a], mse) 200 | 201 | # Averaged RMSE and Inclination Indicator in time 202 | pos_rmse_vs_time = np.sqrt((error2[:2, ...]).sum(axis=0)).mean(axis=1) 203 | inc_ind_vs_time = lcr.mean(axis=1) 204 | 205 | # Plots 206 | plt.subplot(g[0, 2:]) 207 | plt.title('RMSE') 208 | plt.plot(pos_rmse_vs_time[:, 0], label='UKF', color='g') 209 | plt.plot(pos_rmse_vs_time[:, 1], label='UKF-trunc', color='r') 210 | plt.legend() 211 | plt.subplot(g[1, 2:]) 212 | plt.title('Inclination Indicator $I^2$') 213 | plt.plot(inc_ind_vs_time[:, 0], label='UKF', color='g') 214 | plt.plot(inc_ind_vs_time[:, 1], label='UKF-trunc', color='r') 215 | plt.legend() 216 | plt.show() 217 | 218 | print('Average RMSE: {}'.format(pos_rmse_vs_time.mean(axis=0))) 219 | print('Average I2: {}'.format(inc_ind_vs_time.mean(axis=0))) 220 | 221 | 222 | if __name__ == '__main__': 223 | # truncated transform significantly improves the SKL between the true and approximate Gaussian 224 | mt_trunc_demo(TruncatedUnscentedTransform, UnscentedTransform, [2, 5, 10, 15], kappa=0, full_input_cov=True) 225 | 226 | # truncated transform performance virtually identical when applied in filtering 227 | # ukf_trunc_demo(mc_sims=50) 228 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [nosetests] 2 | 3 | verbose=1 4 | 5 | nocapture=1 -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | 3 | with open('README.md') as file: 4 | ld = file.read() 5 | 6 | setuptools.setup( 7 | name='ssmtoybox', 8 | version='0.1.1a0', 9 | url='https://github.com/jacobnzw/SSMToybox/tree/v0.1.1-alpha', 10 | license='MIT', 11 | author='Jakub Prüher', 12 | author_email='jacobnzw@gmail.com', 13 | description='Local filters based on Bayesian quadrature', 14 | long_description=ld, 15 | long_description_content_type='text/markdown', 16 | packages=setuptools.find_packages(), 17 | zip_safe=False, 18 | setup_requires=['nose>=1.0'], 19 | test_suite='nose.collector', 20 | classifiers=[ 21 | 'Programming Language :: Python :: 3', 22 | 'License :: OSI Approved :: MIT License', 23 | 'Operating System :: OS Independent', 24 | 'Development Status :: 3 - Alpha', 25 | 'Intended Audience :: Education', 26 | 'Intended Audience :: Science/Research', 27 | ] 28 | ) -------------------------------------------------------------------------------- /ssmtoybox/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jacobnzw/SSMToybox/1e2a8fd0634f6d53a30e24c21465c8e32c88999b/ssmtoybox/__init__.py -------------------------------------------------------------------------------- /ssmtoybox/bq/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jacobnzw/SSMToybox/1e2a8fd0634f6d53a30e24c21465c8e32c88999b/ssmtoybox/bq/__init__.py -------------------------------------------------------------------------------- /ssmtoybox/tests/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jacobnzw/SSMToybox/1e2a8fd0634f6d53a30e24c21465c8e32c88999b/ssmtoybox/tests/__init__.py -------------------------------------------------------------------------------- /ssmtoybox/tests/test_bqkern.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import numpy as np 4 | import numpy.linalg as la 5 | from numpy import newaxis as na 6 | 7 | from ssmtoybox.bq.bqkern import RBFGauss, RBFStudent 8 | 9 | 10 | class RBFKernelTest(TestCase): 11 | 12 | @classmethod 13 | def setUpClass(cls): 14 | cls.par_1d = np.array([[1, 3]]) 15 | cls.par_2d = np.array([[1, 3, 3]]) 16 | cls.kern_rbf_1d = RBFGauss(1, cls.par_1d) 17 | cls.kern_rbf_2d = RBFGauss(2, cls.par_2d) 18 | cls.data_1d = np.array([[1, -1, 0]], dtype=float) 19 | cls.data_2d = np.hstack((np.zeros((2, 1)), np.eye(2), -np.eye(2))) 20 | cls.test_data_1d = np.atleast_2d(np.linspace(-5, 5, 50)) 21 | cls.test_data_2d = np.random.multivariate_normal(np.zeros((2,)), np.eye(2), 50).T 22 | 23 | def test_eval(self): 24 | def kern_eval(x1, x2, par): 25 | # define straightforward implementation to check easily 26 | dim, num_pts_1 = x1.shape 27 | dim, num_pts_2 = x2.shape 28 | assert dim == par.shape[1]-1 29 | alpha = par[0, 0] 30 | Li = np.linalg.inv(np.diag(par[0, 1:] ** 2)) 31 | K = np.zeros((num_pts_1, num_pts_2)) 32 | for i in range(num_pts_1): 33 | for j in range(num_pts_2): 34 | dx = x1[:, i] - x2[:, j] 35 | K[i, j] = np.exp(-0.5 * (dx.T.dot(Li).dot(dx))) 36 | return alpha**2 * K 37 | 38 | # check dimension, shape, symmetry and positive definiteness 39 | K = self.kern_rbf_1d.eval(self.par_1d, self.data_1d) 40 | self.assertTrue(K.ndim == 2) 41 | self.assertTrue(K.shape == (3, 3)) 42 | self.assertTrue(np.array_equal(K, K.T)) 43 | la.cholesky(K) 44 | # same result as the obvious implementation? 45 | K_true = kern_eval(self.data_1d, self.data_1d, self.par_1d) 46 | self.assertTrue(np.array_equal(K, K_true)) 47 | 48 | # higher-dimensional inputs 49 | K = self.kern_rbf_2d.eval(self.par_2d, self.data_2d) 50 | self.assertTrue(K.ndim == 2) 51 | self.assertTrue(K.shape == (5, 5)) 52 | self.assertTrue(np.array_equal(K, K.T)) 53 | la.cholesky(K) 54 | # same result as the obvious implementation? 55 | K_true = kern_eval(self.data_2d, self.data_2d, self.par_2d) 56 | self.assertTrue(np.array_equal(K, K_true)) 57 | 58 | # check computation of cross-covariances kx, kxx 59 | kx = self.kern_rbf_1d.eval(self.par_1d, self.test_data_1d, self.data_1d) 60 | kxx = self.kern_rbf_1d.eval(self.par_1d, self.test_data_1d, self.test_data_1d) 61 | kxx_diag = self.kern_rbf_1d.eval(self.par_1d, self.test_data_1d, self.test_data_1d, diag=True) 62 | self.assertTrue(kx.shape == (50, 3)) 63 | self.assertTrue(kxx.shape == (50, 50)) 64 | self.assertTrue(kxx_diag.shape == (50,)) 65 | 66 | kx = self.kern_rbf_2d.eval(self.par_2d, self.test_data_2d, self.data_2d) 67 | kxx = self.kern_rbf_2d.eval(self.par_2d, self.test_data_2d, self.test_data_2d) 68 | kxx_diag = self.kern_rbf_2d.eval(self.par_2d, self.test_data_2d, self.test_data_2d, diag=True) 69 | self.assertTrue(kx.shape == (50, 5)) 70 | self.assertTrue(kxx.shape == (50, 50)) 71 | self.assertTrue(kxx_diag.shape == (50,)) 72 | 73 | def test_exp_x_kx(self): 74 | def kx_eval(x, par): 75 | # simple straightforward easy to check implementation 76 | dim, num_pts = x.shape 77 | assert dim == par.shape[1]-1 78 | alpha = par[0, 0] 79 | L = np.diag(par[0, 1:] ** 2) 80 | A = np.linalg.inv(L + np.eye(dim)) 81 | c = alpha**2 * np.linalg.det(np.linalg.inv(L) + np.eye(dim)) ** (-0.5) 82 | q = np.zeros((num_pts, )) 83 | for i in range(num_pts): 84 | q[i] = c * np.exp(-0.5*(x[:, i].T.dot(A).dot(x[:, i]))) 85 | return q 86 | 87 | q = self.kern_rbf_1d.exp_x_kx(self.par_1d, self.data_1d) 88 | q_true = kx_eval(self.data_1d, self.par_1d) 89 | self.assertTrue(q.shape == (3,)) 90 | self.assertTrue(np.alltrue(q >= 0)) 91 | self.assertTrue(np.array_equal(q, q_true)) 92 | 93 | q = self.kern_rbf_2d.exp_x_kx(self.par_2d, self.data_2d) 94 | q_true = kx_eval(self.data_2d, self.par_2d) 95 | self.assertTrue(q.shape == (5,)) 96 | self.assertTrue(np.alltrue(q >= 0)) 97 | self.assertTrue(np.array_equal(q, q_true)) 98 | 99 | def test_exp_x_kxx(self): 100 | self.kern_rbf_1d.exp_x_kxx(self.par_1d) 101 | self.kern_rbf_2d.exp_x_kxx(self.par_2d) 102 | 103 | def test_exp_xy_kxy(self): 104 | self.kern_rbf_1d.exp_xy_kxy(self.par_1d) 105 | self.kern_rbf_2d.exp_xy_kxy(self.par_2d) 106 | 107 | def test_exp_x_xkx(self): 108 | def xkx_eval(x, par): 109 | # simple straightforward easy to check implementation 110 | dim, num_pts = x.shape 111 | assert dim == par.shape[1]-1 112 | alpha = par[0, 0] 113 | L = np.diag(par[0, 1:] ** 2) 114 | A = np.linalg.inv(L + np.eye(dim)) 115 | c = alpha**2 * np.linalg.det(np.linalg.inv(L) + np.eye(dim)) ** (-0.5) 116 | R = np.zeros(x.shape) 117 | for i in range(num_pts): 118 | R[:, i] = c * np.exp(-0.5*(x[:, i].T.dot(A).dot(x[:, i]))) * (A.dot(x[:, i])) 119 | return R 120 | 121 | r = self.kern_rbf_1d.exp_x_xkx(self.par_1d, self.data_1d) 122 | self.assertTrue(r.shape == (1, 3)) 123 | r_true = xkx_eval(self.data_1d, self.par_1d) 124 | self.assertTrue(np.allclose(r, r_true)) 125 | 126 | r = self.kern_rbf_2d.exp_x_xkx(self.par_2d, self.data_2d) 127 | r_true = xkx_eval(self.data_2d, self.par_2d) 128 | self.assertTrue(r.shape == (2, 5)) 129 | self.assertTrue(np.allclose(r, r_true)) 130 | 131 | def test_exp_x_kxkx(self): 132 | q = self.kern_rbf_1d.exp_x_kxkx(self.par_1d, self.par_1d, self.data_1d) 133 | self.assertTrue(q.shape == (3, 3)) 134 | self.assertTrue(np.array_equal(q, q.T), 'Result not symmetric.') 135 | la.cholesky(q) 136 | 137 | q = self.kern_rbf_2d.exp_x_kxkx(self.par_2d, self.par_2d, self.data_2d) 138 | self.assertTrue(q.shape == (5, 5)) 139 | self.assertTrue(np.array_equal(q, q.T), 'Result not symmetric.') 140 | la.cholesky(q) 141 | 142 | def test_mc_verification(self): 143 | dim = 2 144 | 145 | q = self.kern_rbf_2d.exp_x_kx(self.par_2d, self.data_2d) 146 | Q = self.kern_rbf_2d.exp_x_kxkx(self.par_2d, self.par_2d, self.data_2d) 147 | R = self.kern_rbf_2d.exp_x_xkx(self.par_2d, self.data_2d) 148 | 149 | # approximate expectations using cumulative moving average MC 150 | def cma_mc(new_samples, old_avg, old_avg_size, axis=0): 151 | b_size = new_samples.shape[axis] 152 | return (new_samples.sum(axis=axis) + old_avg_size * old_avg) / (old_avg_size + b_size) 153 | 154 | batch_size = 100000 155 | num_iter = 100 156 | q_mc, Q_mc, R_mc = 0, 0, 0 157 | for i in range(num_iter): 158 | # sample from standard Gaussian 159 | x_samples = np.random.multivariate_normal(np.zeros((dim, )), np.eye(dim), size=batch_size).T 160 | k = self.kern_rbf_2d.eval(self.par_2d, x_samples, self.data_2d, scaling=False) 161 | q_mc = cma_mc(k, q_mc, i*batch_size, axis=0) 162 | Q_mc = cma_mc(k[:, na, :] * k[..., na], Q_mc, i*batch_size, axis=0) 163 | R_mc = cma_mc(x_samples[..., na] * k[na, ...], R_mc, i*batch_size, axis=1) 164 | 165 | # compare MC approximates with analytic expressions 166 | tol = 2e-3 167 | print('Norm of the difference using {:d} samples.'.format(batch_size*num_iter)) 168 | print('q {:.2e}'.format(np.linalg.norm(q - q_mc))) 169 | print('Q {:.2e}'.format(np.linalg.norm(Q - Q_mc))) 170 | print('R {:.2e}'.format(np.linalg.norm(R - R_mc))) 171 | self.assertLessEqual(np.linalg.norm(q - q_mc), tol) 172 | self.assertLessEqual(np.linalg.norm(Q - Q_mc), tol) 173 | self.assertLessEqual(np.linalg.norm(R - R_mc), tol) 174 | 175 | def test_par_gradient(self): 176 | dim = 2 177 | x = np.hstack((np.zeros((dim, 1)), np.eye(dim), -np.eye(dim))) 178 | y = x[0, :] 179 | 180 | par = np.array([[1, 1, 3]], dtype=float) 181 | kernel = RBFGauss(dim, par) 182 | dK_dpar = kernel.der_par(par.squeeze(), x) 183 | 184 | 185 | class RBFStudentKernelTest(TestCase): 186 | 187 | @classmethod 188 | def setUpClass(cls): 189 | from ssmtoybox.mtran import FullySymmetricStudentTransform 190 | cls.points = FullySymmetricStudentTransform.unit_sigma_points(2) 191 | cls.num_pts = cls.points.shape[1] 192 | 193 | def test_expectations_dim(self): 194 | dim = 2 195 | par = np.array([[1.5, 3.0, 3.0]]) 196 | ker = RBFStudent(dim, par, num_samples=10000, num_batches=10) 197 | 198 | q = ker.exp_x_kx(par, self.points) 199 | self.assertTrue(q.shape == (self.num_pts, )) 200 | 201 | Q = ker.exp_x_kxkx(par, par, self.points) 202 | self.assertTrue(Q.shape == (self.num_pts, self.num_pts)) 203 | self.assertTrue(np.array_equal(Q, Q.T), 'Q not symmetric') 204 | 205 | R = ker.exp_x_xkx(par, self.points) 206 | self.assertTrue(R.shape == (dim, self.num_pts)) 207 | 208 | kbar = ker.exp_x_kxx(par) 209 | self.assertTrue(kbar.shape == ()) 210 | 211 | kbarbar = ker.exp_xy_kxy(par) 212 | self.assertTrue(kbarbar.shape == ()) 213 | 214 | def test_expectation_xy_kxy(self): 215 | dim = 2 216 | par = np.array([[1.5, 3.0, 3.0]]) 217 | ker = RBFStudent(dim, par, num_samples=2e6, num_batches=10000) 218 | 219 | import time 220 | t0 = time.time() 221 | ker.exp_xy_kxy(par) 222 | elapsed = time.time() - t0 223 | print('{:.6f} [sec] elapsed'.format(elapsed)) 224 | # import cProfile 225 | # cProfile.runctx('ker.exp_xy_kxy(par)', None, {'ker': ker, 'par': par, 'dim': dim}) 226 | 227 | def test_expectation_x_kxkx(self): 228 | dim = 2 229 | par = np.array([[1.5, 3.0, 3.0]]) 230 | ker = RBFStudent(dim, par, num_samples=2e6, num_batches=1000) 231 | 232 | import time 233 | t0 = time.time() 234 | ker.exp_x_kxkx(par, par, self.points) 235 | elapsed = time.time() - t0 236 | print('{:.6f} [sec] elapsed'.format(elapsed)) 237 | -------------------------------------------------------------------------------- /ssmtoybox/tests/test_bqmtran.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | from unittest import TestCase 3 | 4 | import numpy as np 5 | import numpy.linalg as la 6 | 7 | from ssmtoybox.bq.bqmtran import GaussianProcessTransform, MultiOutputGaussianProcessTransform, BayesSardTransform 8 | from ssmtoybox.ssmod import UNGMTransition, Pendulum2DTransition, CoordinatedTurnTransition, ReentryVehicle2DTransition 9 | from ssmtoybox.utils import GaussRV 10 | 11 | np.set_printoptions(precision=4) 12 | 13 | 14 | class GPQuadTest(TestCase): 15 | 16 | @classmethod 17 | def setUpClass(cls): 18 | cls.models = [] 19 | cls.models.append(UNGMTransition(GaussRV(1), GaussRV(1))) 20 | cls.models.append(Pendulum2DTransition(GaussRV(2), GaussRV(2))) 21 | 22 | def test_weights_rbf(self): 23 | dim = 1 24 | khyp = np.array([[1, 3]], dtype=np.float) 25 | phyp = {'kappa': 0.0, 'alpha': 1.0} 26 | tf = GaussianProcessTransform(dim, 1, khyp, point_par=phyp) 27 | wm, wc, wcc = tf.wm, tf.Wc, tf.Wcc 28 | print('wm = \n{}\nwc = \n{}\nwcc = \n{}'.format(wm, wc, wcc)) 29 | self.assertTrue(np.allclose(wc, wc.T), "Covariance weight matrix not symmetric.") 30 | # print 'GP model variance: {}'.format(tf.model.exp_model_variance()) 31 | 32 | dim = 2 33 | khyp = np.array([[1, 3, 3]], dtype=np.float) 34 | phyp = {'kappa': 0.0, 'alpha': 1.0} 35 | tf = GaussianProcessTransform(dim, 1, khyp, point_par=phyp) 36 | wm, wc, wcc = tf.wm, tf.Wc, tf.Wcc 37 | print('wm = \n{}\nwc = \n{}\nwcc = \n{}'.format(wm, wc, wcc)) 38 | self.assertTrue(np.allclose(wc, wc.T), "Covariance weight matrix not symmetric.") 39 | 40 | def test_rbf_scaling_invariance(self): 41 | dim = 5 42 | ker_par = np.array([[1, 3, 3, 3, 3, 3]], dtype=np.float) 43 | tf = GaussianProcessTransform(dim, 1, ker_par) 44 | w0 = tf.weights([1] + dim * [1000]) 45 | w1 = tf.weights([358.0] + dim * [1000.0]) 46 | self.assertTrue(np.alltrue([np.array_equal(a, b) for a, b in zip(w0, w1)])) 47 | 48 | def test_expected_model_variance(self): 49 | dim = 2 50 | ker_par = np.array([[1, 3, 3]], dtype=np.float) 51 | tf = GaussianProcessTransform(dim, 1, ker_par, point_str='sr') 52 | emv0 = tf.model.exp_model_variance(ker_par) 53 | emv1 = tf.model.exp_model_variance(ker_par) 54 | # expected model variance must be positive even for numerically unpleasant settings 55 | self.assertTrue(np.alltrue(np.array([emv0, emv1]) >= 0)) 56 | 57 | def test_integral_variance(self): 58 | dim = 2 59 | ker_par = np.array([[1, 3, 3]], dtype=np.float) 60 | tf = GaussianProcessTransform(dim, 1, ker_par, point_str='sr') 61 | ivar0 = tf.model.integral_variance([1, 600, 6]) 62 | ivar1 = tf.model.integral_variance([1.1, 600, 6]) 63 | # expected model variance must be positive even for numerically unpleasant settings 64 | self.assertTrue(np.alltrue(np.array([ivar0, ivar1]) >= 0)) 65 | 66 | def test_apply(self): 67 | for mod in self.models: 68 | f = mod.dyn_eval 69 | dim = mod.dim_in 70 | ker_par = np.hstack((np.ones((1, 1)), 3*np.ones((1, dim)))) 71 | tf = GaussianProcessTransform(dim, dim, ker_par) 72 | mean, cov = np.zeros(dim, ), np.eye(dim) 73 | tmean, tcov, tccov = tf.apply(f, mean, cov, np.atleast_1d(1.0)) 74 | print("Transformed moments\nmean: {}\ncov: {}\nccov: {}".format(tmean, tcov, tccov)) 75 | 76 | self.assertTrue(tf.I_out.shape == (dim, dim)) 77 | # test positive definiteness 78 | try: 79 | la.cholesky(tcov) 80 | except la.LinAlgError: 81 | self.fail("Output covariance not positive definite.") 82 | 83 | # test symmetry 84 | self.assertTrue(np.allclose(tcov, tcov.T), "Output covariance not closely symmetric.") 85 | # self.assertTrue(np.array_equal(tcov, tcov.T), "Output covariance not exactly symmetric.") 86 | 87 | 88 | class BSQTransformTest(TestCase): 89 | def test_polar2cartesian(self): 90 | def polar2cartesian(x, pars): 91 | return x[0] * np.array([np.cos(x[1]), np.sin(x[1])]) 92 | 93 | mean_in = np.array([1, np.pi / 2]) 94 | cov_in = np.diag([0.05 ** 2, (np.pi / 10) ** 2]) 95 | alpha_ut = np.array([[0, 1, 0, 2, 0], 96 | [0, 0, 1, 0, 2]]) 97 | par = np.array([[1.0, 1, 1]]) 98 | mt = BayesSardTransform(2, 2, par, multi_ind=alpha_ut, point_str='ut') 99 | mean_out, cov_out, cc = mt.apply(polar2cartesian, mean_in, cov_in, np.atleast_1d(0)) 100 | self.assertTrue(mt.I_out.shape == (2, 2)) 101 | try: 102 | la.cholesky(cov_out) 103 | except la.LinAlgError: 104 | self.fail("Weights not positive definite. Min eigval: {}".format(la.eigvalsh(cov_out).min())) 105 | 106 | 107 | @unittest.skip('Multi-output models are experimental and not a priority.') 108 | class GPQMOTest(TestCase): 109 | 110 | @classmethod 111 | def setUpClass(cls): 112 | cls.models = [] 113 | cls.models.append(UNGMTransition(GaussRV(1), GaussRV(1))) 114 | cls.models.append(Pendulum2DTransition(GaussRV(2), GaussRV(2))) 115 | 116 | def test_weights_rbf(self): 117 | dim_in, dim_out = 1, 1 118 | khyp = np.array([[1, 3]]) 119 | phyp = {'kappa': 0.0, 'alpha': 1.0} 120 | tf = MultiOutputGaussianProcessTransform(dim_in, dim_out, khyp, point_par=phyp) 121 | wm, wc, wcc = tf.wm, tf.Wc, tf.Wcc 122 | self.assertTrue(np.allclose(wc, wc.swapaxes(0, 1).swapaxes(2, 3)), "Covariance weight matrix not symmetric.") 123 | 124 | dim_in, dim_out = 4, 4 125 | khyp = np.array([[1, 3, 3, 3, 3], 126 | [1, 1, 1, 1, 1], 127 | [1, 2, 2, 2, 2], 128 | [1, 3, 3, 3, 3]]) 129 | phyp = {'kappa': 0.0, 'alpha': 1.0} 130 | tf = MultiOutputGaussianProcessTransform(dim_in, dim_out, khyp, point_par=phyp) 131 | wm, wc, wcc = tf.wm, tf.Wc, tf.Wcc 132 | self.assertTrue(np.allclose(wc, wc.swapaxes(0, 1).swapaxes(2, 3)), "Covariance weight matrix not symmetric.") 133 | 134 | def test_apply(self): 135 | dyn = Pendulum2DTransition(GaussRV(2), GaussRV(2)) 136 | f = dyn.dyn_eval 137 | dim_in, dim_out = dyn.dim_in, dyn.dim_state 138 | ker_par = np.hstack((np.ones((dim_out, 1)), 3*np.ones((dim_out, dim_in)))) 139 | tf = MultiOutputGaussianProcessTransform(dim_in, dim_out, ker_par) 140 | mean, cov = np.zeros(dim_in, ), np.eye(dim_in) 141 | tmean, tcov, tccov = tf.apply(f, mean, cov, np.atleast_1d(1.0)) 142 | print("Transformed moments\nmean: {}\ncov: {}\nccov: {}".format(tmean, tcov, tccov)) 143 | 144 | # test positive definiteness 145 | try: 146 | la.cholesky(tcov) 147 | except la.LinAlgError: 148 | self.fail("Output covariance not positive definite.") 149 | 150 | # test symmetry 151 | self.assertTrue(np.allclose(tcov, tcov.T), "Output covariance not closely symmetric.") 152 | # self.assertTrue(np.array_equal(tcov, tcov.T), "Output covariance not exactly symmetric.") 153 | 154 | def test_single_vs_multi_output(self): 155 | # results of the GPQ and GPQMO should be same if parameters properly chosen, GPQ is a special case of GPQMO 156 | m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932]) 157 | P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1]) 158 | x0 = GaussRV(5, m0, P0) 159 | dyn = ReentryVehicle2DTransition(x0, GaussRV(5)) 160 | f = dyn.dyn_eval 161 | dim_in, dim_out = dyn.dim_in, dyn.dim_state 162 | 163 | # input mean and covariance 164 | mean_in, cov_in = m0, P0 165 | 166 | # single-output GPQ 167 | ker_par_so = np.hstack((np.ones((1, 1)), 25 * np.ones((1, dim_in)))) 168 | tf_so = GaussianProcessTransform(dim_in, dim_out, ker_par_so) 169 | 170 | # multi-output GPQ 171 | ker_par_mo = np.hstack((np.ones((dim_out, 1)), 25 * np.ones((dim_out, dim_in)))) 172 | tf_mo = MultiOutputGaussianProcessTransform(dim_in, dim_out, ker_par_mo) 173 | 174 | # transformed moments 175 | # FIXME: transformed covariances different 176 | mean_so, cov_so, ccov_so = tf_so.apply(f, mean_in, cov_in, np.atleast_1d(0)) 177 | mean_mo, cov_mo, ccov_mo = tf_mo.apply(f, mean_in, cov_in, np.atleast_1d(0)) 178 | 179 | print('mean delta: {}'.format(np.abs(mean_so - mean_mo).max())) 180 | print('cov delta: {}'.format(np.abs(cov_so - cov_mo).max())) 181 | print('ccov delta: {}'.format(np.abs(ccov_so - ccov_mo).max())) 182 | 183 | # results of GPQ and GPQMO should be the same 184 | self.assertTrue(np.array_equal(mean_so, mean_mo)) 185 | self.assertTrue(np.array_equal(cov_so, cov_mo)) 186 | self.assertTrue(np.array_equal(ccov_so, ccov_mo)) 187 | 188 | def test_optimize_1D(self): 189 | # test on simple 1D example, plot the fit 190 | steps = 100 191 | dyn = UNGMTransition(GaussRV(1), GaussRV(1)) 192 | x, y = dyn.simulate_discrete(steps) 193 | 194 | f = dyn.dyn_eval 195 | dim_in, dim_out = dyn.dim_in, dyn.dim_state 196 | 197 | par0 = 1 + np.random.rand(dim_out, dim_in + 1) 198 | tf = MultiOutputGaussianProcessTransform(dim_in, dim_out, par0) 199 | 200 | # use sampled system state trajectory to create training data 201 | fy = np.zeros((dim_out, steps)) 202 | for k in range(steps): 203 | fy[:, k] = f(x[:, k, 0], k) 204 | 205 | b = [np.log((0.1, 1.0001))] + dim_in * [(None, None)] 206 | opt = {'xtol': 1e-2, 'maxiter': 100} 207 | log_par, res_list = tf.model.optimize(np.log(par0), fy, x[..., 0], bounds=b, method='L-BFGS-B', options=opt) 208 | 209 | print(np.exp(log_par)) 210 | self.assertTrue(False) 211 | 212 | def test_optimize(self): 213 | steps = 350 214 | m0 = np.array([1000, 300, 1000, 0, np.deg2rad(-3)]) 215 | P0 = np.diag([100, 10, 100, 10, 0.1]) 216 | x0 = GaussRV(5, m0, P0) 217 | dt = 0.1 218 | Q = np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]]) 219 | q = GaussRV(5, cov=Q) 220 | dyn = CoordinatedTurnTransition(x0, q, dt) 221 | x, y = dyn.simulate_discrete(steps) 222 | 223 | f = dyn.dyn_eval 224 | dim_in, dim_out = dyn.dim_in, dyn.dim_state 225 | 226 | # par0 = np.hstack((np.ones((dim_out, 1)), 5*np.ones((dim_out, dim_in+1)))) 227 | par0 = 10*np.ones((dim_out, dim_in+1)) 228 | tf = MultiOutputGaussianProcessTransform(dim_in, dim_out, par0) 229 | 230 | # use sampled system state trajectory to create training data 231 | fy = np.zeros((dim_out, steps)) 232 | for k in range(steps): 233 | fy[:, k] = f(x[:, k, 0], 0) 234 | 235 | opt = {'maxiter': 100} 236 | log_par, res_list = tf.model.optimize(np.log(par0), fy, x[..., 0], method='BFGS', options=opt) 237 | 238 | print(np.exp(log_par)) -------------------------------------------------------------------------------- /ssmtoybox/tests/test_mtran.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import numpy as np 4 | 5 | from ssmtoybox.mtran import MonteCarloTransform, TruncatedSphericalRadialTransform, FullySymmetricStudentTransform 6 | from ssmtoybox.ssmod import UNGMTransition 7 | from ssmtoybox.utils import GaussRV, StudentRV 8 | 9 | 10 | def sum_of_squares(x, pars, dx=False): 11 | """Sum of squares test function. 12 | 13 | If x is Gaussian random variable than x.T.dot(x) is chi-squared distributed with mean d and variance 2d, 14 | where d is the dimension of x. 15 | """ 16 | if not dx: 17 | return np.atleast_1d(x.T.dot(x)) 18 | else: 19 | return np.atleast_1d(2 * x) 20 | 21 | 22 | def cartesian2polar(x, pars, dx=False): 23 | return np.array([np.sqrt(x[0] ** 2 + x[1] ** 2), np.arctan2(x[1], x[0])]) 24 | 25 | 26 | class SigmaPointTruncTest(TestCase): 27 | def test_apply(self): 28 | d, d_eff = 5, 2 29 | t = TruncatedSphericalRadialTransform(d, d_eff) 30 | f = cartesian2polar 31 | mean, cov = np.zeros(d), np.eye(d) 32 | t.apply(f, mean, cov, None) 33 | 34 | 35 | class MonteCarloTest(TestCase): 36 | def test_crash(self): 37 | d = 1 38 | tmc = MonteCarloTransform(d, n=1e4) 39 | f = UNGMTransition(GaussRV(1, cov=1.0), GaussRV(1, cov=10.0)).dyn_eval 40 | mean = np.zeros(d) 41 | cov = np.eye(d) 42 | # does it crash ? 43 | tmc.apply(f, mean, cov, np.atleast_1d(1.0)) 44 | 45 | def test_increasing_samples(self): 46 | d = 1 47 | tmc = ( 48 | MonteCarloTransform(d, n=1e1), 49 | MonteCarloTransform(d, n=1e2), 50 | MonteCarloTransform(d, n=1e3), 51 | MonteCarloTransform(d, n=1e4), 52 | MonteCarloTransform(d, n=1e5), 53 | ) 54 | f = sum_of_squares # UNGM().dyn_eval 55 | mean = np.zeros(d) 56 | cov = np.eye(d) 57 | # does it crash ? 58 | for t in tmc: 59 | print(t.apply(f, mean, cov, np.atleast_1d(1.0))) 60 | 61 | 62 | class FullySymmetricStudentTest(TestCase): 63 | 64 | def test_symmetric_set(self): 65 | 66 | # 1D points 67 | dim = 1 68 | sp = FullySymmetricStudentTransform.symmetric_set(dim, []) 69 | self.assertEqual(sp.ndim, 2) 70 | self.assertEqual(sp.shape, (dim, 1)) 71 | sp = FullySymmetricStudentTransform.symmetric_set(dim, [1]) 72 | self.assertEqual(sp.shape, (dim, 2*dim)) 73 | sp = FullySymmetricStudentTransform.symmetric_set(dim, [1, 1]) 74 | self.assertEqual(sp.shape, (dim, 2*dim*(dim-1))) 75 | 76 | # 2D points 77 | dim = 2 78 | sp = FullySymmetricStudentTransform.symmetric_set(dim, []) 79 | self.assertEqual(sp.shape, (dim, 1)) 80 | sp = FullySymmetricStudentTransform.symmetric_set(dim, [1]) 81 | self.assertEqual(sp.shape, (dim, 2*dim)) 82 | sp = FullySymmetricStudentTransform.symmetric_set(dim, [1, 1]) 83 | self.assertEqual(sp.shape, (dim, 2 * dim * (dim - 1))) 84 | 85 | # 3D points 86 | dim = 3 87 | sp = FullySymmetricStudentTransform.symmetric_set(dim, [1, 1]) 88 | self.assertEqual(sp.shape, (dim, 2 * dim * (dim - 1))) 89 | 90 | def test_crash(self): 91 | dim = 1 92 | mt = FullySymmetricStudentTransform(dim, degree=3) 93 | f = UNGMTransition(StudentRV(1, scale=1.0), StudentRV(1, scale=10.0)).dyn_eval 94 | mean = np.zeros(dim) 95 | cov = np.eye(dim) 96 | # does it crash ? 97 | mt.apply(f, mean, cov, np.atleast_1d(1.0)) 98 | 99 | dim = 2 100 | mt = FullySymmetricStudentTransform(dim, degree=5) 101 | f = sum_of_squares 102 | mean = np.zeros(dim) 103 | cov = np.eye(dim) 104 | # does it crash ? 105 | mt.apply(f, mean, cov, np.atleast_1d(1.0)) 106 | -------------------------------------------------------------------------------- /ssmtoybox/tests/test_mult_dot_einsum.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import logging 4 | import numpy as np 5 | import numpy.linalg as la 6 | from scipy.linalg import cho_factor, cho_solve 7 | 8 | from ssmtoybox.bq.bqmtran import GaussianProcessTransform 9 | from ssmtoybox.mtran import MonteCarloTransform, UnscentedTransform 10 | from ssmtoybox.ssmod import ReentryVehicle2DTransition 11 | from ssmtoybox.utils import GaussRV 12 | 13 | logging.basicConfig(level=logging.DEBUG) 14 | 15 | 16 | def sym(a): 17 | return 0.5 * (a + a.T) 18 | 19 | 20 | def cho_inv(a): 21 | n = a.shape[0] 22 | return cho_solve(cho_factor(a), np.eye(n)) 23 | 24 | 25 | class MultTest(TestCase): 26 | 27 | def test_dot_matvec_matmat(self): 28 | # Does numpy.dot use different subroutines for matrix/vector and matrix/matrix multiplication? 29 | 30 | # dot internals 31 | n, e = 300, 150 32 | A = 10 * np.random.randn(n, n) 33 | B = 50 * np.random.randn(e, n) 34 | A = sym(A) # symmetrize A 35 | 36 | b = B[0, :] 37 | 38 | c = b.dot(A) 39 | C = B.dot(A) 40 | self.assertTrue(np.all(c == C[0, :]), 41 | "MAX DIFF: {:.4e}".format(np.abs(c - C[0, :]).max())) 42 | 43 | def test_einsum_dot(self): 44 | # einsum and dot give different results? 45 | 46 | dim_in, dim_out = 2, 1 47 | ker_par_mo = np.hstack((np.ones((dim_out, 1)), 1 * np.ones((dim_out, dim_in)))) 48 | tf_mo = GaussianProcessTransform(dim_in, dim_out, ker_par_mo, point_str='sr') 49 | iK, Q = tf_mo.model.iK, tf_mo.model.Q 50 | 51 | C1 = iK.dot(Q).dot(iK) 52 | C2 = np.einsum('ab, bc, cd', iK, Q, iK) 53 | 54 | self.assertTrue(np.allclose(C1, C2), "MAX DIFF: {:.4e}".format(np.abs(C1 - C2).max())) 55 | 56 | def test_cho_dot_ein(self): 57 | # attempt to compute the transformed covariance using cholesky decomposition 58 | 59 | # integrand 60 | # input moments 61 | mean_in = np.array([6500.4, 349.14, 1.8093, 6.7967, 0.6932]) 62 | cov_in = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1]) 63 | 64 | f = ReentryVehicle2DTransition(GaussRV(5, mean_in, cov_in), GaussRV(3)).dyn_eval 65 | dim_in, dim_out = ReentryVehicle2DTransition.dim_state, 1 66 | 67 | # transform 68 | ker_par_mo = np.hstack((np.ones((dim_out, 1)), 25 * np.ones((dim_out, dim_in)))) 69 | tf_so = GaussianProcessTransform(dim_in, dim_out, ker_par_mo, point_str='sr') 70 | 71 | # Monte-Carlo for ground truth 72 | # tf_ut = UnscentedTransform(dim_in) 73 | # tf_ut.apply(f, mean_in, cov_in, np.atleast_1d(1), None) 74 | tf_mc = MonteCarloTransform(dim_in, 1000) 75 | mean_mc, cov_mc, ccov_mc = tf_mc.apply(f, mean_in, cov_in, np.atleast_1d(1)) 76 | C_MC = cov_mc + np.outer(mean_mc, mean_mc.T) 77 | 78 | # evaluate integrand 79 | x = mean_in[:, None] + la.cholesky(cov_in).dot(tf_so.model.points) 80 | Y = np.apply_along_axis(f, 0, x, 1.0, None) 81 | 82 | # covariance via np.dot 83 | iK, Q = tf_so.model.iK, tf_so.model.Q 84 | C1 = iK.dot(Q).dot(iK) 85 | C1 = Y.dot(C1).dot(Y.T) 86 | 87 | # covariance via np.einsum 88 | C2 = np.einsum('ab, bc, cd', iK, Q, iK) 89 | C2 = np.einsum('ab,bc,cd', Y, C2, Y.T) 90 | 91 | # covariance via np.dot and cholesky 92 | K = tf_so.model.kernel.eval(tf_so.model.kernel.par, tf_so.model.points) 93 | L_lower = la.cholesky(K) 94 | Lq = la.cholesky(Q) 95 | phi = la.solve(L_lower, Lq) 96 | psi = la.solve(L_lower, Y.T) 97 | bet = psi.T.dot(phi) 98 | C3_dot = bet.dot(bet.T) 99 | C3_ein = np.einsum('ij, jk', bet, bet.T) 100 | 101 | logging.debug("MAX DIFF: {:.4e}".format(np.abs(C1 - C2).max())) 102 | logging.debug("MAX DIFF: {:.4e}".format(np.abs(C3_dot - C3_ein).max())) 103 | self.assertTrue(np.allclose(C1, C2), "MAX DIFF: {:.4e}".format(np.abs(C1 - C2).max())) 104 | self.assertTrue(np.allclose(C3_dot, C3_ein), "MAX DIFF: {:.4e}".format(np.abs(C3_dot - C3_ein).max())) 105 | self.assertTrue(np.allclose(C1, C3_dot), "MAX DIFF: {:.4e}".format(np.abs(C1 - C3_dot).max())) 106 | -------------------------------------------------------------------------------- /ssmtoybox/tests/test_ssinf.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import numpy as np 4 | 5 | from ssmtoybox.ssinf import MarginalizedGaussianProcessKalman, GaussianProcessKalman, BayesSardKalman, \ 6 | StudentProcessStudent, StudentProcessKalman, FullySymmetricStudent 7 | from ssmtoybox.ssinf import UnscentedKalman, ExtendedKalman, GaussHermiteKalman 8 | from ssmtoybox.ssmod import UNGMMeasurement, UNGMNAMeasurement, Pendulum2DMeasurement, BearingMeasurement, \ 9 | Radar2DMeasurement 10 | from ssmtoybox.ssmod import UNGMTransition, UNGMNATransition, Pendulum2DTransition, CoordinatedTurnTransition, \ 11 | ReentryVehicle2DTransition, ConstantTurnRateSpeed, ConstantVelocity 12 | from ssmtoybox.utils import GaussRV, StudentRV 13 | 14 | np.set_printoptions(precision=4) 15 | 16 | 17 | class GaussianInferenceTest(TestCase): 18 | 19 | @classmethod 20 | def setUpClass(cls): 21 | cls.ssm = {} 22 | # setup UNGM 23 | x0 = GaussRV(1) 24 | q = GaussRV(1, cov=np.array([[10.0]])) 25 | r = GaussRV(1) 26 | dyn = UNGMTransition(x0, q) 27 | obs = UNGMMeasurement(r, 1) 28 | x = dyn.simulate_discrete(100) 29 | y = obs.simulate_measurements(x) 30 | cls.ssm.update({'ungm': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}}) 31 | 32 | # setup UNGM with non-additive noise 33 | x0 = GaussRV(1) 34 | q = GaussRV(1, cov=np.array([[10.0]])) 35 | r = GaussRV(1) 36 | dyn = UNGMNATransition(x0, q) 37 | obs = UNGMNAMeasurement(r, 1) 38 | x = dyn.simulate_discrete(100) 39 | y = obs.simulate_measurements(x) 40 | cls.ssm.update({'ungmna': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}}) 41 | 42 | # setup 2D pendulum 43 | x0 = GaussRV(2, mean=np.array([1.5, 0]), cov=0.01 * np.eye(2)) 44 | dt = 0.01 45 | q = GaussRV(2, cov=0.01 * np.array([[(dt ** 3) / 3, (dt ** 2) / 2], [(dt ** 2) / 2, dt]])) 46 | r = GaussRV(1, cov=np.array([[0.1]])) 47 | dyn = Pendulum2DTransition(x0, q, dt=dt) 48 | obs = Pendulum2DMeasurement(r, dyn.dim_state) 49 | x = dyn.simulate_discrete(100) 50 | y = obs.simulate_measurements(x) 51 | cls.ssm.update({'pend': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}}) 52 | 53 | # setup reentry vehicle radar tracking 54 | m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932]) 55 | P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1]) 56 | x0 = GaussRV(5, m0, P0) 57 | q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 1e-6])) 58 | r = GaussRV(2, cov=np.diag([1e-6, 0.17e-6])) 59 | dyn = ReentryVehicle2DTransition(x0, q) 60 | obs = Radar2DMeasurement(r, 5) 61 | x = dyn.simulate_discrete(100) 62 | y = obs.simulate_measurements(x) 63 | cls.ssm.update({'rer': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}}) 64 | 65 | # setup coordinated turn bearing only tracking 66 | m0 = np.array([1000, 300, 1000, 0, np.deg2rad(-3.0)]) 67 | P0 = np.diag([100, 10, 100, 10, 0.1]) 68 | x0 = GaussRV(5, m0, P0) 69 | dt = 0.1 70 | rho_1, rho_2 = 0.1, 1.75e-4 71 | A = np.array([[dt**3/3, dt**2/2], 72 | [dt**2/2, dt]]) 73 | Q = np.zeros((5, 5)) 74 | Q[:2, :2], Q[2:4, 2:4], Q[4, 4] = rho_1*A, rho_1*A, rho_2*dt 75 | q = GaussRV(5, cov=Q) 76 | r = GaussRV(4, cov=10e-3*np.eye(4)) 77 | sen = np.vstack((1000 * np.eye(2), -1000 * np.eye(2))).astype(np.float) 78 | dyn = CoordinatedTurnTransition(x0, q) 79 | obs = BearingMeasurement(r, 5, state_index=[0, 2], sensor_pos=sen) 80 | x = dyn.simulate_discrete(100) 81 | y = obs.simulate_measurements(x) 82 | cls.ssm.update({'ctb': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}}) 83 | 84 | # setup CTRS with radar measurements 85 | x0 = GaussRV(5, cov=0.1*np.eye(5)) 86 | q = GaussRV(2, cov=np.diag([0.1, 0.1*np.pi])) 87 | r = GaussRV(2, cov=np.diag([0.3, 0.03])) 88 | dyn = ConstantTurnRateSpeed(x0, q) 89 | obs = Radar2DMeasurement(r, 5) 90 | x = dyn.simulate_discrete(100) 91 | y = obs.simulate_measurements(x) 92 | cls.ssm.update({'ctrs': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}}) 93 | 94 | def test_extended_kalman(self): 95 | """ 96 | Test Extended KF on range of SSMs. 97 | """ 98 | for ssm_name, data in self.ssm.items(): 99 | if ssm_name in ['rer', 'ctb', 'ctrs']: 100 | # Jacobians not implemented for reentry and coordinate turn 101 | continue 102 | print('Testing: {} ...'.format(ssm_name.upper()), end=' ') 103 | try: 104 | alg = ExtendedKalman(data['dyn'], data['obs']) 105 | alg.forward_pass(data['y'][..., 0]) 106 | alg.backward_pass() 107 | alg.reset() 108 | except BaseException as e: 109 | print('Failed: {}'.format(e)) 110 | continue 111 | print('OK') 112 | 113 | def test_unscented_kalman(self): 114 | """ 115 | Test Unscented KF on range of SSMs. 116 | """ 117 | for ssm_name, data in self.ssm.items(): 118 | print('Testing: {} ...'.format(ssm_name.upper()), end=' ') 119 | try: 120 | alg = UnscentedKalman(data['dyn'], data['obs']) 121 | alg.forward_pass(data['y'][..., 0]) 122 | alg.backward_pass() 123 | alg.reset() 124 | except BaseException as e: 125 | print('Failed: {}'.format(e)) 126 | continue 127 | print('OK') 128 | 129 | def test_gauss_hermite_kalman(self): 130 | """ 131 | Test Gauss-Hermite KF on range of SSMs. 132 | """ 133 | for ssm_name, data in self.ssm.items(): 134 | print('Testing: {} ...'.format(ssm_name.upper()), end=' ') 135 | try: 136 | alg = GaussHermiteKalman(data['dyn'], data['obs']) 137 | alg.forward_pass(data['y'][..., 0]) 138 | alg.backward_pass() 139 | alg.reset() 140 | except BaseException as e: 141 | print('Failed {}'.format(e)) 142 | continue 143 | print('OK') 144 | 145 | def test_gaussian_process_kalman(self): 146 | """ 147 | Test Gaussian Process Quadrature KF on range of SSMs. 148 | """ 149 | for ssm_name, data in self.ssm.items(): 150 | if ssm_name in ['rer', 'ctb']: 151 | # GPQ kernel pars hard to find on higher-dimensional systems like reentry or CT 152 | continue 153 | print('Testing: {} ...'.format(ssm_name.upper()), end=' ') 154 | # setup kernel parameters 155 | kpar_dyn = np.atleast_2d(np.ones(data['dyn'].dim_in + 1)) 156 | kpar_obs = np.atleast_2d(np.ones(data['obs'].dim_in + 1)) 157 | try: 158 | alg = GaussianProcessKalman(data['dyn'], data['obs'], kpar_dyn, kpar_obs) 159 | alg.forward_pass(data['y'][..., 0]) 160 | alg.backward_pass() 161 | alg.reset() 162 | except BaseException as e: 163 | print('Failed: {}'.format(e)) 164 | continue 165 | print('OK') 166 | 167 | def test_student_process_kalman(self): 168 | """ 169 | Test Student Process Quadrature KF on range of SSMs. 170 | """ 171 | for ssm_name, data in self.ssm.items(): 172 | if ssm_name in ['rer', 'ctb']: 173 | # TPQ kernel pars hard to find on higher-dimensional systems like reentry or CT 174 | continue 175 | print('Testing: {} ...'.format(ssm_name.upper()), end=' ') 176 | # setup kernel parameters 177 | kpar_dyn = np.atleast_2d(np.ones(data['dyn'].dim_in + 1)) 178 | kpar_obs = np.atleast_2d(np.ones(data['obs'].dim_in + 1)) 179 | try: 180 | alg = StudentProcessKalman(data['dyn'], data['obs'], kpar_dyn, kpar_obs) 181 | alg.forward_pass(data['y'][..., 0]) 182 | alg.backward_pass() 183 | alg.reset() 184 | except BaseException as e: 185 | print('Failed: {}'.format(e)) 186 | continue 187 | print('OK') 188 | 189 | def test_bayes_sard_kalman(self): 190 | """ 191 | Test Bayes-Sard Quadrature KF on range of SSMs. 192 | """ 193 | for ssm_name, data in self.ssm.items(): 194 | print('Testing: {} ...'.format(ssm_name.upper()), end=' ') 195 | # setup kernel parameters and multi-indices (for polynomial mean function) 196 | dim = data['dyn'].dim_in 197 | kpar_dyn = np.atleast_2d(np.ones(dim + 1)) 198 | alpha_dyn = np.hstack((np.zeros((dim, 1)), np.eye(dim), 2 * np.eye(dim))).astype(int) 199 | dim = data['obs'].dim_in 200 | kpar_obs = np.atleast_2d(np.ones(dim + 1)) 201 | alpha_obs = np.hstack((np.zeros((dim, 1)), np.eye(dim), 2 * np.eye(dim))).astype(int) 202 | try: 203 | alg = BayesSardKalman(data['dyn'], data['obs'], kpar_dyn, kpar_obs, alpha_dyn, alpha_obs) 204 | alg.forward_pass(data['y'][..., 0]) 205 | alg.backward_pass() 206 | alg.reset() 207 | except BaseException as e: 208 | print('Failed: {}'.format(e)) 209 | continue 210 | print('OK') 211 | 212 | 213 | class StudentInferenceTest(TestCase): 214 | 215 | @classmethod 216 | def setUpClass(cls): 217 | cls.ssm = {} 218 | # setup UNGM with Student RVs 219 | x0 = StudentRV(1) 220 | q = StudentRV(1, scale=np.array([[10.0]])) 221 | r = StudentRV(1) 222 | dyn = UNGMTransition(x0, q) 223 | obs = UNGMMeasurement(r, dyn.dim_state) 224 | x = dyn.simulate_discrete(100) 225 | y = obs.simulate_measurements(x) 226 | cls.ssm.update({'ungm': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}}) 227 | 228 | # setup CV with Student RVs 229 | m_0 = np.array([10175, 295, 980, -35]).astype(np.float) 230 | P_0 = np.diag([10000, 100, 10000, 100]).astype(np.float) 231 | nu_0 = 1000.0 232 | x0 = StudentRV(4, m_0, P_0, nu_0) 233 | Q = np.diag([50, 5]).astype(np.float) 234 | nu_q = 1000.0 235 | q = StudentRV(2, scale=Q, dof=nu_q) 236 | R = np.diag([50, 0.4e-6]).astype(np.float) 237 | nu_r = 4.0 238 | r = StudentRV(2, scale=R, dof=nu_r) 239 | dyn = ConstantVelocity(x0, q, dt=0.5) 240 | obs = Radar2DMeasurement(r, 4) 241 | x = dyn.simulate_discrete(100) 242 | y = obs.simulate_measurements(x) 243 | cls.ssm.update({'cv': {'dyn': dyn, 'obs': obs, 'x': x, 'y': y}}) 244 | 245 | def test_student_process_student(self): 246 | """ 247 | Test t-Process Quadrature SF on a range of SSMs. 248 | """ 249 | 250 | for ssm_name, data in self.ssm.items(): 251 | dim = data['x'].shape[0] 252 | kerpar = np.atleast_2d(np.ones(dim + 1)) 253 | np.random.seed(1) # for reproducibility reasons 254 | filt = StudentProcessStudent(data['dyn'], data['obs'], kerpar, kerpar) 255 | filt.forward_pass(data['y'][..., 0]) 256 | 257 | def test_fully_symmetric_student(self): 258 | """ 259 | Test fully-symmetric SF. 260 | """ 261 | 262 | for ssm_name, data in self.ssm.items(): 263 | filt = FullySymmetricStudent(data['dyn'], data['obs']) 264 | filt.forward_pass(data['y'][..., 0]) 265 | 266 | 267 | class GPQMarginalizedTest(TestCase): 268 | 269 | @classmethod 270 | def setUpClass(cls): 271 | # setup UNGM 272 | x0 = GaussRV(1, cov=np.atleast_2d(1.0)) 273 | q = GaussRV(1, cov=np.atleast_2d(10.0)) 274 | cls.dyn_ungm = UNGMTransition(x0, q) 275 | r = GaussRV(1, cov=np.atleast_2d(1.0)) 276 | cls.obs_ungm = UNGMMeasurement(r, 1) 277 | 278 | # setup pendulum 279 | dt = 0.01 280 | x0 = GaussRV(2, np.array([1.5, 0]), 0.01*np.eye(2)) 281 | q = GaussRV(2, cov=np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]])) 282 | cls.dyn_pend = Pendulum2DTransition(x0, q, dt) 283 | r = GaussRV(1, cov=np.atleast_2d(0.1)) 284 | cls.obs_pend = Pendulum2DMeasurement(r, cls.dyn_pend.dim_state) 285 | 286 | def test_init(self): 287 | alg = MarginalizedGaussianProcessKalman(self.dyn_ungm, self.obs_ungm, 'rbf', 'sr') 288 | 289 | def test_time_update(self): 290 | alg = MarginalizedGaussianProcessKalman(self.dyn_ungm, self.obs_ungm, 'rbf', 'sr') 291 | alg._time_update(1) 292 | par_dyn, par_obs = np.array([1, 1]), np.array([1, 1]) 293 | alg._time_update(1, par_dyn, par_obs) 294 | 295 | def test_laplace_approx(self): 296 | alg = MarginalizedGaussianProcessKalman(self.dyn_ungm, self.obs_ungm, 'rbf', 'sr') 297 | # Random measurement 298 | y = np.sqrt(10)*np.random.randn(1) 299 | alg._param_posterior_moments(y, 10) 300 | # does parameter posterior have positive semi-definite covariance? 301 | self.assertTrue(np.all(np.linalg.eigvals(alg.param_cov) >= 0)) 302 | 303 | def test_measurement_update(self): 304 | y = self.obs_ungm.simulate_measurements(self.dyn_ungm.simulate_discrete(5)) 305 | alg = MarginalizedGaussianProcessKalman(self.dyn_ungm, self.obs_ungm, 'rbf', 'sr') 306 | alg._measurement_update(y[:, 0, 0], 1) 307 | 308 | def test_filtering_ungm(self): 309 | y = self.obs_ungm.simulate_measurements(self.dyn_ungm.simulate_discrete(100)) 310 | alg = MarginalizedGaussianProcessKalman(self.dyn_ungm, self.obs_ungm, 'rbf', 'sr') 311 | alg.forward_pass(y[..., 0]) 312 | 313 | def test_filtering_pendulum(self): 314 | y = self.obs_pend.simulate_measurements(self.dyn_pend.simulate_discrete(100)) 315 | alg = MarginalizedGaussianProcessKalman(self.dyn_pend, self.obs_pend, 'rbf', 'sr') 316 | alg.forward_pass(y[..., 0]) 317 | -------------------------------------------------------------------------------- /ssmtoybox/tests/test_ssmod.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | 6 | from ssmtoybox.ssmod import UNGMTransition, UNGMMeasurement, UNGMNATransition, UNGMNAMeasurement, \ 7 | ConstantTurnRateSpeed, Radar2DMeasurement, ReentryVehicle2DTransition 8 | from ssmtoybox.utils import GaussRV 9 | 10 | 11 | def default_bq_hypers(dyn, obs): 12 | hypers_f = np.atleast_2d(np.hstack((1, 3.0 * np.ones(dyn.dim_in)))) 13 | hypers_h = np.atleast_2d(np.hstack((1, 3.0 * np.ones(obs.dim_in)))) 14 | return hypers_f, hypers_h 15 | 16 | 17 | class TestUNGM(unittest.TestCase): 18 | def test_dyn_fcn(self): 19 | pass 20 | 21 | def test_meas_fcn(self): 22 | pass 23 | 24 | def test_simulate(self): 25 | time_steps = 50 26 | # UNGM additive noise 27 | dim = 1 28 | init_dist = GaussRV(dim) 29 | noise_dist = GaussRV(dim, cov=np.atleast_2d(10.0)) 30 | ungm_dyn = UNGMTransition(init_dist, noise_dist) 31 | ungm_meas = UNGMMeasurement(GaussRV(dim), ungm_dyn.dim_state) 32 | x = ungm_dyn.simulate_discrete(time_steps, mc_sims=20) 33 | y = ungm_meas.simulate_measurements(x) 34 | 35 | # UNGM non-additive noise 36 | ungmna_dyn = UNGMNATransition(init_dist, noise_dist) 37 | ungmna_meas = UNGMNAMeasurement(GaussRV(dim), ungm_dyn.dim_state) 38 | x = ungmna_dyn.simulate_discrete(time_steps, mc_sims=20) 39 | y = ungmna_meas.simulate_measurements(x) 40 | 41 | 42 | class TestPendulum(unittest.TestCase): 43 | pass 44 | 45 | 46 | class TestReentry(unittest.TestCase): 47 | 48 | def test_simulate_continuous(self): 49 | m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932]) 50 | P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1]) 51 | x0 = GaussRV(5, m0, P0) 52 | q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 1e-6])) 53 | dyn = ReentryVehicle2DTransition(x0, q, dt=0.05) 54 | x = dyn.simulate_continuous(200, dt=0.05) 55 | 56 | plt.figure() 57 | plt.plot(x[0, ...], x[1, ...], color='r') 58 | plt.show() 59 | 60 | 61 | class TestCTRS(unittest.TestCase): 62 | 63 | def test_simulate(self): 64 | # setup CTRS with radar measurements 65 | x0 = GaussRV(5, cov=0.1 * np.eye(5)) 66 | q = GaussRV(2, cov=np.diag([0.1, 0.1 * np.pi])) 67 | r = GaussRV(2, cov=np.diag([0.3, 0.03])) 68 | dyn = ConstantTurnRateSpeed(x0, q) 69 | obs = Radar2DMeasurement(r, 5) 70 | x = dyn.simulate_discrete(100, 10) 71 | y = obs.simulate_measurements(x) 72 | 73 | plt.figure() 74 | plt.plot(x[0, ...], x[1, ...], alpha=0.25, color='b') 75 | plt.show() 76 | 77 | 78 | class TestMeasurementModels(unittest.TestCase): 79 | 80 | def test_radar(self): 81 | r = GaussRV(2) 82 | dim_state = 5 83 | st_ind = np.array([0, 2]) 84 | radar_location = np.array([6378.0, 0]) 85 | obs = Radar2DMeasurement(r, dim_state, state_index=st_ind, radar_loc=radar_location) 86 | st, n = np.random.randn(5), np.random.randn(2) 87 | 88 | # check evaluation of the measurement function 89 | hx = obs.meas_eval(st, n, dx=False) 90 | self.assertTrue(hx.shape == (2, )) 91 | 92 | jac = obs.meas_eval(st, n, dx=True) 93 | # check proper dimensions 94 | self.assertEqual(jac.shape, (2, 5)) 95 | # non-zero columns only at state_indexes 96 | self.assertTrue(np.array_equal(np.nonzero(jac.sum(axis=0))[0], st_ind)) 97 | -------------------------------------------------------------------------------- /ssmtoybox/tests/test_utils.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from ssmtoybox.utils import * 4 | 5 | 6 | class TestMetrics(unittest.TestCase): 7 | 8 | def setUp(self): 9 | dim = 5 10 | self.x = np.random.randn(dim, ) 11 | self.m = np.random.randn(dim, ) 12 | self.cov = np.random.randn(dim, dim) 13 | self.cov = self.cov.dot(self.cov.T) 14 | self.mse = np.random.randn(dim, dim) 15 | 16 | def test_nll(self): 17 | neg_log_likelihood(self.x, self.m, self.cov) 18 | 19 | def test_log_cred_ratio(self): 20 | log_cred_ratio(self.x, self.m, self.cov, self.mse) 21 | 22 | 23 | class TestMSEMatrix(unittest.TestCase): 24 | 25 | def test_sample_mse_matrix(self): 26 | dim = 5 27 | mc = 100 28 | x = np.random.randn(dim, mc) 29 | m = np.random.randn(dim, mc) 30 | mse_matrix(x, m) 31 | 32 | 33 | class TestGaussMixture(unittest.TestCase): 34 | 35 | def test_gauss_mixture(self): 36 | means = ([0, 0], [1, 1], [3, 0], [0, -3]) 37 | covs = (0.1 * np.eye(2), 0.2 * np.eye(2), 0.3 * np.eye(2), 0.1 * np.eye(2)) 38 | alphas = (0.15, 0.3, 0.4, 0.15) 39 | num_samples = 1000 40 | samples, indexes = gauss_mixture(means, covs, alphas, num_samples) 41 | 42 | import matplotlib.pyplot as plt 43 | plot_opts = {'linestyle': '', 'marker': '.', 'markersize': 2} 44 | for i in range(len(alphas)): 45 | sel = indexes == i 46 | plt.plot(samples[sel, 0], samples[sel, 1], **plot_opts) 47 | plt.show() -------------------------------------------------------------------------------- /ssmtoybox/utils.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from abc import ABCMeta, abstractmethod 3 | 4 | import numba as nb 5 | import numpy as np 6 | import pandas as pd 7 | import scipy as sp 8 | from numpy import newaxis as na, linalg as la 9 | 10 | """ 11 | Preliminary implementation of routines computing various performance metrics used in state estimation. 12 | 13 | Every function expects data in a numpy array of shape (D, N, M, ...), where 14 | D - dimension, N - time steps, M - MC simulations, ... - other optional irrelevant dimensions. 15 | """ 16 | 17 | 18 | def squared_error(x, m): 19 | """ 20 | Squared Error 21 | 22 | .. math:: 23 | \mathrm{SE} = (x_k - m_k)^2 24 | 25 | Parameters 26 | ---------- 27 | x: (dim_x, num_time_steps, num_mc_sims) ndarray 28 | True state. 29 | 30 | m: (dim_x, num_time_steps, num_mc_sims, num_algs) ndarray 31 | State mean (state estimate). 32 | 33 | Returns 34 | ------- 35 | (d, time_steps, mc_sims) ndarray 36 | Difference between the true state and its estimate squared. 37 | """ 38 | return (x - m) ** 2 39 | 40 | 41 | def mse_matrix(x, m): 42 | """ 43 | Sample Mean Square Error matrix 44 | 45 | Parameters 46 | ---------- 47 | x: (dim_x, 1) ndarray 48 | True state. 49 | 50 | m: (dim_x, num_mc_sims) ndarray 51 | State mean (state estimate). 52 | 53 | Returns 54 | ------- 55 | : (dim_x, dim_x) ndarray 56 | Sample mean square error matrix. 57 | """ 58 | 59 | d, mc_sims = m.shape 60 | dx = x - m 61 | mse = np.empty((d, d, mc_sims)) 62 | for s in range(mc_sims): 63 | mse[..., s] = np.outer(dx[..., s], dx[..., s]) 64 | return mse.mean(axis=2) # average over MC simulations 65 | 66 | 67 | def log_cred_ratio(x, m, P, MSE): 68 | """ 69 | Logarithm of Credibility Ratio [Li2006]_ is given by 70 | 71 | .. math:: 72 | \\gamma_n = 10 \\log_{10} \\frac{(x - m_n)^{\\top}P_n^{-1}(x - m_n)}{(x - m_n)^{\\top}\Sigma^{-1}(x - m_n)} 73 | 74 | Parameters 75 | ---------- 76 | x : (dim_x, ) ndarray 77 | True state. 78 | 79 | m : (dim_x, ) ndarray 80 | State mean. 81 | 82 | P : (dim_x, dim_x) ndarray 83 | State covariance matrix. 84 | 85 | MSE : (dim_x, dim_x) ndarray 86 | Mean square error matrix. 87 | 88 | Returns 89 | ------- 90 | : float 91 | Logarithm of credibility ratio. 92 | 93 | Notes 94 | ----- 95 | Log credibility ratio is defined in [Li2006]_ and is an essential quantity for computing the inclination indicator 96 | 97 | .. math:: 98 | I^2 = \\frac{1}{N}\\sum\\limits_{n=1}^{N} \\gamma_n, 99 | 100 | and the non-credibility index given by 101 | 102 | .. math:: 103 | NCI = \\frac{1}{N}\\sum\\limits_{n=1}^{N} \\| \\gamma_n \\|. 104 | 105 | Since in state estimation examples one can either average over time or MC simulations, the implementation of I^2 106 | and NCI is left for the user. 107 | 108 | References 109 | ---------- 110 | .. [Li2006] X. R. Li and Z. Zhao, “Measuring Estimator’s Credibility: Noncredibility Index,” 111 | in Information Fusion, 2006 9th International Conference on, 2006, pp. 1–8. 112 | """ 113 | dx = x - m 114 | sqrtP = mat_sqrt(P) 115 | sqrtMSE = mat_sqrt(MSE) 116 | sqrtP_dx = sp.linalg.solve(sqrtP, dx) 117 | sqrtMSE_dx = sp.linalg.solve(sqrtMSE, dx) 118 | dx_icov_dx = sqrtP_dx.T.dot(sqrtP_dx) 119 | dx_imse_dx = sqrtMSE_dx.T.dot(sqrtMSE_dx) 120 | return 10 * (sp.log10(dx_icov_dx) - sp.log10(dx_imse_dx)) 121 | 122 | 123 | def neg_log_likelihood(x, m, P): 124 | """ 125 | Negative log-likelihood of the state estimate given the true state. 126 | 127 | Parameters 128 | ---------- 129 | x : (dim_x, ) ndarray 130 | True state. 131 | 132 | m : (dim_x, ) ndarray 133 | State mean. 134 | 135 | P : (dim_x, dim_x) ndarray 136 | State covariance matrix. 137 | 138 | Returns 139 | ------- 140 | : float 141 | Negative logarithm of likelihood of the state given the true state. 142 | """ 143 | 144 | dx = x - m 145 | d = x.shape[0] 146 | dx_iP_dx = dx.dot(np.linalg.inv(P)).dot(dx) 147 | sign, logdet = np.linalg.slogdet(P) 148 | return 0.5 * (sign*logdet + dx_iP_dx + d * np.log(2 * np.pi)) 149 | 150 | 151 | def kl_divergence(mean_0, cov_0, mean_1, cov_1): 152 | """ 153 | KL-divergence between the true and approximate Gaussian probability density functions. 154 | 155 | Parameters 156 | ---------- 157 | mean_0 : (dim_x, ) ndarray 158 | Mean of the true distribution. 159 | 160 | cov_0 : (dim_x, dim_x) ndarray 161 | Covariance of the true distribution. 162 | 163 | mean_1 : (dim_x, ) ndarray 164 | Mean of the approximate distribution. 165 | 166 | cov_1 : (dim_x, dim_x) ndarray 167 | Covariance of the approximate distribution. 168 | 169 | Returns 170 | ------- 171 | : float 172 | KL-divergence of two Gaussian densities. 173 | """ 174 | k = 1 if np.isscalar(mean_0) else mean_0.shape[0] 175 | cov_0, cov_1 = np.atleast_2d(cov_0, cov_1) 176 | dmu = mean_0 - mean_1 177 | dmu = np.asarray(dmu) 178 | det_0 = np.linalg.det(cov_0) 179 | det_1 = np.linalg.det(cov_1) 180 | inv_1 = np.linalg.inv(cov_1) 181 | kl = 0.5 * (np.trace(np.dot(inv_1, cov_0)) + np.dot(dmu.T, inv_1).dot(dmu) + np.log(det_0 / det_1) - k) 182 | return np.asscalar(kl) 183 | 184 | 185 | def symmetrized_kl_divergence(mean_0, cov_0, mean_1, cov_1): 186 | """ 187 | Symmetrized KL-divergence 188 | 189 | .. math:: 190 | 191 | \\mathrm{SKL} = \\frac{1}{2}[KL(q(x)||p(x)) + KL(p(x)||q(x))] 192 | 193 | 194 | between the true Gaussian PDF :math:`p(x) = \\mathrm{N}(x | m_0, C_0)` and the approximate Gaussian PDF 195 | :math:`q(x) = \\mathrm{N}(x | m_1, C_1)`. 196 | 197 | Parameters 198 | ---------- 199 | mean_0 : (dim_x, ) ndarray 200 | Mean of the true distribution. 201 | 202 | cov_0 : (dim_x, dim_x) ndarray 203 | Covariance of the true distribution. 204 | 205 | mean_1 : (dim_x, ) ndarray 206 | Mean of the approximate distribution. 207 | 208 | cov_1 : (dim_x, dim_x) ndarray 209 | Covariance of the approximate distribution. 210 | 211 | Returns 212 | ------- 213 | : float 214 | Symmetrized KL-divergence of two Gaussian densities. 215 | 216 | Notes 217 | ----- 218 | Other symmetrizations exist. 219 | """ 220 | return 0.5 * (kl_divergence(mean_0, cov_0, mean_1, cov_1) + kl_divergence(mean_1, cov_1, mean_0, cov_0)) 221 | 222 | 223 | def bootstrap_var(data, samples=1000): 224 | """ 225 | Estimates variance of a given data sample by bootstrapping. 226 | 227 | Parameters 228 | ---------- 229 | data: (1, mc_sims) ndarray 230 | Data set. 231 | samples: int, optional 232 | Number of samples to use during bootstrapping. 233 | 234 | Returns 235 | ------- 236 | : float 237 | Bootstrap estimate of variance of the data set. 238 | """ 239 | data = data.squeeze() 240 | mc_sims = data.shape[0] 241 | # sample with replacement to create new datasets 242 | smp_data = np.random.choice(data, (samples, mc_sims)) 243 | # calculate sample mean of each dataset and variance of the means 244 | return np.var(np.mean(smp_data, 1)) 245 | 246 | 247 | def print_table(data, row_labels=None, col_labels=None, latex=False): 248 | pd.DataFrame(data, index=row_labels, columns=col_labels) 249 | print(pd) 250 | if latex: 251 | pd.to_latex() 252 | 253 | 254 | def gauss_mixture(means, covs, alphas, size): 255 | """ 256 | Draw samples from Gaussian mixture. 257 | 258 | Parameters 259 | ---------- 260 | means : tuple of ndarrays 261 | Mean for each of the mixture components. 262 | 263 | covs : tuple of ndarrays 264 | Covariance for each of the mixture components. 265 | 266 | alphas : 1d ndarray 267 | Mixing proportions, must have same length as means and covs. 268 | 269 | size : int or tuple of ints #TODO: tuple of ints not yet handled. 270 | Number of samples to draw or shape of the output array containing samples. 271 | 272 | Returns 273 | ------- 274 | samples : ndarray 275 | Samples from the Gaussian mixture. 276 | 277 | indexes : ndarray 278 | Component of indices corresponding to samples in 279 | """ 280 | if len(means) != len(covs) or len(covs) != len(alphas): 281 | raise ValueError('means, covs and alphas need to have the same length.') 282 | 283 | n_samples = np.prod(size) 284 | n_dim = len(means[0]) 285 | # draw from discrete distribution according to the mixing proportions 286 | ci = np.random.choice(np.arange(len(alphas)), p=alphas, size=size) 287 | ci_counts = np.unique(ci, return_counts=True)[1] 288 | 289 | # draw samples from each of the component Gaussians 290 | samples = np.empty((n_samples, n_dim)) 291 | indexes = np.empty(n_samples, dtype=int) 292 | start = 0 293 | for ind, c in enumerate(ci_counts): 294 | end = start + c 295 | samples[start:end, :] = np.random.multivariate_normal(means[ind], covs[ind], size=c) 296 | indexes[start:end] = ind 297 | start = end 298 | from sklearn.utils import shuffle 299 | return shuffle(samples, indexes) 300 | 301 | 302 | def bigauss_mixture(m0, c0, m1, c1, alpha, size): 303 | """ 304 | Samples from a Gaussian mixture with two components. 305 | 306 | Draw samples of a random variable :math:`X` following a Gaussian mixture density with two components, 307 | given by 308 | 309 | .. math:: 310 | X \\sim \\alpha \\mathrm{N}(m_0, C_0) + (1 - \\alpha)\\mathrm{N}(m_1, C_1) 311 | 312 | Parameters 313 | ---------- 314 | m0 : (dim_x, ) ndarray 315 | Mean of the first component. 316 | 317 | c0 : (dim_x, dim_x) ndarray 318 | Covariance of the first component. 319 | 320 | m1 : (dim_x, ) ndarray 321 | Mean of the second component. 322 | 323 | c1 : (dim_x, dim_x) ndarray 324 | Covariance of the second component. 325 | 326 | alpha : float 327 | Mixing proportions, alpha. 328 | 329 | size : int or tuple of ints 330 | Number of samples to draw, gets passed into Numpy's random number generators. 331 | 332 | Returns 333 | ------- 334 | : ndarray 335 | Samples of a Gaussian mixture with two components. 336 | 337 | Notes 338 | ----- 339 | Very inefficient implementation, because it throws away a lot of the samples! 340 | """ 341 | mi = np.random.binomial(1, alpha, size).T # 1 w.p. alpha, 0 w.p. 1-alpha 342 | n0 = np.random.multivariate_normal(m0, c0, size).T 343 | n1 = np.random.multivariate_normal(m1, c1, size).T 344 | m1 = (mi[na, ...] == True) 345 | m0 = np.logical_not(m1) 346 | return m1 * n0 + m0 * n1 347 | 348 | 349 | def multivariate_t(mean, scale, nu, size): 350 | """ 351 | Samples from a multivariate Student's t-distribution. 352 | 353 | Samples of a random variable :math:`X` following a multivariate t-distribution 354 | :math:`X \\sim \\mathrm{St}(\\mu, \\Sigma, \\nu)`. 355 | 356 | Parameters 357 | ---------- 358 | mean : (dim_x, ) ndarray 359 | Mean vector. 360 | 361 | scale : (dim_x, dim_x) ndarray 362 | Scale matrix. 363 | 364 | nu : float 365 | Degrees of freedom. 366 | 367 | size : int or tuple of ints 368 | Number of samples to draw, gets passed into Numpy's random number generators. 369 | 370 | Returns 371 | ------- 372 | : ndarray 373 | Samples of a multivariate Student's t-distribution with two components. 374 | 375 | Notes 376 | ----- 377 | If :math:`y \\sim \\mathrm{N}(0, \\Sigma)` and :math:`u \\sim \\mathrm{Gamma}(k=\\nu/2, \\theta=2/\\nu)`, 378 | then :math:`x \\sim \\mathrm{St}(\\mu, \\Sigma, \\nu)`, where :math:`x = \\mu + y\\frac{1}{\\sqrt{u}}`. 379 | """ 380 | v = np.random.gamma(nu / 2, 2 / nu, size)[:, na] 381 | n = np.random.multivariate_normal(np.zeros_like(mean), scale, size) 382 | return mean[na, :] + n / np.sqrt(v) 383 | 384 | 385 | def maha(x, y, V=None): 386 | """ 387 | Mahalanobis distance of all pairs of supplied data points. 388 | 389 | Parameters 390 | ---------- 391 | x : (num_points, dim_x) ndarray 392 | Data points. 393 | 394 | y : (num_points, dim_x) ndarray 395 | Data points. 396 | 397 | V : ndarray (dim_x, dim_x) 398 | Weight matrix, if `V=None`, `V=eye(D)` is used. 399 | 400 | Returns 401 | ------- 402 | : (num_points, num_points) ndarray 403 | Pair-wise Mahalanobis distance of rows of x and y with given weight matrix V. 404 | """ 405 | if V is None: 406 | V = np.eye(x.shape[1]) 407 | x2V = np.sum(x.dot(V) * x, 1) 408 | y2V = np.sum(y.dot(V) * y, 1) 409 | return (x2V[:, na] + y2V[:, na].T) - 2 * x.dot(V).dot(y.T) 410 | 411 | 412 | def mat_sqrt(a): 413 | """ 414 | Matrix square-root. 415 | 416 | Parameters 417 | ---------- 418 | a : (n, n) ndarray 419 | Matrix to factor. 420 | 421 | Returns 422 | ------- 423 | : (n, n) ndarray 424 | If `a` is symmetric positive-definite, `cholesky(a)` is returned. Otherwise `u.dot(sqrt(s))` is returned, 425 | where `u, s, v = svd(a)`. 426 | """ 427 | try: 428 | b = sp.linalg.cholesky(a, lower=True) 429 | except np.linalg.linalg.LinAlgError: 430 | print('Cholesky failed, using SVD.', file=sys.stderr) 431 | u, s, v = sp.linalg.svd(a) 432 | b = u.dot(np.diag(np.sqrt(s))) 433 | return b 434 | 435 | 436 | def ellipse_points(pos, mat): 437 | """ 438 | Points on an ellipse given by center position and symmetric positive-definite matrix. 439 | 440 | Parameters 441 | ---------- 442 | pos : (dim_x) ndarray 443 | specifying center of the ellipse. 444 | 445 | mat : (dim_x, dim_x) ndarray 446 | Symmetric positive-definite matrix. 447 | 448 | Returns 449 | ------- 450 | x : (dim_x, 1) ndarray 451 | Points on an ellipse defined my the input mean and covariance. 452 | """ 453 | w, v = la.eig(mat) 454 | theta = np.linspace(0, 2 * np.pi) 455 | t = np.asarray((np.cos(theta), np.sin(theta))) 456 | return pos[:, na] + np.dot(v, np.sqrt(w[:, na]) * t) 457 | 458 | 459 | def n_sum_k(n, k): 460 | """Generates all n-tuples summing to k.""" 461 | assert k >= 0 462 | if k == 0: 463 | return np.zeros((n, 1), dtype=np.int) 464 | if k == 1: 465 | return np.eye(n, dtype=np.int) 466 | else: 467 | a = n_sum_k(n, k - 1) 468 | I = np.eye(n, dtype=np.int) 469 | temp = np.zeros((n, (n * (1 + n) // 2) - 1), dtype=np.int) 470 | tind = 0 471 | for i in range(n - 1): 472 | for j in range(i, n): 473 | temp[:, tind] = a[:, i] + I[:, j] 474 | tind = tind + 1 475 | return np.hstack((temp, a[:, n - 1:] + I[:, -1, None])) 476 | 477 | 478 | @nb.jit(nopython=True) 479 | def vandermonde(mul_ind, x): 480 | """ 481 | Vandermonde matrix with multivariate polynomial basis. 482 | 483 | Parameters 484 | ---------- 485 | mul_ind : (dim, num_basis) ndarray 486 | Matrix where each column is a multi-index which specifies a multivariate monomial. 487 | 488 | x : (dim, num_points) ndarray 489 | Sigma-points. 490 | 491 | Returns 492 | ------- 493 | : (num_points, num_basis) ndarray 494 | Vandermonde matrix evaluated for all sigma-points. 495 | """ 496 | dim, num_pts = x.shape 497 | num_basis = mul_ind.shape[1] 498 | vdm = np.zeros((num_pts, num_basis)) 499 | for n in range(num_pts): 500 | for b in range(num_basis): 501 | vdm[n, b] = np.prod(x[:, n] ** mul_ind[:, b]) 502 | return vdm 503 | 504 | 505 | def ode_euler(func, x, q, time, dt): 506 | """ 507 | ODE integration using Euler approximation. 508 | 509 | Parameters 510 | ---------- 511 | func : function 512 | Function defining the system dynamics. 513 | 514 | x : (dim_x, ) ndarray 515 | Previous system state. 516 | 517 | q : (dim_q, ) ndarray 518 | System (process) noise. 519 | 520 | time : (dim_par, ) ndarray 521 | Time index. 522 | 523 | dt : float 524 | Discretization step. 525 | 526 | Returns 527 | ------- 528 | : (dim_x, ) ndarray 529 | State in the next time step. 530 | """ 531 | xdot = func(x, q, time) 532 | return x + dt * xdot 533 | 534 | 535 | def ode_runge_kutta_4(func, x, q, time, dt): 536 | """ 537 | ODE integration using 4th-order Runge-Kutta approximation. 538 | 539 | Parameters 540 | ---------- 541 | func : function 542 | Function defining the system dynamics. 543 | 544 | x : (dim_x, ) ndarray 545 | Previous system state. 546 | 547 | q : (dim_q, ) ndarray 548 | System (process) noise. 549 | 550 | time : (dim_par, ) ndarray 551 | Time index. 552 | 553 | dt : float 554 | Discretization step. 555 | 556 | Returns 557 | ------- 558 | : (dim_x, ) ndarray 559 | State in the next time step. 560 | """ 561 | dt2 = 0.5 * dt 562 | k1 = func(x, q, time) 563 | k2 = func(x + dt2 * k1, q, time) 564 | k3 = func(x + dt2 * k2, q, time) 565 | k4 = func(x + dt * k3, q, time) 566 | return x + (dt / 6) * (k1 + 2 * (k2 + k3) + k4) 567 | 568 | 569 | class RandomVariable(metaclass=ABCMeta): 570 | 571 | @abstractmethod 572 | def sample(self, size): 573 | pass 574 | 575 | @abstractmethod 576 | def get_stats(self): 577 | pass 578 | 579 | 580 | class GaussRV(RandomVariable): 581 | """ 582 | Gaussian random variable. 583 | 584 | Parameters 585 | ---------- 586 | dim : int 587 | Dimensionality of the random variable. 588 | 589 | mean : (dim, ) ndarray, optional 590 | Mean. If `None`, zero mean. 591 | 592 | cov : (dim, dim) ndarray, optional 593 | Covariance matrix. If `None`, unit covariance matrix. 594 | """ 595 | 596 | def __init__(self, dim, mean=None, cov=None): 597 | # standard Gaussian distribution if mean, cov not specified 598 | if mean is None: 599 | mean = np.zeros((dim, )) 600 | # if mean is scalar, ensure 1D mean 601 | mean = np.atleast_1d(mean) 602 | if mean.ndim != 1: 603 | ValueError( 604 | "{:s}: mean has to be 1D array. Supplied {:d}D array.".format(self.__class__.__name__, mean.ndim)) 605 | 606 | if cov is None: 607 | cov = np.eye(dim) 608 | # if cov is scalar, ensure 2D cov 609 | cov = np.atleast_2d(cov) 610 | if cov.ndim != 2: 611 | ValueError( 612 | "{:s}: covariance has to be 2D array. Supplied {:d}D array.".format(self.__class__.__name__, cov.ndim)) 613 | 614 | self.dim = dim 615 | self.mean = mean 616 | self.cov = cov 617 | 618 | def sample(self, size): 619 | return np.moveaxis(np.random.multivariate_normal(self.mean, self.cov, size), -1, 0) 620 | 621 | def get_stats(self): 622 | return self.mean, self.cov 623 | 624 | 625 | class StudentRV(RandomVariable): 626 | """ 627 | Student's t random variable. 628 | 629 | Parameters 630 | ---------- 631 | dim : int 632 | Dimensionality of the random variable. 633 | 634 | mean : (dim, ) ndarray, optional 635 | Mean. If `None`, zero mean. 636 | 637 | scale : (dim, dim) ndarray, optional 638 | Scale matrix. If `None`, unit scale matrix. 639 | 640 | dof : float, optional 641 | Degrees of freedom. Must be > 2. Default `dof=3`. 642 | """ 643 | 644 | def __init__(self, dim, mean=None, scale=None, dof=3.0): 645 | # zero mean if not given 646 | if mean is None: 647 | mean = np.zeros((dim,)) 648 | mean = np.atleast_1d(mean) 649 | if mean.ndim != 1: 650 | ValueError( 651 | "{:s}: mean has to be 1D array. Supplied {:d}D array.".format(self.__class__.__name__, mean.ndim)) 652 | 653 | # unit scale if not given 654 | if scale is None: 655 | scale = np.eye(dim) 656 | scale = np.atleast_2d(scale) 657 | if scale.ndim != 2: 658 | ValueError("{:s}: scale matrix has to be 2D array. Supplied {:d}D array.".format( 659 | self.__class__.__name__, scale.ndim)) 660 | 661 | # dof must be > 2 662 | if dof <= 2.0: 663 | dof = 3.0 664 | 665 | self.dim = dim 666 | self.mean = mean 667 | self.scale = scale 668 | self.dof = dof 669 | 670 | def sample(self, size): 671 | return np.moveaxis(multivariate_t(self.mean, self.scale, self.dof, size), -1, 0) 672 | 673 | def get_stats(self): 674 | return self.mean, self.scale, self.dof 675 | --------------------------------------------------------------------------------