pax_global_header00006660000000000000000000000064133574770540014531gustar00rootroot0000000000000052 comment=582885dd9e01c2784a5786b02fb527b46f090370 filterpy-1.4.5/000077500000000000000000000000001335747705400133765ustar00rootroot00000000000000filterpy-1.4.5/.gitattributes000066400000000000000000000003441335747705400162720ustar00rootroot00000000000000# Auto detect text files and perform LF normalization * text eol=lf # Standard to msysgit *.pdf diff=astextplain *.PDF diff=astextplain *.rtf diff=astextplain *.RTF diff=astextplain *.py eol=lf *.txt eol=lf *.cfg eol=lf filterpy-1.4.5/.gitignore000066400000000000000000000003371335747705400153710ustar00rootroot00000000000000*.pyc *.tex *.toc *.aux *.out book_files/ dist/ *egg-info* _build/ _static _templatesq quadrature.py .settings/ .cache/ win-32/ win-64/ linux-32/ linux-64/ osx-64/ filterpy/kalman/i.py filterpy/inprogress.py .pytest_cache filterpy-1.4.5/.pylintrc000066400000000000000000000377421335747705400152600ustar00rootroot00000000000000[MASTER] # A comma-separated list of package or module names from where C extensions may # be loaded. Extensions are loading into the active Python interpreter and may # run arbitrary code extension-pkg-whitelist= # Add files or directories to the blacklist. They should be base names, not # paths. ignore=CVS # Add files or directories matching the regex patterns to the blacklist. The # regex matches against base names, not paths. ignore-patterns= # Python code to execute, usually for sys.path manipulation such as # pygtk.require(). #init-hook= # Use multiple processes to speed up Pylint. jobs=1 # List of plugins (as comma separated values of python modules names) to load, # usually to register additional checkers. load-plugins= # Pickle collected data for later comparisons. persistent=yes # Specify a configuration file. #rcfile= # When enabled, pylint would attempt to guess common misconfiguration and emit # user-friendly hints instead of false-positive error messages suggestion-mode=yes # Allow loading of arbitrary C extensions. Extensions are imported into the # active Python interpreter and may run arbitrary code. unsafe-load-any-extension=no [MESSAGES CONTROL] # Only show warnings with the listed confidence levels. Leave empty to show # all. Valid levels: HIGH, INFERENCE, INFERENCE_FAILURE, UNDEFINED confidence= # Disable the message, report, category or checker with the given id(s). You # can either give multiple identifiers separated by comma (,) or put this # option multiple times (only on the command line, not in the configuration # file where it should appear only once).You can also use "--disable=all" to # disable everything first and then reenable specific checks. For example, if # you want to run only the similarities checker, you can use "--disable=all # --enable=similarities". If you want to run only the classes checker, but have # no Warning level messages displayed, use"--disable=all --enable=classes # --disable=W" disable=print-statement, parameter-unpacking, unpacking-in-except, old-raise-syntax, backtick, long-suffix, old-ne-operator, old-octal-literal, import-star-module-level, non-ascii-bytes-literal, raw-checker-failed, bad-inline-option, locally-disabled, locally-enabled, file-ignored, suppressed-message, useless-suppression, deprecated-pragma, duplicate-code, apply-builtin, basestring-builtin, buffer-builtin, cmp-builtin, coerce-builtin, execfile-builtin, file-builtin, long-builtin, raw_input-builtin, reduce-builtin, standarderror-builtin, unicode-builtin, xrange-builtin, coerce-method, delslice-method, getslice-method, setslice-method, no-absolute-import, old-division, dict-iter-method, dict-view-method, next-method-called, metaclass-assignment, indexing-exception, raising-string, reload-builtin, oct-method, hex-method, nonzero-method, cmp-method, input-builtin, round-builtin, intern-builtin, unichr-builtin, map-builtin-not-iterating, zip-builtin-not-iterating, range-builtin-not-iterating, filter-builtin-not-iterating, using-cmp-argument, eq-without-hash, div-method, idiv-method, rdiv-method, exception-message-attribute, invalid-str-codec, sys-max-int, bad-python3-import, deprecated-string-function, deprecated-str-translate-call, deprecated-itertools-function, deprecated-types-field, next-method-defined, dict-items-not-iterating, dict-keys-not-iterating, dict-values-not-iterating # Enable the message, report, category or checker with the given id(s). You can # either give multiple identifier separated by comma (,) or put this option # multiple time (only on the command line, not in the configuration file where # it should appear only once). See also the "--disable" option for examples. enable=c-extension-no-member [REPORTS] # Python expression which should return a note less than 10 (10 is the highest # note). You have access to the variables errors warning, statement which # respectively contain the number of errors / warnings messages and the total # number of statements analyzed. This is used by the global evaluation report # (RP0004). evaluation=10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10) # Template used to display messages. This is a python new-style format string # used to format the message information. See doc for all details #msg-template= # Set the output format. Available formats are text, parseable, colorized, json # and msvs (visual studio).You can also give a reporter class, eg # mypackage.mymodule.MyReporterClass. output-format=text # Tells whether to display a full report or only the messages reports=no # Activate the evaluation score. score=yes [REFACTORING] # Maximum number of nested blocks for function / method body max-nested-blocks=5 # Complete name of functions that never returns. When checking for # inconsistent-return-statements if a never returning function is called then # it will be considered as an explicit return statement and no message will be # printed. never-returning-functions=optparse.Values,sys.exit [BASIC] # Naming style matching correct argument names argument-naming-style=snake_case # Regular expression matching correct argument names. Overrides argument- # naming-style #argument-rgx= # Naming style matching correct attribute names attr-naming-style=snake_case # Regular expression matching correct attribute names. Overrides attr-naming- # style #attr-rgx= # Bad variable names which should always be refused, separated by a comma bad-names=foo, bar, baz, toto, tutu, tata # Naming style matching correct class attribute names class-attribute-naming-style=any # Regular expression matching correct class attribute names. Overrides class- # attribute-naming-style #class-attribute-rgx= # Naming style matching correct class names class-naming-style=PascalCase # Regular expression matching correct class names. Overrides class-naming-style #class-rgx= # Naming style matching correct constant names const-naming-style=UPPER_CASE # Regular expression matching correct constant names. Overrides const-naming- # style #const-rgx= # Minimum line length for functions/classes that require docstrings, shorter # ones are exempt. docstring-min-length=-1 # Naming style matching correct function names function-naming-style=snake_case # Regular expression matching correct function names. Overrides function- # naming-style #function-rgx= # Good variable names which should always be accepted, separated by a comma good-names=i, j, k, ex, Run, _ # Include a hint for the correct naming format with invalid-name include-naming-hint=no # Naming style matching correct inline iteration names inlinevar-naming-style=any # Regular expression matching correct inline iteration names. Overrides # inlinevar-naming-style #inlinevar-rgx= # Naming style matching correct method names method-naming-style=snake_case # Regular expression matching correct method names. Overrides method-naming- # style #method-rgx= # Naming style matching correct module names module-naming-style=snake_case # Regular expression matching correct module names. Overrides module-naming- # style #module-rgx= # Colon-delimited sets of names that determine each other's naming style when # the name regexes allow several styles. name-group= # Regular expression which should only match function or class names that do # not require a docstring. no-docstring-rgx=^_ # List of decorators that produce properties, such as abc.abstractproperty. Add # to this list to register other decorators that produce valid properties. property-classes=abc.abstractproperty # Naming style matching correct variable names variable-naming-style=snake_case # Regular expression matching correct variable names. Overrides variable- # naming-style #variable-rgx= [FORMAT] # Expected format of line ending, e.g. empty (any line ending), LF or CRLF. expected-line-ending-format= # Regexp for a line that is allowed to be longer than the limit. ignore-long-lines=^\s*(# )??$ # Number of spaces of indent required inside a hanging or continued line. indent-after-paren=4 # String used as indentation unit. This is usually " " (4 spaces) or "\t" (1 # tab). indent-string=' ' # Maximum number of characters on a single line. max-line-length=100 # Maximum number of lines in a module max-module-lines=1000 # List of optional constructs for which whitespace checking is disabled. `dict- # separator` is used to allow tabulation in dicts, etc.: {1 : 1,\n222: 2}. # `trailing-comma` allows a space between comma and closing bracket: (a, ). # `empty-line` allows space-only lines. no-space-check=trailing-comma, dict-separator # Allow the body of a class to be on the same line as the declaration if body # contains single statement. single-line-class-stmt=no # Allow the body of an if to be on the same line as the test if there is no # else. single-line-if-stmt=no [LOGGING] # Logging modules to check that the string format arguments are in logging # function parameter format logging-modules=logging [MISCELLANEOUS] # List of note tags to take in consideration, separated by a comma. notes=FIXME, XXX, TODO [SIMILARITIES] # Ignore comments when computing similarities. ignore-comments=yes # Ignore docstrings when computing similarities. ignore-docstrings=yes # Ignore imports when computing similarities. ignore-imports=no # Minimum lines number of a similarity. min-similarity-lines=4 [SPELLING] # Limits count of emitted suggestions for spelling mistakes max-spelling-suggestions=4 # Spelling dictionary name. Available dictionaries: none. To make it working # install python-enchant package. spelling-dict= # List of comma separated words that should not be checked. spelling-ignore-words= # A path to a file that contains private dictionary; one word per line. spelling-private-dict-file= # Tells whether to store unknown words to indicated private dictionary in # --spelling-private-dict-file option instead of raising a message. spelling-store-unknown-words=no [TYPECHECK] # List of decorators that produce context managers, such as # contextlib.contextmanager. Add to this list to register other decorators that # produce valid context managers. contextmanager-decorators=contextlib.contextmanager # List of members which are set dynamically and missed by pylint inference # system, and so shouldn't trigger E1101 when accessed. Python regular # expressions are accepted. generated-members= # Tells whether missing members accessed in mixin class should be ignored. A # mixin class is detected if its name ends with "mixin" (case insensitive). ignore-mixin-members=yes # This flag controls whether pylint should warn about no-member and similar # checks whenever an opaque object is returned when inferring. The inference # can return multiple potential results while evaluating a Python object, but # some branches might not be evaluated, which results in partial inference. In # that case, it might be useful to still emit no-member and other checks for # the rest of the inferred objects. ignore-on-opaque-inference=yes # List of class names for which member attributes should not be checked (useful # for classes with dynamically set attributes). This supports the use of # qualified names. ignored-classes=optparse.Values,thread._local,_thread._local # List of module names for which member attributes should not be checked # (useful for modules/projects where namespaces are manipulated during runtime # and thus existing member attributes cannot be deduced by static analysis. It # supports qualified module names, as well as Unix pattern matching. ignored-modules= # Show a hint with possible names when a member name was not found. The aspect # of finding the hint is based on edit distance. missing-member-hint=yes # The minimum edit distance a name should have in order to be considered a # similar match for a missing member name. missing-member-hint-distance=1 # The total number of similar names that should be taken in consideration when # showing a hint for a missing member. missing-member-max-choices=1 [VARIABLES] # List of additional names supposed to be defined in builtins. Remember that # you should avoid to define new builtins when possible. additional-builtins= # Tells whether unused global variables should be treated as a violation. allow-global-unused-variables=yes # List of strings which can identify a callback function by name. A callback # name must start or end with one of those strings. callbacks=cb_, _cb # A regular expression matching the name of dummy variables (i.e. expectedly # not used). dummy-variables-rgx=_+$|(_[a-zA-Z0-9_]*[a-zA-Z0-9]+?$)|dummy|^ignored_|^unused_ # Argument names that match this expression will be ignored. Default to name # with leading underscore ignored-argument-names=_.*|^ignored_|^unused_ # Tells whether we should check for unused import in __init__ files. init-import=no # List of qualified module names which can have objects that can redefine # builtins. redefining-builtins-modules=six.moves,past.builtins,future.builtins [CLASSES] # List of method names used to declare (i.e. assign) instance attributes. defining-attr-methods=__init__, __new__, setUp # List of member names, which should be excluded from the protected access # warning. exclude-protected=_asdict, _fields, _replace, _source, _make # List of valid names for the first argument in a class method. valid-classmethod-first-arg=cls # List of valid names for the first argument in a metaclass class method. valid-metaclass-classmethod-first-arg=mcs [DESIGN] # Maximum number of arguments for function / method max-args=5 # Maximum number of attributes for a class (see R0902). max-attributes=7 # Maximum number of boolean expressions in a if statement max-bool-expr=5 # Maximum number of branch for function / method body max-branches=12 # Maximum number of locals for function / method body max-locals=15 # Maximum number of parents for a class (see R0901). max-parents=7 # Maximum number of public methods for a class (see R0904). max-public-methods=20 # Maximum number of return / yield for function / method body max-returns=6 # Maximum number of statements in function / method body max-statements=50 # Minimum number of public methods for a class (see R0903). min-public-methods=2 [IMPORTS] # Allow wildcard imports from modules that define __all__. allow-wildcard-with-all=no # Analyse import fallback blocks. This can be used to support both Python 2 and # 3 compatible code, which means that the block might have code that exists # only in one or another interpreter, leading to false positives when analysed. analyse-fallback-blocks=no # Deprecated modules which should not be used, separated by a comma deprecated-modules=optparse,tkinter.tix # Create a graph of external dependencies in the given file (report RP0402 must # not be disabled) ext-import-graph= # Create a graph of every (i.e. internal and external) dependencies in the # given file (report RP0402 must not be disabled) import-graph= # Create a graph of internal dependencies in the given file (report RP0402 must # not be disabled) int-import-graph= # Force import order to recognize a module as part of the standard # compatibility libraries. known-standard-library= # Force import order to recognize a module as part of a third party library. known-third-party=enchant [EXCEPTIONS] # Exceptions that will emit a warning when being caught. Defaults to # "Exception" overgeneral-exceptions=Exception filterpy-1.4.5/LICENSE000066400000000000000000000020741335747705400144060ustar00rootroot00000000000000The MIT License (MIT) Copyright (c) 2015 Roger R. Labbe Jr Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. filterpy-1.4.5/MANIFEST.in000066400000000000000000000007201335747705400151330ustar00rootroot00000000000000include filterpy/changelog.txt include LICENSE recursive-include filterpy/common/tests *.py recursive-include filterpy/discrete_bayes/tests *.py recursive-include filterpy/gh/tests *.py recursive-include filterpy/hinfinity/tests *.py recursive-include filterpy/kalman/tests *.py recursive-include filterpy/leastsq/tests *.py recursive-include filterpy/memory/tests *.py recursive-include filterpy/monte_carlo/tests *.py recursive-include filterpy/stats/tests *.py filterpy-1.4.5/README.rst000066400000000000000000000246031335747705400150720ustar00rootroot00000000000000FilterPy - Kalman filters and other optimal and non-optimal estimation filters in Python. ----------------------------------------------------------------------------------------- .. image:: https://img.shields.io/pypi/v/filterpy.svg :target: https://pypi.python.org/pypi/filterpy **NOTE**: Imminent drop of support of Python 2.7, 3.4. See section below for details. This library provides Kalman filtering and various related optimal and non-optimal filtering software written in Python. It contains Kalman filters, Extended Kalman filters, Unscented Kalman filters, Kalman smoothers, Least Squares filters, fading memory filters, g-h filters, discrete Bayes, and more. This is code I am developing in conjunction with my book Kalman and Bayesian Filter in Python, which you can read/download at https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/ My aim is largely pedalogical - I opt for clear code that matches the equations in the relevant texts on a 1-to-1 basis, even when that has a performance cost. There are places where this tradeoff is unclear - for example, I find it somewhat clearer to write a small set of equations using linear algebra, but numpy's overhead on small matrices makes it run slower than writing each equation out by hand. Furthermore, books such Zarchan present the written out form, not the linear algebra form. It is hard for me to choose which presentation is 'clearer' - it depends on the audience. In that case I usually opt for the faster implementation. I use NumPy and SciPy for all of the computations. I have experimented with Numba and it yields impressive speed ups with minimal costs, but I am not convinced that I want to add that requirement to my project. It is still on my list of things to figure out, however. Sphinx generated documentation lives at http://filterpy.readthedocs.org/. Generation is triggered by git when I do a check in, so this will always be bleeding edge development version - it will often be ahead of the released version. Plan for dropping Python 2.7 support ------------------------------------ I haven't finalized my decision on this, but NumPy is dropping Python 2.7 support in December 2018. I will certainly drop Python 2.7 support by then; I will probably do it much sooner. At the moment FilterPy is on version 1.x. I plan to fork the project to version 2.0, and support only Python 3.5+. The 1.x version will still be available, but I will not support it. If I add something amazing to 2.0 and someone really begs, I might backport it; more likely I would accept a pull request with the feature backported to 1.x. But to be honest I don't forsee this happening. Why 3.5+, and not 3.4+? 3.5 introduced the matrix multiply symbol, and I want my code to take advantage of it. Plus, to be honest, I'm being selfish. I don't want to spend my life supporting this package, and moving as far into the present as possible means a few extra years before the Python version I choose becomes hopelessly dated and a liability. I recognize this makes people running the default Python in their linux distribution more painful. All I can say is I did not decide to do the Python 3 fork, and I don't have the time to support the bifurcation any longer. I am making edits to the package now in support of my book; once those are done I'll probably create the 2.0 branch. I'm contemplating a SLAM addition to the book, and am not sure if I will do this in 3.5+ only or not. Installation ------------ The most general installation is just to use pip, which should come with any modern Python distribution. .. image:: https://img.shields.io/pypi/v/filterpy.svg :target: https://pypi.python.org/pypi/filterpy :: pip install filterpy If you prefer to download the source yourself :: cd git clone http://github.com/rlabbe/filterpy python setup.py install If you use Anaconda, you can install from the conda-forge channel. You will need to add the conda-forge channel if you haven't already done so: :: conda config --add channels conda-forge and then install with: :: conda install filterpy And, if you want to install from the bleeding edge git version :: pip install git+https://github.com/rlabbe/filterpy.git Note: I make no guarantees that everything works if you install from here. I'm the only developer, and so I don't worry about dev/release branches and the like. Unless I fix a bug for you and tell you to get this version because I haven't made a new release yet, I strongly advise not installing from git. Basic use --------- Full documentation is at https://filterpy.readthedocs.io/en/latest/ First, import the filters and helper functions. .. code-block:: python import numpy as np from filterpy.kalman import KalmanFilter from filterpy.common import Q_discrete_white_noise Now, create the filter .. code-block:: python my_filter = KalmanFilter(dim_x=2, dim_z=1) Initialize the filter's matrices. .. code-block:: python my_filter.x = np.array([[2.], [0.]]) # initial state (location and velocity) my_filter.F = np.array([[1.,1.], [0.,1.]]) # state transition matrix my_filter.H = np.array([[1.,0.]]) # Measurement function my_filter.P *= 1000. # covariance matrix my_filter.R = 5 # state uncertainty my_filter.Q = Q_discrete_white_noise(2, dt, .1) # process uncertainty Finally, run the filter. .. code-block:: python while True: my_filter.predict() my_filter.update(get_some_measurement()) # do something with the output x = my_filter.x do_something_amazing(x) Sorry, that is the extent of the documentation here. However, the library is broken up into subdirectories: gh, kalman, memory, leastsq, and so on. Each subdirectory contains python files relating to that form of filter. The functions and methods contain pretty good docstrings on use. My book https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/ uses this library, and is the place to go if you are trying to learn about Kalman filtering and/or this library. These two are not exactly in sync - my normal development cycle is to add files here, test them, figure out how to present them pedalogically, then write the appropriate section or chapter in the book. So there is code here that is not discussed yet in the book. Requirements ------------ This library uses NumPy, SciPy, Matplotlib, and Python. I haven't extensively tested backwards compatibility - I use the Anaconda distribution, and so I am on Python 3.6 and 2.7.14, along with whatever version of NumPy, SciPy, and matplotlib they provide. But I am using pretty basic Python - numpy.array, maybe a list comprehension in my tests. I import from **__future__** to ensure the code works in Python 2 and 3. Testing ------- All tests are written to work with py.test. Just type ``py.test`` at the command line. As explained above, the tests are not robust. I'm still at the stage where visual plots are the best way to see how things are working. Apologies, but I think it is a sound choice for development. It is easy for a filter to perform within theoretical limits (which we can write a non-visual test for) yet be 'off' in some way. The code itself contains tests in the form of asserts and properties that ensure that arrays are of the proper dimension, etc. References ---------- I use three main texts as my refererence, though I do own the majority of the Kalman filtering literature. First is Paul Zarchan's 'Fundamentals of Kalman Filtering: A Practical Approach'. I think it by far the best Kalman filtering book out there if you are interested in practical applications more than writing a thesis. The second book I use is Eli Brookner's 'Tracking and Kalman Filtering Made Easy'. This is an astonishingly good book; its first chapter is actually readable by the layperson! Brookner starts from the g-h filter, and shows how all other filters - the Kalman filter, least squares, fading memory, etc., all derive from the g-h filter. It greatly simplifies many aspects of analysis and/or intuitive understanding of your problem. In contrast, Zarchan starts from least squares, and then moves on to Kalman filtering. I find that he downplays the predict-update aspect of the algorithms, but he has a wealth of worked examples and comparisons between different methods. I think both viewpoints are needed, and so I can't imagine discarding one book. Brookner also focuses on issues that are ignored in other books - track initialization, detecting and discarding noise, tracking multiple objects, an so on. I said three books. I also like and use Bar-Shalom's Estimation with Applications to Tracking and Navigation. Much more mathematical than the previous two books, I would not recommend it as a first text unless you already have a background in control theory or optimal estimation. Once you have that experience, this book is a gem. Every sentence is crystal clear, his language is precise, but each abstract mathematical statement is followed with something like "and this means...". License ------- .. image:: https://anaconda.org/rlabbe/filterpy/badges/license.svg :target: https://anaconda.org/rlabbe/filterpy The MIT License (MIT) Copyright (c) 2015 Roger R. Labbe Jr Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.TION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. filterpy-1.4.5/conda/000077500000000000000000000000001335747705400144625ustar00rootroot00000000000000filterpy-1.4.5/conda/bld.bat000066400000000000000000000003541335747705400157150ustar00rootroot00000000000000"%PYTHON%" setup.py install if errorlevel 1 exit 1 :: Add more build steps here, if they are necessary. :: See :: http://docs.continuum.io/conda/build.html :: for a list of environment variables that are set during the build process. filterpy-1.4.5/conda/build.sh000066400000000000000000000003331335747705400161140ustar00rootroot00000000000000#!/bin/bash $PYTHON setup.py install # Add more build steps here, if they are necessary. # See # http://docs.continuum.io/conda/build.html # for a list of environment variables that are set during the build process. filterpy-1.4.5/conda/conda_install.txt000066400000000000000000000015421335747705400200370ustar00rootroot00000000000000This documents how I build filterpy for Anaconda. for some reason the files are being copied into anaconda3 directory, despite what the online instructions say. I am following instruction from here: conda.pydata.org/docs/build_tutorials/pkgs.html # first time $ conda install conda-build $ conda config --set anaconda_upload no # every time $ cd ~/Dropbox/filterpy/conda $ edit meta.yaml, update version # $ conda skeleton pypi --version 0.1.2 filterpy $ conda build . $ conda convert --platform all ~/anaconda3/conda-bld/linux-64/filterpy-0.1.2-0.tar.bz2 $ anaconda upload --force linux-64/filterpy-0.1.2-0.tar.bz2 $ anaconda upload --force linux-32/filterpy-0.1.2-0.tar.bz2 $ anaconda upload --force osx-64/filterpy-0.1.2-0.tar.bz2 $ anaconda upload --force win-32/filterpy-0.1.2-0.tar.bz2 $ anaconda upload --force win-64/filterpy-0.1.2-0.tar.bz2 filterpy-1.4.5/conda/filterpy/000077500000000000000000000000001335747705400163205ustar00rootroot00000000000000filterpy-1.4.5/conda/filterpy/bld.bat000066400000000000000000000003541335747705400175530ustar00rootroot00000000000000"%PYTHON%" setup.py install if errorlevel 1 exit 1 :: Add more build steps here, if they are necessary. :: See :: http://docs.continuum.io/conda/build.html :: for a list of environment variables that are set during the build process. filterpy-1.4.5/conda/filterpy/build.sh000066400000000000000000000003331335747705400177520ustar00rootroot00000000000000#!/bin/bash $PYTHON setup.py install # Add more build steps here, if they are necessary. # See # http://docs.continuum.io/conda/build.html # for a list of environment variables that are set during the build process. filterpy-1.4.5/conda/filterpy/meta.yaml000066400000000000000000000033311335747705400201320ustar00rootroot00000000000000package: name: filterpy version: "0.1.3" source: fn: filterpy-0.1.3.tar.gz url: https://pypi.python.org/packages/75/df/5dbf357a3da1b49af18b2be00da15c7f86397b5b263bea1b9d65b5bd51cf/filterpy-0.1.3.tar.gz md5: ba1e6fb84efaae3d21a73ba9fb563cab # patches: # List any patch files here # - fix.patch # build: # noarch_python: True # preserve_egg_dir: True # entry_points: # Put any entry points (scripts to be generated automatically) here. The # syntax is module:function. For example # # - filterpy = filterpy:main # # Would create an entry point called filterpy that calls filterpy.main() # If this is a new build for the same version, increment the build # number. If you do not include this key, it defaults to 0. # number: 1 requirements: build: - python - setuptools - numpy - scipy - matplotlib run: - python - numpy - scipy - matplotlib test: # Python imports imports: - filterpy - filterpy.common - filterpy.discrete_bayes - filterpy.examples - filterpy.gh - filterpy.hinfinity - filterpy.kalman - filterpy.leastsq - filterpy.memory - filterpy.monte_carlo - filterpy.stats # commands: # You can put test commands to be run here. Use this to test that the # entry points work. # You can also put a file called run_test.py in the recipe that will be run # at test time. # requires: # Put any additional test requirements here. For example # - nose about: home: https://github.com/rlabbe/filterpy license: MIT License summary: 'Kalman filtering and optimal estimation library' # See # http://docs.continuum.io/conda/build.html for # more information about meta.yaml filterpy-1.4.5/conda/meta.yaml000066400000000000000000000032641335747705400163010ustar00rootroot00000000000000package: name: filterpy version: "0.1.3" source: fn: filterpy-0.1.3.tar.gz url: https://pypi.python.org/packages/source/f/filterpy/filterpy-0.1.3.tar.gz md5: ba1e6fb84efaae3d21a73ba9fb563cab # patches: # List any patch files here # - fix.patch # build: # noarch_python: True # preserve_egg_dir: True # entry_points: # Put any entry points (scripts to be generated automatically) here. The # syntax is module:function. For example # # - filterpy = filterpy:main # # Would create an entry point called filterpy that calls filterpy.main() # If this is a new build for the same version, increment the build # number. If you do not include this key, it defaults to 0. # number: 1 requirements: build: - python >=2.7 - setuptools - numpy - scipy - matplotlib run: - python >=2.7 - numpy - scipy - matplotlib test: # Python imports imports: - filterpy - filterpy.common - filterpy.discrete_bayes - filterpy.examples - filterpy.gh - filterpy.hinfinity - filterpy.kalman - filterpy.leastsq - filterpy.memory - filterpy.monte_carlo - filterpy.stats # commands: # You can put test commands to be run here. Use this to test that the # entry points work. # You can also put a file called run_test.py in the recipe that will be run # at test time. # requires: # Put any additional test requirements here. For example # - nose about: home: https://github.com/rlabbe/filterpy license: MIT License summary: 'Kalman filtering and optimal estimation library' # See # http://docs.continuum.io/conda/build.html for # more information about meta.yaml filterpy-1.4.5/creating_a_release.rst000066400000000000000000000023551335747705400177310ustar00rootroot00000000000000Steps to Create Release ======================= * run pytest * run pylint --disable=similarities filterpy * update filterpy/filterpy/__init__.py with the version number. * update filterpy/filterpy/changelog.txt with the changes for this release. * 'rm *' in dist * If necessary, edit filterpy/docs/index.rst to add any classes. Add .rst file for those new classes to the /docs subdirectories. * In /docs, run 'make html'. Inspect docs/_build/html/index.html for correctness. * Once docs are good, commit to git. * tag with 'git tag -a 0.1.23 -m "version 0.1.23" * push to origin. That automatically triggers a build on readthedocs.org. * push tags to origin with git push origin --tags * Update pypi.org with 'bash pypi-install.sh' * You need to manually update the documentation code at pythonhosted, PyPi's documentation server. cd /docs/_build/html zip -r filterpy.zip *.* add all files to a zip file (index.html must be at base) go to https://pypi.python.org/pypi?%3Aaction=pkg_edit&name=filterpy scroll to bottom, add the zip file you just made click 'Upload Documentation button' it usually takes several minutes for the documentation to show up here: https://pythonhosted.org/filterpy/ filterpy-1.4.5/docs/000077500000000000000000000000001335747705400143265ustar00rootroot00000000000000filterpy-1.4.5/docs/Makefile000066400000000000000000000151621335747705400157730ustar00rootroot00000000000000# Makefile for Sphinx documentation # # You can set these variables from the command line. SPHINXOPTS = SPHINXBUILD = sphinx-build PAPER = BUILDDIR = _build # User-friendly check for sphinx-build ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) $(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) endif # Internal variables. PAPEROPT_a4 = -D latex_paper_size=a4 PAPEROPT_letter = -D latex_paper_size=letter ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . # the i18n builder cannot share the environment and doctrees with the others I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . .PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext help: @echo "Please use \`make ' where is one of" @echo " html to make standalone HTML files" @echo " dirhtml to make HTML files named index.html in directories" @echo " singlehtml to make a single large HTML file" @echo " pickle to make pickle files" @echo " json to make JSON files" @echo " htmlhelp to make HTML files and a HTML help project" @echo " qthelp to make HTML files and a qthelp project" @echo " devhelp to make HTML files and a Devhelp project" @echo " epub to make an epub" @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" @echo " latexpdf to make LaTeX files and run them through pdflatex" @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" @echo " text to make text files" @echo " man to make manual pages" @echo " texinfo to make Texinfo files" @echo " info to make Texinfo files and run them through makeinfo" @echo " gettext to make PO message catalogs" @echo " changes to make an overview of all changed/added/deprecated items" @echo " xml to make Docutils-native XML files" @echo " pseudoxml to make pseudoxml-XML files for display purposes" @echo " linkcheck to check all external links for integrity" @echo " doctest to run all doctests embedded in the documentation (if enabled)" clean: rm -rf $(BUILDDIR)/* html: $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html @echo @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." dirhtml: $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml @echo @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." singlehtml: $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml @echo @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." pickle: $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle @echo @echo "Build finished; now you can process the pickle files." json: $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json @echo @echo "Build finished; now you can process the JSON files." htmlhelp: $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp @echo @echo "Build finished; now you can run HTML Help Workshop with the" \ ".hhp project file in $(BUILDDIR)/htmlhelp." qthelp: $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp @echo @echo "Build finished; now you can run "qcollectiongenerator" with the" \ ".qhcp project file in $(BUILDDIR)/qthelp, like this:" @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/FilterPy.qhcp" @echo "To view the help file:" @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/FilterPy.qhc" devhelp: $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp @echo @echo "Build finished." @echo "To view the help file:" @echo "# mkdir -p $$HOME/.local/share/devhelp/FilterPy" @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/FilterPy" @echo "# devhelp" epub: $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub @echo @echo "Build finished. The epub file is in $(BUILDDIR)/epub." latex: $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex @echo @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." @echo "Run \`make' in that directory to run these through (pdf)latex" \ "(use \`make latexpdf' here to do that automatically)." latexpdf: $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex @echo "Running LaTeX files through pdflatex..." $(MAKE) -C $(BUILDDIR)/latex all-pdf @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." latexpdfja: $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex @echo "Running LaTeX files through platex and dvipdfmx..." $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." text: $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text @echo @echo "Build finished. The text files are in $(BUILDDIR)/text." man: $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man @echo @echo "Build finished. The manual pages are in $(BUILDDIR)/man." texinfo: $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo @echo @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." @echo "Run \`make' in that directory to run these through makeinfo" \ "(use \`make info' here to do that automatically)." info: $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo @echo "Running Texinfo files through makeinfo..." make -C $(BUILDDIR)/texinfo info @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." gettext: $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale @echo @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." changes: $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes @echo @echo "The overview file is in $(BUILDDIR)/changes." linkcheck: $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck @echo @echo "Link check complete; look for any errors in the above output " \ "or in $(BUILDDIR)/linkcheck/output.txt." doctest: $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest @echo "Testing of doctests in the sources finished, look at the " \ "results in $(BUILDDIR)/doctest/output.txt." xml: $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml @echo @echo "Build finished. The XML files are in $(BUILDDIR)/xml." pseudoxml: $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml @echo @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." filterpy-1.4.5/docs/common/000077500000000000000000000000001335747705400156165ustar00rootroot00000000000000filterpy-1.4.5/docs/common/common.rst000066400000000000000000000011571335747705400176440ustar00rootroot00000000000000common ====== A collection of functions used throughout FilterPy, and/or functions that you will find useful when you build your filters. .. automodule:: filterpy.common ----- .. autofunction:: Saver ----- .. autofunction:: Q_discrete_white_noise ----- .. autofunction:: Q_continuous_white_noise ----- .. autofunction:: van_loan_discretization ----- .. autofunction:: linear_ode_discretation ----- .. autofunction:: kinematic_kf ----- .. autofunction:: kinematic_state_transition ----- .. autofunction:: runge_kutta4 ----- .. autofunction:: inv_diagonal ----- .. autofunction:: outer_product_sum filterpy-1.4.5/docs/conf.py000066400000000000000000000260501335747705400156300ustar00rootroot00000000000000#!/usr/bin/env python3 # -*- coding: utf-8 -*- # # FilterPy documentation build configuration file, created by # sphinx-quickstart on Sat Nov 22 14:54:37 2014. # # This file is execfile()d with the current directory set to its # containing dir. # # Note that not all possible configuration values are present in this # autogenerated file. # # All configuration values have a default; values that are commented out # serve to show the default. import sys import os import mock MOCK_MODULES = ['numpy', 'scipy', 'matplotlib', 'matplotlib.pyplot', 'scipy.linalg', 'numpy.linalg', 'matplotlib.pyplot', 'numpy.random', 'scipy.sparse', 'scipy.sparse.linalg', 'scipy.stats', 'matplotlib.patches', 'scipy.ndimage.filters', 'scipy.ndimage.interpolation', 'scipy.ndimage'] for mod_name in MOCK_MODULES: sys.modules[mod_name] = mock.Mock() # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. #sys.path.insert(0, os.path.abspath('.')) #sys.path.insert(0, os.path.abspath('../filterpy')) sys.path.insert(0, os.path.abspath('../')) from filterpy import * import filterpy import filterpy.kalman # -- General configuration ------------------------------------------------ # If your documentation needs a minimal Sphinx version, state it here. #needs_sphinx = '1.0' # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ 'sphinx.ext.autodoc', 'sphinx.ext.autosummary', 'sphinx.ext.doctest', 'numpydoc', 'sphinx.ext.intersphinx', 'sphinx.ext.mathjax', 'sphinx.ext.viewcode', 'sphinx.ext.autodoc', 'sphinx.ext.napoleon' ] numpydoc_show_class_members = False # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] # The suffix of source filenames. source_suffix = '.rst' # The encoding of source files. #source_encoding = 'utf-8-sig' # The master toctree document. master_doc = 'index' # General information about the project. project = 'FilterPy' copyright = '2014-2016, Roger R. Labbe' # The version info for the project you're documenting, acts as replacement for # |version| and |release|, also used in various other places throughout the # built documents. # # The short X.Y version. version = filterpy.__version__ # The full version, including alpha/beta/rc tags. release = filterpy.__version__ # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. #language = None # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: #today = '' # Else, today_fmt is used as the format for a strftime call. #today_fmt = '%B %d, %Y' # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. exclude_patterns = ['_build'] # The reST default role (used for this markup: `text`) to use for all # documents. #default_role = "autolink" # If true, '()' will be appended to :func: etc. cross-reference text. add_function_parentheses = False # If true, the current module name will be prepended to all description # unit titles (such as .. function::). #add_module_names = True # If true, sectionauthor and moduleauthor directives will be shown in the # output. They are ignored by default. #show_authors = False # The name of the Pygments (syntax highlighting) style to use. pygments_style = 'sphinx' # A list of ignored prefixes for module index sorting. #modindex_common_prefix = [] # If true, keep warnings as "system message" paragraphs in the built documents. #keep_warnings = False # -- Options for HTML output ---------------------------------------------- # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. html_theme = 'sphinxdoc' # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the # documentation. #html_theme_options = {} # Add any paths that contain custom themes here, relative to this directory. #html_theme_path = [] # The name for this set of Sphinx documents. If None, it defaults to # " v documentation". #html_title = None # A shorter title for the navigation bar. Default is the same as html_title. #html_short_title = None # The name of an image file (relative to this directory) to place at the top # of the sidebar. #html_logo = None # The name of an image file (within the static path) to use as favicon of the # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 # pixels large. #html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". html_static_path = ['_static'] # Add any extra paths that contain custom files (such as robots.txt or # .htaccess) here, relative to this directory. These files are copied # directly to the root of the documentation. #html_extra_path = [] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. #html_last_updated_fmt = '%b %d, %Y' # If true, SmartyPants will be used to convert quotes and dashes to # typographically correct entities. #html_use_smartypants = True # Custom sidebar templates, maps document names to template names. #html_sidebars = {} # Additional templates that should be rendered to pages, maps page names to # template names. #html_additional_pages = {} # If false, no module index is generated. #html_domain_indices = True # If false, no index is generated. #html_use_index = True # If true, the index is split into individual pages for each letter. #html_split_index = False # If true, links to the reST sources are added to the pages. html_show_sourcelink = True # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. html_show_sphinx = True # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. html_show_copyright = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. #html_use_opensearch = '' # This is the file name suffix for HTML files (e.g. ".xhtml"). #html_file_suffix = None # Output file base name for HTML help builder. htmlhelp_basename = 'FilterPydoc' # -- Options for LaTeX output --------------------------------------------- latex_elements = { # The paper size ('letterpaper' or 'a4paper'). #'papersize': 'letterpaper', # The font size ('10pt', '11pt' or '12pt'). #'pointsize': '10pt', # Additional stuff for the LaTeX preamble. #'preamble': '', } # Grouping the document tree into LaTeX files. List of tuples # (source start file, target name, title, # author, documentclass [howto, manual, or own class]). latex_documents = [ ('index', 'FilterPy.tex', 'FilterPy Documentation', 'Roger R. Labbe', 'manual'), ] # The name of an image file (relative to this directory) to place at the top of # the title page. #latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. #latex_use_parts = False # If true, show page references after internal links. #latex_show_pagerefs = False # If true, show URL addresses after external links. #latex_show_urls = False # Documents to append as an appendix to all manuals. #latex_appendices = [] # If false, no module index is generated. #latex_domain_indices = True # -- Options for manual page output --------------------------------------- # One entry per manual page. List of tuples # (source start file, name, description, authors, manual section). man_pages = [ ('index', 'filterpy', 'FilterPy Documentation', ['Roger R. Labbe'], 1) ] # If true, show URL addresses after external links. #man_show_urls = False # -- Options for Texinfo output ------------------------------------------- # Grouping the document tree into Texinfo files. List of tuples # (source start file, target name, title, author, # dir menu entry, description, category) texinfo_documents = [ ('index', 'FilterPy', 'FilterPy Documentation', 'Roger R. Labbe', 'FilterPy', 'One line description of project.', 'Miscellaneous'), ] # Documents to append as an appendix to all manuals. #texinfo_appendices = [] # If false, no module index is generated. #texinfo_domain_indices = True # How to display URL addresses: 'footnote', 'no', or 'inline'. #texinfo_show_urls = 'footnote' # If true, do not generate a @detailmenu in the "Top" node's menu. #texinfo_no_detailmenu = False # -- Options for Epub output ---------------------------------------------- # Bibliographic Dublin Core info. epub_title = 'FilterPy' epub_author = 'Roger R. Labbe' epub_publisher = 'Roger R. Labbe' epub_copyright = '2015, Roger R. Labbe' # The basename for the epub file. It defaults to the project name. #epub_basename = 'FilterPy' # The HTML theme for the epub output. Since the default themes are not optimized # for small screen space, using the same theme for HTML and epub output is # usually not wise. This defaults to 'epub', a theme designed to save visual # space. #epub_theme = 'epub' # The language of the text. It defaults to the language option # or en if the language is not set. #epub_language = '' # The scheme of the identifier. Typical schemes are ISBN or URL. #epub_scheme = '' # The unique identifier of the text. This can be a ISBN number # or the project homepage. #epub_identifier = '' # A unique identification for the text. #epub_uid = '' # A tuple containing the cover image and cover page html template filenames. #epub_cover = () # A sequence of (type, uri, title) tuples for the guide element of content.opf. #epub_guide = () # HTML files that should be inserted before the pages created by sphinx. # The format is a list of tuples containing the path and title. #epub_pre_files = [] # HTML files shat should be inserted after the pages created by sphinx. # The format is a list of tuples containing the path and title. #epub_post_files = [] # A list of files that should not be packed into the epub file. epub_exclude_files = ['search.html'] # The depth of the table of contents in toc.ncx. #epub_tocdepth = 3 # Allow duplicate toc entries. #epub_tocdup = True # Choose between 'default' and 'includehidden'. #epub_tocscope = 'default' # Fix unsupported image types using the PIL. #epub_fix_images = False # Scale large images. #epub_max_image_width = 0 # How to display URL addresses: 'footnote', 'no', or 'inline'. #epub_show_urls = 'inline' # If false, no index is generated. #epub_use_index = True # Example configuration for intersphinx: refer to the Python standard library. intersphinx_mapping = {'http://docs.python.org/': None} autodoc_member_order = 'bysource' filterpy-1.4.5/docs/discrete_bayes/000077500000000000000000000000001335747705400173135ustar00rootroot00000000000000filterpy-1.4.5/docs/discrete_bayes/discrete_bayes.rst000066400000000000000000000002751335747705400230360ustar00rootroot00000000000000Discrete Bayes ============== words go here .. automodule:: filterpy.discrete_bayes ----- .. autofunction:: normalize ----- .. autofunction:: update ----- .. autofunction:: predict filterpy-1.4.5/docs/gh/000077500000000000000000000000001335747705400147245ustar00rootroot00000000000000filterpy-1.4.5/docs/gh/GHFilter.rst000066400000000000000000000002311335747705400171160ustar00rootroot00000000000000GHFilter ======== Implements the g-h filter. ------- .. automodule:: filterpy.gh .. autoclass:: GHFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/gh/GHFilterOrder.rst000066400000000000000000000002101335747705400201070ustar00rootroot00000000000000GHFilterOrder ============= .. automodule:: filterpy.gh .. autoclass:: GHFilterOrder :members: .. automethod:: __init__ filterpy-1.4.5/docs/gh/GHKFilter.rst000066400000000000000000000001671335747705400172410ustar00rootroot00000000000000GHKFilter ========= .. automodule:: filterpy.gh .. autoclass:: GHKFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/gh/benedict_bornder_constants.rst000066400000000000000000000002021335747705400230340ustar00rootroot00000000000000benedict_bornder_constants ========================== .. automodule:: filterpy.gh .. autofunction:: benedict_bornder_constants filterpy-1.4.5/docs/gh/critical_damping_parameters.rst000066400000000000000000000002041335747705400231660ustar00rootroot00000000000000critical_damping_parameters =========================== .. automodule:: filterpy.gh .. autofunction:: critical_damping_parameters filterpy-1.4.5/docs/gh/least_squares_parameters.rst000066400000000000000000000001741335747705400225560ustar00rootroot00000000000000least_squares_parameters ======================== .. automodule:: filterpy.gh .. autofunction:: least_squares_parameters filterpy-1.4.5/docs/gh/optimal_noise_smoothing.rst000066400000000000000000000001711335747705400224060ustar00rootroot00000000000000optimal_noise_smoothing ======================= .. automodule:: filterpy.gh .. autofunction:: optimal_noise_smoothing filterpy-1.4.5/docs/hinfinity/000077500000000000000000000000001335747705400163275ustar00rootroot00000000000000filterpy-1.4.5/docs/hinfinity/HInfinityFilter.rst000066400000000000000000000002241335747705400221260ustar00rootroot00000000000000HInfinityFilter ================== .. automodule:: filterpy.hinfinity .. autoclass:: HInfinityFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/index.rst000066400000000000000000000310301335747705400161640ustar00rootroot00000000000000.. FilterPy documentation master file FilterPy ******** .. toctree:: :maxdepth: 2 FilterPy is a Python library that implements a number of Bayesian filters, most notably Kalman filters. I am writing it in conjunction with my book *Kalman and Bayesian Filters in Python* [1]_, a free book written using Ipython Notebook, hosted on github, and readable via nbviewer. However, it implements a wide variety of functionality that is not described in the book. As such this library has a strong pedalogical flavor. It is rare that I choose the most efficient way to calculate something unless it does not obscure exposition of the concepts of the filtering being done. I will always opt for clarity over speed. I do not mean to imply that this is a toy; I use it all of the time in my job. I mainly develop in Python 3.x, but this should support both Python 2.x and 3.x flavors. At the moment I can not tell you the lowest required version; I tend to develop on the bleeding edge of the Python releases. I am happy to receive bug reports if it does not work with older versions, but testing backwards compatibility is not a high priority at the moment. As the package matures I will shift my focus in that direction. FilterPy requires Numpy [2]_ and SciPy [3]_ to work. The tests and examples also use matplotlib [4]_. For testing I use py.test [5]_. Installation ============ Installation with pip (recommended) ----------------------------------- FilterPy is available on github (https://github.com/rlabbe/filterpy). However, it is also hosted on PyPi, and unless you want to be on the bleeding edge of development I recommend you get it from there. To install from the command line, merely type: .. code-block:: bash $ pip install filterpy To test the installation, from a python REPL type: >>> import filterpy >>> filterpy.__version__ and it should display the version number that you installed. Installation with GitHub ------------------------ You can get the very latest code by getting it from GitHub and then performing the installation. I will say I am not following particularly stringent version control discipline. I mostly stay on **master** and commit things that are not entirely ready for prime-time, mostly because I'm the only one developing. I do not promise that any check in that is not tagged with a version number is usable. .. code-block:: bash $ git clone --depth=1 https://github.com/rlabbe/filterpy.git $ cd filterpy $ python setup.py install `--depth=1` just gets you the last few revisions that I made, which keeps the repo small. If you want the entire repo leave out the `depth` parameter, or fork the repo if you plan to modify it. Use === There are several submodules, each listed below. But in general you will need to import which classes and/or functions you need from the correct submodule, construct the objects, and then execute your code. Something lke >>> from filterpy.kalman import KalmanFilter >>> kf = KalmanFilter(dim_x=3, dim_z=1) I try to provide examples in the help for each class, but this documentation needs a lot of work. For now I refer you to my book mentioned above if the documentation is not adequate. Better yet, write an issue on the GitHub issue tracker. I will respond with an answer as soon as I am online and available (minutes to a day, normally), and then revise the documentation. I shouldn't have to be prodded like this, but life is limited. So prod. Raise issues here: https://github.com/rlabbe/filterpy/issues FilterPy's Naming Conventions ============================== A word on variable names. I am an advocate for descriptive variable names. In the Kalman filter literature the measurement noise covariance matrix is called `R`. The name `R` is not descriptive. I could reasonably call it `measurement_noise_covariance`, and I've seen libraries do that. I've chosen not to. In the end, Kalman filtering is math. To write a Kalman filter you are going to start by sitting down with a piece of paper and doing math. You will be writing and solving normal algebraic equations. Every Kalman filter text and source on the web uses the same equations. You cannot read about the Kalman filter without seeing this equation .. math:: \dot{\mathbf{x}} = \mathbf{Fx} + \mathbf{Gu} + w One of my goals is to bring you to the point where you can read the original literature on Kalman filtering. For nontrivial problems the difficulty is not the implementation of the equations, but learning how to set up the equations so they solve your problem. In other words, every Kalman filter implements :math:`\dot{\mathbf{x}} = \mathbf{Fx} + \mathbf{Gu} + w`; the difficult part is figuring out what to put in the matrices :math:`\mathbf{F}` and :math:`\mathbf{G}` to make your filter work for your problem. Vast amounts of work have been done to apply Kalman filters in various domains, and it would be tragic to be unable to avail yourself of this research. So, like it or not you will need to learn that :math:`\mathbf{F}` is the *state transition matrix* and that :math:`\mathbf{R}` is the *measurement noise covariance*. Once you know that the code will become readable, and until then Kalman filter math, and all publications and web articles on Kalman filters will be inaccessible to you. Finally, I think that mathematical programming is somewhat different than regular programming; what is readable in one domain is not readable in another. `q = x + m` is opaque in a normal context. On the other hand, `x = (.5*a)*t**2 + v_0*t + x_0` is to me the most readable way to program the Newtonian distance equation: .. math:: x = \frac{1}{2}at^2 + v_0 t + x_0 We could write it as .. code-block:: Python distance = (.5 * constant_acceleration) * time_delta**2 + initial_velocity * time_delta + initial_distance but I feel that obscures readability. This is debatable for this one equation; but most mathematical programs, and certainly Kalman filters, use systems of equations. I can most easily follow the code, and ensure that it does not have bugs, when it reads as close to the math as possible. Consider this equation from the Kalman filter: .. math:: \mathbf{K} = \mathbf{PH}^\mathsf{T}[\mathbf{HPH}^\mathsf{T} + \mathbf{R}]^{-1} Python code for this would be .. code-block:: Python K = dot(P, H.T).dot(inv(dot(H, P).dot(H.T) + R)) It's already a bit hard to read because of the `dot` function calls (required because Python does not yet support an operator for matrix multiplication). But compare this to:: kalman_gain = ( dot(apriori_state_covariance, measurement_function_transpose).dot( inv(dot(measurement_function, apriori_state_covariance).dot( measurement_function_transpose) + measurement_noise_covariance))) which I adapted from a popular library. I grant you this version has more context, but I cannot glance at this and see what math it is implementing. In particular, the linear algebra :math:`\mathbf{HPH}^\mathsf{T}` is doing something very specific - multiplying :math:`\mathbf{P}` by :math:`\mathbf{H}` in a way that converts :math:`\mathbf{P}` from *world space* to *measurement space* (we'll learn what that means). It is nearly impossible to see that the Kalman gain (`K`) is just a ratio of one number divided by a second number which has been converted to a different basis. This statement may not convey a lot of information to you before reading the book, but I assure you that :math:`\mathbf{K} = \mathbf{PH}^\mathsf{T}[\mathbf{HPH}^\mathsf{T} + \mathbf{R}]^{-1}` is saying something very succinctly. There are two key pieces of information here - we are finding a ratio, and we are doing it in measurement space. I can see that in my first Python line, I cannot see that in the second line. If you want a counter-argument, my version obscures the information that :math:`\mathbf{P}` is in this context is a *prior* . These comments apply to library code. Calling code should use names like `sensor_noise`, or `gps_sensor_noise`, not `R`. Math code should read like math, and interface or glue code should read like normal code. Context is important. I will not *win* this argument, and some people will not agree with my naming choices. I will finish by stating, very truthfully, that I made two mistakes the first time I typed the second version and it took me awhile to find it. In any case, I aim for using the mathematical symbol names whenever possible, coupled with readable class and function names. So, it is `KalmanFilter.P`, not `KF.P` and not `KalmanFilter.apriori_state_covariance`. Communication ============= Unless it is deeply private (you don't want someone else seeing propietary code, for example), please ask questions and such on the issue tracker, not by email. This is solely so that everyone gets to see the answer. "Issue" doesn't mean bug. Modules ======= filterpy.kalman --------------- The classes in this submodule implement the various Kalman filters. There is also support for smoother functions. The smoothers are methods of the classes. For example, the KalmanFilter class contains rts_smoother to perform Rauch-Tung-Striebal smoothing. Linear Kalman Filters +++++++++++++++++++++ Implements various Kalman filters using the linear equations form of the filter. .. toctree:: :maxdepth: 1 kalman/KalmanFilter kalman/Saver kalman/FixedLagSmoother kalman/SquareRootFilter kalman/InformationFilter kalman/FadingKalmanFilter kalman/MMAEFilterBank kalman/IMMEstimator Extended Kalman Filter ++++++++++++++++++++++ .. toctree:: :maxdepth: 1 kalman/ExtendedKalmanFilter Unscented Kalman Filter +++++++++++++++++++++++ These modules are used to implement the Unscented Kalman filter. .. toctree:: :maxdepth: 1 kalman/UnscentedKalmanFilter kalman/unscented_transform Ensemble Kalman Filter +++++++++++++++++++++++ .. toctree:: :maxdepth: 1 kalman/EnsembleKalmanFilter filterpy.common --------------- Contains various useful functions that support the filtering classes and functions. Most useful are functions to compute the process noise matrix Q. It also implements the Van Loan discretization of a linear differential equation. .. toctree:: :maxdepth: 1 common/common filterpy.stats -------------- Contains statistical functions useful for Kalman filtering such as multivariate Gaussian multiplication, computing the log-likelihood, NESS, and mahalanobis distance, along with plotting routines to plot multivariate Gaussians CDFs, PDFs, and covariance ellipses. .. toctree:: :maxdepth: 1 stats/stats filterpy.monte_carlo -------------------- Routines for Markov Chain Monte Carlo (MCMC) computation, mainly for particle filtering. .. toctree:: :maxdepth: 1 monte_carlo/resampling filterpy.discrete_bayes ----------------------- Routines for performing discrete Bayes filtering. .. toctree:: :maxdepth: 1 discrete_bayes/discrete_bayes filterpy.gh ----------- These classes various g-h filters. The functions are helpers that provide settings for the *g* and *h* parameters for various common filters. .. toctree:: :maxdepth: 1 gh/GHFilterOrder gh/GHFilter gh/GHKFilter gh/optimal_noise_smoothing gh/least_squares_parameters gh/critical_damping_parameters gh/benedict_bornder_constants filterpy.memory --------------- Implements a polynomial fading memory filter. You can achieve the same results, and more, using the KalmanFilter class. However, some books use this form of the fading memory filter, so it is here for completeness. I suppose some would also find this simpler to use than the standard Kalman filter. .. toctree:: :maxdepth: 1 memory/FadingMemoryFilter filterpy.hinfinity ------------------ .. toctree:: :maxdepth: 1 hinfinity/HInfinityFilter filterpy.leastsq ++++++++++++++++ .. toctree:: :maxdepth: 1 leastsq/LeastSquaresFilter References ========== .. [1] Labbe, Roger. "Kalman and Bayesian Filters in Python". github repo: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python read online: http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb PDF version (often lags the two sources above) https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/Kalman_and_Bayesian_Filters_in_Python.pdf .. [2] NumPy http://www.numpy.org .. [3] SciPy http://www.scipy.org .. [4] matplotlib http://http://matplotlib.org/ .. [5] pytest http://pytest.org/latest/ Indices and tables ================== * :ref:`genindex` * :ref:`modindex` * :ref:`search` filterpy-1.4.5/docs/kalman/000077500000000000000000000000001335747705400155715ustar00rootroot00000000000000filterpy-1.4.5/docs/kalman/EnsembleKalmanFilter.rst000066400000000000000000000004141335747705400223460ustar00rootroot00000000000000EnsembleKalmanFilter ===================== Introduction and Overview ------------------------- This implements the Ensemble Kalman filter. .. automodule:: filterpy.kalman -------- .. autoclass:: EnsembleKalmanFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/ExtendedKalmanFilter.rst000066400000000000000000000034161335747705400223610ustar00rootroot00000000000000ExtendedKalmanFilter ==================== Introduction and Overview ------------------------- Implements a extended Kalman filter. For now the best documentation is my free book Kalman and Bayesian Filters in Python [1]_ The test files in this directory also give you a basic idea of use, albeit without much description. In brief, you will first construct this object, specifying the size of the state vector with `dim_x` and the size of the measurement vector that you will be using with `dim_z`. These are mostly used to perform size checks when you assign values to the various matrices. For example, if you specified `dim_z=2` and then try to assign a 3x3 matrix to R (the measurement noise matrix you will get an assert exception because R should be 2x2. (If for whatever reason you need to alter the size of things midstream just use the underscore version of the matrices to assign directly: your_filter._R = a_3x3_matrix.) After construction the filter will have default matrices created for you, but you must specify the values for each. It's usually easiest to just overwrite them rather than assign to each element yourself. This will be clearer in the example below. All are of type numpy.array. **References** .. [1] Labbe, Roger. "Kalman and Bayesian Filters in Python". github repo: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python read online: http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb PDF version (often lags the two sources above) https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/Kalman_and_Bayesian_Filters_in_Python.pdf ------- .. automodule:: filterpy.kalman .. autoclass:: ExtendedKalmanFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/FadingKalmanFilter.rst000066400000000000000000000003511335747705400220040ustar00rootroot00000000000000FadingKalmanFilter ================== Implements a fading memory Kalman filter. ------- .. automodule:: filterpy.kalman Fading Memory Kalman filter .. autoclass:: FadingKalmanFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/FixedLagSmoother.rst000066400000000000000000000003751335747705400215340ustar00rootroot00000000000000FixedLagSmoother ================ Introduction and Overview ------------------------- This implements a fixed lag Kalman smoother. ------- .. automodule:: filterpy.kalman .. autoclass:: FixedLagSmoother :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/IMMEstimator.rst000066400000000000000000000002701335747705400206340ustar00rootroot00000000000000IMM Estimator ================ needs documentation.... ------- .. automodule:: filterpy.kalman .. autoclass:: IMMEstimator :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/InformationFilter.rst000066400000000000000000000004161335747705400217570ustar00rootroot00000000000000InformationFilter ================= Introduction and Overview ------------------------- This is a basic implementation of the information filter. ------- .. automodule:: filterpy.kalman .. autoclass:: InformationFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/KalmanFilter.rst000066400000000000000000000125521335747705400207010ustar00rootroot00000000000000KalmanFilter ============ Implements a linear Kalman filter. For now the best documentation is my free book Kalman and Bayesian Filters in Python [2]_ The test files in this directory also give you a basic idea of use, albeit without much description. In brief, you will first construct this object, specifying the size of the state vector with `dim_x` and the size of the measurement vector that you will be using with `dim_z`. These are mostly used to perform size checks when you assign values to the various matrices. For example, if you specified `dim_z=2` and then try to assign a 3x3 matrix to R (the measurement noise matrix you will get an assert exception because R should be 2x2. (If for whatever reason you need to alter the size of things midstream just use the underscore version of the matrices to assign directly: your_filter._R = a_3x3_matrix.) After construction the filter will have default matrices created for you, but you must specify the values for each. It's usually easiest to just overwrite them rather than assign to each element yourself. This will be clearer in the example below. All are of type numpy.array. These are the matrices (instance variables) which you must specify. All are of type numpy.array (do NOT use numpy.matrix) If dimensional analysis allows you to get away with a 1x1 matrix you may also use a scalar. All elements must have a type of float. **Instance Variables** You will have to assign reasonable values to all of these before running the filter. All must have dtype of float. x : ndarray (dim_x, 1), default = [0,0,0...0] filter state estimate P : ndarray (dim_x, dim_x), default eye(dim_x) covariance matrix Q : ndarray (dim_x, dim_x), default eye(dim_x) Process uncertainty/noise R : ndarray (dim_z, dim_z), default eye(dim_z) measurement uncertainty/noise H : ndarray (dim_z, dim_x) measurement function F : ndarray (dim_x, dim_x) state transistion matrix B : ndarray (dim_x, dim_u), default 0 control transition matrix **Optional Instance Variables** alpha : float Assign a value > 1.0 to turn this into a fading memory filter. **Read-only Instance Variables** K : ndarray Kalman gain that was used in the most recent update() call. y : ndarray Residual calculated in the most recent update() call. I.e., the different between the measurement and the current estimated state projected into measurement space (z - Hx) S : ndarray System uncertainty projected into measurement space. I.e., HPH' + R. Probably not very useful, but it is here if you want it. likelihood : float Likelihood of last measurment update. log_likelihood : float Log likelihood of last measurment update. **Example** Here is a filter that tracks position and velocity using a sensor that only reads position. First construct the object with the required dimensionality. .. code:: from filterpy.kalman import KalmanFilter f = KalmanFilter (dim_x=2, dim_z=1) Assign the initial value for the state (position and velocity). You can do this with a two dimensional array like so: .. code:: f.x = np.array([[2.], # position [0.]]) # velocity or just use a one dimensional array, which I prefer doing. .. code:: f.x = np.array([2., 0.]) Define the state transition matrix: .. code:: f.F = np.array([[1.,1.], [0.,1.]]) Define the measurement function: .. code:: f.H = np.array([[1.,0.]]) Define the covariance matrix. Here I take advantage of the fact that P already contains np.eye(dim_x), and just multiply by the uncertainty: .. code:: f.P *= 1000. I could have written: .. code:: f.P = np.array([[1000., 0.], [ 0., 1000.] ]) You decide which is more readable and understandable. Now assign the measurement noise. Here the dimension is 1x1, so I can use a scalar .. code:: f.R = 5 I could have done this instead: .. code:: f.R = np.array([[5.]]) Note that this must be a 2 dimensional array, as must all the matrices. Finally, I will assign the process noise. Here I will take advantage of another FilterPy library function: .. code:: from filterpy.common import Q_discrete_white_noise f.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13) Now just perform the standard predict/update loop: while some_condition_is_true: .. code:: z = get_sensor_reading() f.predict() f.update(z) do_something_with_estimate (f.x) **Procedural Form** This module also contains stand alone functions to perform Kalman filtering. Use these if you are not a fan of objects. **Example** .. code:: while True: z, R = read_sensor() x, P = predict(x, P, F, Q) x, P = update(x, P, z, R, H) **References** .. [2] Labbe, Roger. "Kalman and Bayesian Filters in Python". github repo: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python read online: http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb PDF version (often lags the two sources above) https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/Kalman_and_Bayesian_Filters_in_Python.pdf ------- .. automodule:: filterpy.kalman Kalman filter .. autoclass:: KalmanFilter :members: .. automethod:: __init__ .. autofunction:: update .. autofunction:: predict .. autofunction:: batch_filter filterpy-1.4.5/docs/kalman/MMAEFilterBank.rst000066400000000000000000000016331335747705400210070ustar00rootroot00000000000000MMAE Filter Bank ================ needs documentation.... **Example** .. code:: from filterpy.kalman import MMAEFilterBank pos, zs = generate_data(120, noise_factor=0.2) z_xs = zs[:, 0] t = np.arange(0, len(z_xs) * dt, dt) dt = 0.1 filters = [make_cv_filter(dt), make_ca_filter(dt)] H_cv = np.array([[1., 0, 0], [0., 1, 0]]) H_ca = np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) bank = MMAEFilterBank(filters, (0.5, 0.5), dim_x=3, H=(H_cv, H_ca)) xs, probs = [], [] for z in z_xs: bank.predict() bank.update(z) xs.append(bank.x[0]) probs.append(bank.p[0]) plt.subplot(121) plt.plot(xs) plt.subplot(122) plt.plot(probs) ------- .. automodule:: filterpy.kalman .. autoclass:: MMAEFilterBank :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/Saver.rst000066400000000000000000000014041335747705400174020ustar00rootroot00000000000000Saver ===== This is a helper class designed to allow you to save the state of the Kalman filter for each epoch. Each instance variable is stored in a list when you call save(). This class is deprecated as of version 1.3.2 and will be deleted soon. Instead, see the class filterpy.common.Saver, which works for any class, not just a KalmanFilter object. **Example** .. code:: saver = Saver(kf) for i in range(N): kf.predict() kf.update(zs[i]) saver.save() saver.to_array() # convert all to np.array # plot the 0th element of kf.x over all epoches plot(saver.xs[:, 0]) ------- .. automodule:: filterpy.kalman Kalman filter saver .. autoclass:: Saver :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/SquareRootFilter.rst000066400000000000000000000014021335747705400215720ustar00rootroot00000000000000SquareRootKalmanFilter ====================== Introduction and Overview ------------------------- This implements a square root Kalman filter. No real attempt has been made to make this fast; it is a pedalogical exercise. The idea is that by computing and storing the square root of the covariance matrix we get about double the significant number of bits. Some authors consider this somewhat unnecessary with modern hardware. Of course, with microcontrollers being all the rage these days, that calculus has changed. But, will you really run a Kalman filter in Python on a tiny chip? No. So, this is for learning. ------- .. automodule:: filterpy.kalman Square Root Kalman Filter .. autoclass:: SquareRootKalmanFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/UnscentedKalmanFilter.rst000066400000000000000000000010431335747705400225430ustar00rootroot00000000000000UnscentedKalmanFilter ===================== Introduction and Overview ------------------------- This implements the unscented Kalman filter. .. automodule:: filterpy.kalman -------- .. autoclass:: UnscentedKalmanFilter :members: .. automethod:: __init__ -------- .. autoclass:: MerweScaledSigmaPoints :members: .. automethod:: __init__ -------- .. autoclass:: JulierSigmaPoints :members: .. automethod:: __init__ -------- .. autoclass:: SimplexSigmaPoints :members: .. automethod:: __init__ filterpy-1.4.5/docs/kalman/unscented_transform.rst000066400000000000000000000001601335747705400224030ustar00rootroot00000000000000unscented_transform =================== .. automodule:: filterpy.kalman .. autofunction:: unscented_transform filterpy-1.4.5/docs/leastsq/000077500000000000000000000000001335747705400160025ustar00rootroot00000000000000filterpy-1.4.5/docs/leastsq/LeastSquaresFilter.rst000066400000000000000000000002271335747705400223170ustar00rootroot00000000000000LeastSquaresFilter ================== .. automodule:: filterpy.leastsq .. autoclass:: LeastSquaresFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/make.bat000066400000000000000000000144771335747705400157500ustar00rootroot00000000000000@ECHO OFF REM Command file for Sphinx documentation if "%SPHINXBUILD%" == "" ( set SPHINXBUILD=sphinx-build ) set BUILDDIR=_build set ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% . set I18NSPHINXOPTS=%SPHINXOPTS% . if NOT "%PAPER%" == "" ( set ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS% set I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS% ) if "%1" == "" goto help if "%1" == "help" ( :help echo.Please use `make ^` where ^ is one of echo. html to make standalone HTML files echo. dirhtml to make HTML files named index.html in directories echo. singlehtml to make a single large HTML file echo. pickle to make pickle files echo. json to make JSON files echo. htmlhelp to make HTML files and a HTML help project echo. qthelp to make HTML files and a qthelp project echo. devhelp to make HTML files and a Devhelp project echo. epub to make an epub echo. latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter echo. text to make text files echo. man to make manual pages echo. texinfo to make Texinfo files echo. gettext to make PO message catalogs echo. changes to make an overview over all changed/added/deprecated items echo. xml to make Docutils-native XML files echo. pseudoxml to make pseudoxml-XML files for display purposes echo. linkcheck to check all external links for integrity echo. doctest to run all doctests embedded in the documentation if enabled goto end ) if "%1" == "clean" ( for /d %%i in (%BUILDDIR%\*) do rmdir /q /s %%i del /q /s %BUILDDIR%\* goto end ) %SPHINXBUILD% 2> nul if errorlevel 9009 ( echo. echo.The 'sphinx-build' command was not found. Make sure you have Sphinx echo.installed, then set the SPHINXBUILD environment variable to point echo.to the full path of the 'sphinx-build' executable. Alternatively you echo.may add the Sphinx directory to PATH. echo. echo.If you don't have Sphinx installed, grab it from echo.http://sphinx-doc.org/ exit /b 1 ) if "%1" == "html" ( %SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html if errorlevel 1 exit /b 1 echo. echo.Build finished. The HTML pages are in %BUILDDIR%/html. goto end ) if "%1" == "dirhtml" ( %SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml if errorlevel 1 exit /b 1 echo. echo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml. goto end ) if "%1" == "singlehtml" ( %SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml if errorlevel 1 exit /b 1 echo. echo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml. goto end ) if "%1" == "pickle" ( %SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle if errorlevel 1 exit /b 1 echo. echo.Build finished; now you can process the pickle files. goto end ) if "%1" == "json" ( %SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json if errorlevel 1 exit /b 1 echo. echo.Build finished; now you can process the JSON files. goto end ) if "%1" == "htmlhelp" ( %SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp if errorlevel 1 exit /b 1 echo. echo.Build finished; now you can run HTML Help Workshop with the ^ .hhp project file in %BUILDDIR%/htmlhelp. goto end ) if "%1" == "qthelp" ( %SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp if errorlevel 1 exit /b 1 echo. echo.Build finished; now you can run "qcollectiongenerator" with the ^ .qhcp project file in %BUILDDIR%/qthelp, like this: echo.^> qcollectiongenerator %BUILDDIR%\qthelp\FilterPy.qhcp echo.To view the help file: echo.^> assistant -collectionFile %BUILDDIR%\qthelp\FilterPy.ghc goto end ) if "%1" == "devhelp" ( %SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp if errorlevel 1 exit /b 1 echo. echo.Build finished. goto end ) if "%1" == "epub" ( %SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub if errorlevel 1 exit /b 1 echo. echo.Build finished. The epub file is in %BUILDDIR%/epub. goto end ) if "%1" == "latex" ( %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex if errorlevel 1 exit /b 1 echo. echo.Build finished; the LaTeX files are in %BUILDDIR%/latex. goto end ) if "%1" == "latexpdf" ( %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex cd %BUILDDIR%/latex make all-pdf cd %BUILDDIR%/.. echo. echo.Build finished; the PDF files are in %BUILDDIR%/latex. goto end ) if "%1" == "latexpdfja" ( %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex cd %BUILDDIR%/latex make all-pdf-ja cd %BUILDDIR%/.. echo. echo.Build finished; the PDF files are in %BUILDDIR%/latex. goto end ) if "%1" == "text" ( %SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text if errorlevel 1 exit /b 1 echo. echo.Build finished. The text files are in %BUILDDIR%/text. goto end ) if "%1" == "man" ( %SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man if errorlevel 1 exit /b 1 echo. echo.Build finished. The manual pages are in %BUILDDIR%/man. goto end ) if "%1" == "texinfo" ( %SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo if errorlevel 1 exit /b 1 echo. echo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo. goto end ) if "%1" == "gettext" ( %SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale if errorlevel 1 exit /b 1 echo. echo.Build finished. The message catalogs are in %BUILDDIR%/locale. goto end ) if "%1" == "changes" ( %SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes if errorlevel 1 exit /b 1 echo. echo.The overview file is in %BUILDDIR%/changes. goto end ) if "%1" == "linkcheck" ( %SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck if errorlevel 1 exit /b 1 echo. echo.Link check complete; look for any errors in the above output ^ or in %BUILDDIR%/linkcheck/output.txt. goto end ) if "%1" == "doctest" ( %SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest if errorlevel 1 exit /b 1 echo. echo.Testing of doctests in the sources finished, look at the ^ results in %BUILDDIR%/doctest/output.txt. goto end ) if "%1" == "xml" ( %SPHINXBUILD% -b xml %ALLSPHINXOPTS% %BUILDDIR%/xml if errorlevel 1 exit /b 1 echo. echo.Build finished. The XML files are in %BUILDDIR%/xml. goto end ) if "%1" == "pseudoxml" ( %SPHINXBUILD% -b pseudoxml %ALLSPHINXOPTS% %BUILDDIR%/pseudoxml if errorlevel 1 exit /b 1 echo. echo.Build finished. The pseudo-XML files are in %BUILDDIR%/pseudoxml. goto end ) :end filterpy-1.4.5/docs/memory/000077500000000000000000000000001335747705400156365ustar00rootroot00000000000000filterpy-1.4.5/docs/memory/FadingMemoryFilter.rst000066400000000000000000000010011335747705400221070ustar00rootroot00000000000000FadingMemoryFilter ================== Introduction and Overview ------------------------- Implements a polynomial fading memory filter. You can achieve the same results, and more, using the KalmanFilter class. However, some books use this form of the fading memory filter, so it is here for completeness. I suppose some would also find this simpler to use than the standard Kalman filter. ------ .. automodule:: filterpy.memory .. autoclass:: FadingMemoryFilter :members: .. automethod:: __init__ filterpy-1.4.5/docs/monte_carlo/000077500000000000000000000000001335747705400166305ustar00rootroot00000000000000filterpy-1.4.5/docs/monte_carlo/resampling.rst000066400000000000000000000010231335747705400215170ustar00rootroot00000000000000resampling ========== Routines for resampling particles from particle filters based on their current weights. All these routines take a list of normalized weights and returns a list of indexes to the weights that should be chosen for resampling. The caller is responsible for performing the actual resample. .. automodule:: filterpy.monte_carlo ----- .. autofunction:: residual_resample ----- .. autofunction:: stratified_resample ----- .. autofunction:: systematic_resample ----- .. autofunction:: multinomial_resample filterpy-1.4.5/docs/stats/000077500000000000000000000000001335747705400154645ustar00rootroot00000000000000filterpy-1.4.5/docs/stats/stats.rst000066400000000000000000000015261335747705400173600ustar00rootroot00000000000000stats ===== A collection of functions used to compute and plot statistics relevant to Bayesian filters. .. automodule:: filterpy.stats ----- .. autofunction:: gaussian ----- .. autofunction:: mul ----- .. autofunction:: add ----- .. autofunction:: log_likelihood ----- .. autofunction:: likelihood ----- .. autofunction:: logpdf ----- .. autofunction:: multivariate_gaussian ----- .. autofunction:: multivariate_multiply ----- .. autofunction:: plot_gaussian_cdf ----- .. autofunction:: plot_gaussian_pdf ----- .. autofunction:: plot_discrete_cdf ----- .. autofunction:: plot_gaussian ----- .. autofunction:: covariance_ellipse ----- .. autofunction:: plot_covariance_ellipse ----- .. autofunction:: norm_cdf ----- .. autofunction:: rand_student_t ----- .. autofunction:: NESS ----- .. autofunction:: mahalanobis filterpy-1.4.5/filterpy/000077500000000000000000000000001335747705400152345ustar00rootroot00000000000000filterpy-1.4.5/filterpy/__init__.py000066400000000000000000000005521335747705400173470ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. filterpy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ __version__ = "1.4.5" filterpy-1.4.5/filterpy/changelog.txt000066400000000000000000000351101335747705400177240ustar00rootroot00000000000000Version 1.4.5 ============= * Removed deprecated filterpy.kalman.Saver class (use filterpy.common.Saver instead) * GitHub #165 Bug in computation of prior state x. * Sped up computation in Cubature and Ensemble filters by using einsum instead of a for loop. Version 1.4.4 ============= * GitHub #60 - Added separate computation of prior and posterior with the IMM. * various pylint compliance changes. Version 1.4.3 ============= * GitHub #152 fixed docstring for unscented_transform() * GitHub #154 Fixed bug in LeastSquaresFilter causing it to diverge * GitHub #155 Mahalanobis computations in several filters did not include taking the sqrt . Version 1.4.2 ============= * added a fast inverse of a diagonal matrix function common.inv_diagonal() * added ability to order Q matrix by dimension or derivative with the helper functions * mahalanobis, log-likelihood, and likelihood only computed when requested; `compute_log_likelihood` attribute removed. * fix for bug where attributes not saved. GitHub #136 * Many minor typos in docstring and comments fixes * Added saving of posterior state to classes * Added exception to IMM if filter bank dimensions do not agree. GitHub #141 * Added Mahalanobis distance to classes that didn't support it * added flatten() to saver as an convienent way to flatten column vectors so they will display nicely in the REPL. * Partially removed dependence on Matplotlib. Matplotlib is only imported by functions that use plotting. Github #148 * Removed ability to pass in single matrix to batch_filter functions/methods. Github #147. In general it is impossible to tell if you have a list of matrices, or an single array, depending on the shape of the arrays and the size of the input. Rather than try and obscurely fail, make the user pass the correctly dimensioned data in. * Github #151 - added ability to specify unscented transform function for the UKF RTS smoother. * Compute log-likelihood and mahalanobis only when requested GitHub #139. Some, but not all classes did this already; I normalized the behavior across all classes. * added .gitattributes to force all files to use unix-style line endings * GitHub #138. Attributes were not being set when z == None on call to update(), meaning things like log-likelihood and mahalanobis had incorrect values. Version 1.4.1 ============= * GitHub 125: used itertools.repeat() instead of [x]*n to get iterable list of constant values * Fixed bug using [:] to copy data instead of np.copy(), which is a deep copy * fixed import bug in common.kinematic_kf() Version 1.4 =========== * UKF refactor. This is a (mildly) breaking change. Passing data to h(x) and f(x) is now done by kwargs, not tuples. The sigma point classes now have attributes Wm and Wc, and weights() was deleted. I also fixed a bug that did not provide a default value for `kappa` for the JulierSigmaPoints class. Version 1.3.2 ============= Fixed build error in Python 2.7 due to using print function without importing it from future. Added filterpy.common.Saver class, which can save all the attribute of any filtering class. Replaces KalmanFilter.Saver, which only worked for the KalmanFilter class. Added optional parameter specifying a Saver object to be passed into all of the batch_filter() functions/methods. Added attribute z to most of the filter classes. This is mostly so the Changes to documentation - mostly making it more consistent. Version 1.3.1 ============= * #49 Added tests to distribution, as they contain a lot of examples Version 1.3 =========== * #113 added plotting of 3D covariance ellipsoid with plot_3d_covariance * Fixed bug where multivariate_gaussian accepted negative covariances * #108 used pylint symbolic names * Got code in compliance with pylint * Fixed #105 - this was just test code, and it turns out the code as was was correct, so I deleted the second return statement * #88 fixed HInfinity.batch_filter to not use R Version 1.2.5 ============= #102 - Bug: UKF was using slow code path when using np.subtract. Version 1.2.4 ============= * BUG: UKF smoother had an important bug in it. DO NOT use the rts_smoother prior to this release! GitHub issue #97 * Added jerk option to the computation of Q. GitHub issue #83 Version 1.2.3 ============= * I pushed a broken version! This should fix that. Do not use 1.2.2. * This push sucks in some minor documentation and PEP8 changes to a few files. Version 1.2.2 ============= * changes log-likelihood and likelihood back into attributes/properties and added to several classes so the IMM code works uniformally across all * bug fix in gh code that ignored k (submitted by Lucal Beyer) * doc block fix by Andriy Teraz Version 1.2.0 ============= * Added computation of mahalanobis distance in stats * Added better default values for F and H in KalmanFilter * Fixed bugs due to calling .dot on float64 values * Removed all of the getter/setters as unpythonic * added function to create kinematic filters * changed log likelihood and likelihood to methods to avoid the slow computation it requires. * Improved documentation of method parameters Version 1.1.0 ============= * fixed bug in likelihood to never return 0, which can happen due to floating point underflow * Added a Saver class to save the state of the KalmanFilter in arrays. Makes it easy to save the intermediate values while performing filtering. * Fixed bug where likelihood() returned None * Altered RTS_smoother algorithm to also return the predicted covariances Version 1.0.0 ============= * Mark as first stable release * Fix #42: For conda-build to work, correct setup.py and MANIFEST.in path to LICENSE file, ensuring they are distributed to pypi. Version 0.1.5 ============= * Fix #53: UKF rts_smoother does not use residual functions * Fixed #54: Comments in multivariate_multiply incorrectly called the covariance the mean. * Added logpdf to stats. Computes logpdf - mostly a wrapper around stats.multivariate_normal.logpdf as older versions of that function do not support the allow_singular keyword. But it also flattens out the vectors for you so you do not have to do anything special with column vectors. Version 0.1.4 ============= * Added Cubature Kalman filter. * Bug in Q_continuous_white_noise(). The first term in the matrix should be (dt**3)/3, not (dt**4)/3. * Added log-likelihood computation to UKF. * Added simplex points for UKF * fixed bug in KF matrix size check Version 0.1.3 ============= * Github issue #37. rts_smoother uses the wrong index for F and Q: they should use k+1, not k. This caused poor smoothing performance when either F or Q are time varying. * Github issue #40. Fixed behavior of multivariate_gaussian to accept list as the covariance matrix. Version 0.1.2 ============= Modified the update() and predict() functions to work in the univariate case. You can pass int/floats into the equations and get floats back. Version 0.1.1 ============= * Added discrete_bayes module, which supports discrete Bayesian filtering. * Brought docstrings (mostly) into compliance with NumPy documentation style. This requires installation of numpy doc with pip install numpydoc docs\conf.py has been modified to use numpydoc. Version 0.1.0 ============== Move to minor version numbering doesn't mean anything other than it got absurd to be using 3 digits for version numbers. We are far past alpha here. I will be moving to 1.0.0 soon, probably after I finish the book and flesh out a few points. * Implemented a fixed-point smoother, but it is not working all that well. Color on this: There are various recusive equations for the fixed point filter that I have found in various book - Simon, Crassidis, and Grewal. None seem to work very well. I have code that works pretty good when R is < 0.5 or so, but then the filter diverges when R is larger. I'm not seeing much in the literature that explains this very well, nor any evidence of this smoother actually being used in practice. I will give this a bit more effort, and if I can't get something reliable I'll put it in a branch and remove from trunk. Someone will have to tackle this on a rainy day. * KalmanFilter.batch_filter() now accepts lists of all the KF matrices * lots of docstring corrections and additions Version 0.0.28 ============== * Deprecated plot_gaussian in favor of plot_gaussian_pdf, which is a more descriptive name. * Added plot_gaussian_cdf and plot_discrete_cdf. Version 0.0.27 ============== * Added function to compute update in the presense of correlated process and measurement noise. * Added IMM filter. * added tests for IMM and MMAE filters * Added display of semi-axis for covariance ellipses * various bug fixes Version 0.0.26 ============== * Added likelihood and log-likelihood to the KalmanFilter class. * Added an MMAE filter bank class. * Added function to compute NEES Version 0.0.25 ============== Installation still messed up, this is a revert to 0.0.23 minus the folder changes. I hope. Version 0.0.24 ============== I messed up the installation on 0.0.23 on pypi, it had no source files in it. Pypi no longer allows you to refresh distribution files, so I had to make a new version number. Only changes are to make the install work - I had to move some of the install files around. This should affect no one but me. Version 0.0.23 ============== * Restructured directories so source code is under filterpy/, not filterpy/filterpy. If you have PYTHONPATH set to point to some_dir/filterpy you will need to change it to some_dir. Shouldn't affect you if you do a normal pip install. Let me know. * Allow KalmanFilter.B to be set to a scalar. * let plot_covariance_ellipse use fc and ec for facecolor and edgecolor. Just to make code shorter in book! Version 0.0.22 ============== BREAKING CHANGE Split statistical functions in filterpy.common into filterpy.stats module. I did not add or change anything, just move functions. If you get an import error, this is probably why! Switch import from filterpy.common to filterpy.stats and everything should work. Version 0.0.21 ============== Added monte_carlo module which contains routines for MCMC - mostly for particle filtering. Version 0.0.20 ============== Several important bug fixes and additions for the UKF filter. It is very important to update your code to this release if you are using the UKF. * You couldn't call update() more than once in a row or the covariance matrix would be computed incorrectly,. * Added way to specify subtract routine in the sigma point classes. * Fixed bug in computation of weights for the Julier sigma points. Version 0.0.19 =========== BREAKING CHANGES!! The unscented kalman filter code has been significantly altered. Your existing code will no longer run. Sorry, but it had to be done. As of version 0.0.18 there were separate classes for the UKF (Julier's) original formulation, and for the scaled UKF. But they are all the same thing, basically, and there were differing levels of support - the scaled version didn't have an RTS smoother, for example. Now the sigma point and weight generation is done with a separate class, and the UKF class just performs the algorithm. This is much more configurable at perhaps the cost of being a bit harder to read and learn. But I didn't want to keep writing batch_filter, rts_smoother, etc, for every possible sigma point filter. The best documentation on this is the chapter on the UKF in my Kalman filter book: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/09_Unscented_Kalman_Filter.ipynb Version 0.0.18 ============== * Added args parameters to Hx and HJacobian of the ExtendedKalmanFilter class so you can pass additional data to them. * Made an exception more human readable by including the size of the matrix that caused the shape error. Version 0.0.17 ============== * Fixed assert in UKF module that incorrectly required kappa to be >= 0. Version 0.0.16 ============== * Added multivariate_multiply to stats module. * IMPORTANT: bug fix in the UKF RTS smoother routine. * various typo fixes. Version 0.0.15 ============== A bunch of small changes and bug fixes. Documentation improvements. Version 0.0.14 ============== The change to _dt was stupid in 0.0.13 . I put it back to _dt, and then added an optional dt parameter to the predict() function. Version 0.0.13 ============== * BREAKING CHANGE: _dt in UKF is now named dt to allow users to rename. You will get an exception if you try to use _dt for now. * fixed bug in EKF. Version 0.0.12 ============== * Mostly a change in the pypi install so that the pip install will include the test directories, and include the changelog and license. * a few small bug fixes. Version 0.0.11 ============== * Breaking change - moved rts_smoother into the KalmanFilter class. * added an rts_smoother method to the UnscentedKalmanFilter class Version 0.0.10 ============== * Modified all filters to allow a 1D array for the state vector x. That is, np.array([1,0]) is allowed, as well as np.array([[1],[0]]) This is a potentially breaking change to your scripts. I tried to test all of the possibilities, but bug may remain. * Added some tests for dimensionality of input to functions. It is far from complete, as I don't want to go overboard running tests for every function call. On the other hand, failures are obsucre. This will be finalized in few releases. Version 0.0.9 ============= * Added Ensemble Kalman filter * bug fixes in UKF Version 0.0.8 ============= Minor changes to Unscented filter, mainly naming of local variables. Version 0.0.7 ============= Significant changes to Unscented filter. Now separate classes for the different sigma computations, and predict/update split out. Provision for supplying your own residual and unscented transform functions. Version 0.0.6 ============= Version 0.0.5 ============= * Fixed and included the fixed lag smoother algorithm. * name change - all Z and Zs to z and zs. They are vectors, not matrices. * Optional H parameter in KalmanFilter.update() to override the H matrix. Useful if you have a variable number of measurements on each update. Version 0.0.4 ============= * Tests and fixes for the ExtendedKalmanFilter * Minor name changes for the methods that compute Q in common Version 0.0.3 ============= * Reverted the name change of .x to .X in the various classes. I have no idea what I was thinking - x is a vector, so it should be lower case. * Moved some code to a new /examples directory to reduce clutter. It is worth noting that the code in there does not run now - it is based on the old procedural unscented KF code, not the new OO based code. However, the test_UKF.py code basically implements this example as a test using the new code. This is more a change for the future. filterpy-1.4.5/filterpy/common/000077500000000000000000000000001335747705400165245ustar00rootroot00000000000000filterpy-1.4.5/filterpy/common/__init__.py000066400000000000000000000010111335747705400206260ustar00rootroot00000000000000"""Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint:disable=wildcard-import from __future__ import absolute_import __all__ = ["helpers", "discretization", "kinematic"] from .helpers import * from .discretization import * from .kinematic import * filterpy-1.4.5/filterpy/common/discretization.py000066400000000000000000000215321335747705400221340ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint:disable=invalid-name, bad-whitespace from __future__ import (absolute_import, division, print_function, unicode_literals) from numpy import zeros, vstack, eye, array from numpy.linalg import inv from scipy.linalg import expm, block_diag def order_by_derivative(Q, dim, block_size): """ Given a matrix Q, ordered assuming state space [x y z x' y' z' x'' y'' z''...] return a reordered matrix assuming an ordering of [ x x' x'' y y' y'' z z' y''] This works for any covariance matrix or state transition function Parameters ---------- Q : np.array, square The matrix to reorder dim : int >= 1 number of independent state variables. 3 for x, y, z block_size : int >= 0 Size of derivatives. Second derivative would be a block size of 3 (x, x', x'') """ N = dim * block_size D = zeros((N, N)) Q = array(Q) for i, x in enumerate(Q.ravel()): f = eye(block_size) * x ix, iy = (i // dim) * block_size, (i % dim) * block_size D[ix:ix+block_size, iy:iy+block_size] = f return D def Q_discrete_white_noise(dim, dt=1., var=1., block_size=1, order_by_dim=True): """ Returns the Q matrix for the Discrete Constant White Noise Model. dim may be either 2, 3, or 4 dt is the time step, and sigma is the variance in the noise. Q is computed as the G * G^T * variance, where G is the process noise per time step. In other words, G = [[.5dt^2][dt]]^T for the constant velocity model. Parameters ----------- dim : int (2, 3, or 4) dimension for Q, where the final dimension is (dim x dim) dt : float, default=1.0 time step in whatever units your filter is using for time. i.e. the amount of time between innovations var : float, default=1.0 variance in the noise block_size : int >= 1 If your state variable contains more than one dimension, such as a 3d constant velocity model [x x' y y' z z']^T, then Q must be a block diagonal matrix. order_by_dim : bool, default=True Defines ordering of variables in the state vector. `True` orders by keeping all derivatives of each dimensions) [x x' x'' y y' y''] whereas `False` interleaves the dimensions [x y z x' y' z' x'' y'' z''] Examples -------- >>> # constant velocity model in a 3D world with a 10 Hz update rate >>> Q_discrete_white_noise(2, dt=0.1, var=1., block_size=3) array([[0.000025, 0.0005 , 0. , 0. , 0. , 0. ], [0.0005 , 0.01 , 0. , 0. , 0. , 0. ], [0. , 0. , 0.000025, 0.0005 , 0. , 0. ], [0. , 0. , 0.0005 , 0.01 , 0. , 0. ], [0. , 0. , 0. , 0. , 0.000025, 0.0005 ], [0. , 0. , 0. , 0. , 0.0005 , 0.01 ]]) References ---------- Bar-Shalom. "Estimation with Applications To Tracking and Navigation". John Wiley & Sons, 2001. Page 274. """ if not (dim == 2 or dim == 3 or dim == 4): raise ValueError("dim must be between 2 and 4") if dim == 2: Q = [[.25*dt**4, .5*dt**3], [ .5*dt**3, dt**2]] elif dim == 3: Q = [[.25*dt**4, .5*dt**3, .5*dt**2], [ .5*dt**3, dt**2, dt], [ .5*dt**2, dt, 1]] else: Q = [[(dt**6)/36, (dt**5)/12, (dt**4)/6, (dt**3)/6], [(dt**5)/12, (dt**4)/4, (dt**3)/2, (dt**2)/2], [(dt**4)/6, (dt**3)/2, dt**2, dt], [(dt**3)/6, (dt**2)/2 , dt, 1.]] if order_by_dim: return block_diag(*[Q]*block_size) * var return order_by_derivative(array(Q), dim, block_size) * var def Q_continuous_white_noise(dim, dt=1., spectral_density=1., block_size=1, order_by_dim=True): """ Returns the Q matrix for the Discretized Continuous White Noise Model. dim may be either 2, 3, 4, dt is the time step, and sigma is the variance in the noise. Parameters ---------- dim : int (2 or 3 or 4) dimension for Q, where the final dimension is (dim x dim) 2 is constant velocity, 3 is constant acceleration, 4 is constant jerk dt : float, default=1.0 time step in whatever units your filter is using for time. i.e. the amount of time between innovations spectral_density : float, default=1.0 spectral density for the continuous process block_size : int >= 1 If your state variable contains more than one dimension, such as a 3d constant velocity model [x x' y y' z z']^T, then Q must be a block diagonal matrix. order_by_dim : bool, default=True Defines ordering of variables in the state vector. `True` orders by keeping all derivatives of each dimensions) [x x' x'' y y' y''] whereas `False` interleaves the dimensions [x y z x' y' z' x'' y'' z''] Examples -------- >>> # constant velocity model in a 3D world with a 10 Hz update rate >>> Q_continuous_white_noise(2, dt=0.1, block_size=3) array([[0.00033333, 0.005 , 0. , 0. , 0. , 0. ], [0.005 , 0.1 , 0. , 0. , 0. , 0. ], [0. , 0. , 0.00033333, 0.005 , 0. , 0. ], [0. , 0. , 0.005 , 0.1 , 0. , 0. ], [0. , 0. , 0. , 0. , 0.00033333, 0.005 ], [0. , 0. , 0. , 0. , 0.005 , 0.1 ]]) """ if not (dim == 2 or dim == 3 or dim == 4): raise ValueError("dim must be between 2 and 4") if dim == 2: Q = [[(dt**3)/3., (dt**2)/2.], [(dt**2)/2., dt]] elif dim == 3: Q = [[(dt**5)/20., (dt**4)/8., (dt**3)/6.], [ (dt**4)/8., (dt**3)/3., (dt**2)/2.], [ (dt**3)/6., (dt**2)/2., dt]] else: Q = [[(dt**7)/252., (dt**6)/72., (dt**5)/30., (dt**4)/24.], [(dt**6)/72., (dt**5)/20., (dt**4)/8., (dt**3)/6.], [(dt**5)/30., (dt**4)/8., (dt**3)/3., (dt**2)/2.], [(dt**4)/24., (dt**3)/6., (dt**2/2.), dt]] if order_by_dim: return block_diag(*[Q]*block_size) * spectral_density return order_by_derivative(array(Q), dim, block_size) * spectral_density def van_loan_discretization(F, G, dt): """ Discretizes a linear differential equation which includes white noise according to the method of C. F. van Loan [1]. Given the continuous model x' = Fx + Gu where u is the unity white noise, we compute and return the sigma and Q_k that discretizes that equation. Examples -------- Given y'' + y = 2u(t), we create the continuous state model of x' = [ 0 1] * x + [0]*u(t) [-1 0] [2] and a time step of 0.1: >>> F = np.array([[0,1],[-1,0]], dtype=float) >>> G = np.array([[0.],[2.]]) >>> phi, Q = van_loan_discretization(F, G, 0.1) >>> phi array([[ 0.99500417, 0.09983342], [-0.09983342, 0.99500417]]) >>> Q array([[ 0.00133067, 0.01993342], [ 0.01993342, 0.39866933]]) (example taken from Brown[2]) References ---------- [1] C. F. van Loan. "Computing Integrals Involving the Matrix Exponential." IEEE Trans. Automomatic Control, AC-23 (3): 395-404 (June 1978) [2] Robert Grover Brown. "Introduction to Random Signals and Applied Kalman Filtering." Forth edition. John Wiley & Sons. p. 126-7. (2012) """ n = F.shape[0] A = zeros((2*n, 2*n)) # we assume u(t) is unity, and require that G incorporate the scaling term # for the noise. Hence W = 1, and GWG' reduces to GG" A[0:n, 0:n] = -F.dot(dt) A[0:n, n:2*n] = G.dot(G.T).dot(dt) A[n:2*n, n:2*n] = F.T.dot(dt) B=expm(A) sigma = B[n:2*n, n:2*n].T Q = sigma.dot(B[0:n, n:2*n]) return (sigma, Q) def linear_ode_discretation(F, L=None, Q=None, dt=1.): n = F.shape[0] if L is None: L = eye(n) if Q is None: Q = zeros((n, n)) A = expm(F * dt) phi = zeros((2*n, 2*n)) phi[0:n, 0:n] = F phi[0:n, n:2*n] = L.dot(Q).dot(L.T) phi[n:2*n, n:2*n] = -F.T zo = vstack((zeros((n, n)), eye(n))) CD = expm(phi*dt).dot(zo) C = CD[0:n, :] D = CD[n:2*n, :] q = C.dot(inv(D)) return (A, q) filterpy-1.4.5/filterpy/common/helpers.py000066400000000000000000000251561335747705400205510ustar00rootroot00000000000000# -*- coding: utf-8 -*- #pylint: disable=invalid-name """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import print_function from collections import defaultdict import copy import inspect import numpy as np class Saver(object): """ Helper class to save the states of any filter object. Each time you call save() all of the attributes (state, covariances, etc) are appended to lists. Generally you would do this once per epoch - predict/update. Then, you can access any of the states by using the [] syntax or by using the . operator. .. code-block:: Python my_saver = Saver() ... do some filtering x = my_saver['x'] x = my_save.x Either returns a list of all of the state `x` values for the entire filtering process. If you want to convert all saved lists into numpy arrays, call to_array(). Parameters ---------- kf : object any object with a __dict__ attribute, but intended to be one of the filtering classes save_current : bool, default=True save the current state of `kf` when the object is created; skip_private: bool, default=False Control skipping any private attribute (anything starting with '_') Turning this on saves memory, but slows down execution a bit. skip_callable: bool, default=False Control skipping any attribute which is a method. Turning this on saves memory, but slows down execution a bit. ignore: (str,) tuple of strings list of keys to ignore. Examples -------- .. code-block:: Python kf = KalmanFilter(...whatever) # initialize kf here saver = Saver(kf) # save data for kf filter for z in zs: kf.predict() kf.update(z) saver.save() x = np.array(s.x) # get the kf.x state in an np.array plt.plot(x[:, 0], x[:, 2]) # ... or ... s.to_array() plt.plot(s.x[:, 0], s.x[:, 2]) """ def __init__(self, kf, save_current=False, skip_private=False, skip_callable=False, ignore=()): """ Construct the save object, optionally saving the current state of the filter""" #pylint: disable=too-many-arguments self._kf = kf self._DL = defaultdict(list) self._skip_private = skip_private self._skip_callable = skip_callable self._ignore = ignore self._len = 0 # need to save all properties since it is possible that the property # is computed only on access. I use this trick a lot to minimize # computing unused information. self.properties = inspect.getmembers( type(kf), lambda o: isinstance(o, property)) if save_current: self.save() def save(self): """ save the current state of the Kalman filter""" kf = self._kf # force all attributes to be computed. this is only necessary # if the class uses properties that compute data only when # accessed for prop in self.properties: self._DL[prop[0]].append(getattr(kf, prop[0])) v = copy.deepcopy(kf.__dict__) if self._skip_private: for key in list(v.keys()): if key.startswith('_'): print('deleting', key) del v[key] if self._skip_callable: for key in list(v.keys()): if callable(v[key]): del v[key] for ig in self._ignore: if ig in v: del v[ig] for key in list(v.keys()): self._DL[key].append(v[key]) self.__dict__.update(self._DL) self._len += 1 def __getitem__(self, key): return self._DL[key] def __len__(self): return self._len @property def keys(self): """ list of all keys""" return list(self._DL.keys()) def to_array(self): """ Convert all saved attributes from a list to np.array. This may or may not work - every saved attribute must have the same shape for every instance. i.e., if `K` changes shape due to `z` changing shape then the call will raise an exception. This can also happen if the default initialization in __init__ gives the variable a different shape then it becomes after a predict/update cycle. """ for key in self.keys: try: self.__dict__[key] = np.array(self._DL[key]) except: # get back to lists so we are in a valid state self.__dict__.update(self._DL) raise ValueError( "could not convert {} into np.array".format(key)) def flatten(self): """ Flattens any np.array of column vectors into 1D arrays. Basically, this makes data readable for humans if you are just inspecting via the REPL. For example, if you have saved a KalmanFilter object with 89 epochs, self.x will be shape (89, 9, 1) (for example). After flatten is run, self.x.shape == (89, 9), which displays nicely from the REPL. There is no way to unflatten, so it's a one way trip. """ for key in self.keys: try: arr = self.__dict__[key] shape = arr.shape if shape[2] == 1: self.__dict__[key] = arr.reshape(shape[0], shape[1]) except: # not an ndarray or not a column vector pass def __repr__(self): return ''.format( hex(id(self)), ' '.join(self.keys)) def runge_kutta4(y, x, dx, f): """computes 4th order Runge-Kutta for dy/dx. Parameters ---------- y : scalar Initial/current value for y x : scalar Initial/current value for x dx : scalar difference in x (e.g. the time step) f : ufunc(y,x) Callable function (y, x) that you supply to compute dy/dx for the specified values. """ k1 = dx * f(y, x) k2 = dx * f(y + 0.5*k1, x + 0.5*dx) k3 = dx * f(y + 0.5*k2, x + 0.5*dx) k4 = dx * f(y + k3, x + dx) return y + (k1 + 2*k2 + 2*k3 + k4) / 6. def pretty_str(label, arr): """ Generates a pretty printed NumPy array with an assignment. Optionally transposes column vectors so they are drawn on one line. Strictly speaking arr can be any time convertible by `str(arr)`, but the output may not be what you want if the type of the variable is not a scalar or an ndarray. Examples -------- >>> pprint('cov', np.array([[4., .1], [.1, 5]])) cov = [[4. 0.1] [0.1 5. ]] >>> print(pretty_str('x', np.array([[1], [2], [3]]))) x = [[1 2 3]].T """ def is_col(a): """ return true if a is a column vector""" try: return a.shape[0] > 1 and a.shape[1] == 1 except (AttributeError, IndexError): return False if label is None: label = '' if label: label += ' = ' if is_col(arr): return label + str(arr.T).replace('\n', '') + '.T' rows = str(arr).split('\n') if not rows: return '' s = label + rows[0] pad = ' ' * len(label) for line in rows[1:]: s = s + '\n' + pad + line return s def pprint(label, arr, **kwargs): """ pretty prints an NumPy array using the function pretty_str. Keyword arguments are passed to the print() function. See Also -------- pretty_str Examples -------- >>> pprint('cov', np.array([[4., .1], [.1, 5]])) cov = [[4. 0.1] [0.1 5. ]] """ print(pretty_str(label, arr), **kwargs) def reshape_z(z, dim_z, ndim): """ ensure z is a (dim_z, 1) shaped vector""" z = np.atleast_2d(z) if z.shape[1] == dim_z: z = z.T if z.shape != (dim_z, 1): raise ValueError('z must be convertible to shape ({}, 1)'.format(dim_z)) if ndim == 1: z = z[:, 0] if ndim == 0: z = z[0, 0] return z def inv_diagonal(S): """ Computes the inverse of a diagonal NxN np.array S. In general this will be much faster than calling np.linalg.inv(). However, does NOT check if the off diagonal elements are non-zero. So long as S is truly diagonal, the output is identical to np.linalg.inv(). Parameters ---------- S : np.array diagonal NxN array to take inverse of Returns ------- S_inv : np.array inverse of S Examples -------- This is meant to be used as a replacement inverse function for the KalmanFilter class when you know the system covariance S is diagonal. It just makes the filter run faster, there is >>> kf = KalmanFilter(dim_x=3, dim_z=1) >>> kf.inv = inv_diagonal # S is 1x1, so safely diagonal """ S = np.asarray(S) if S.ndim != 2 or S.shape[0] != S.shape[1]: raise ValueError('S must be a square Matrix') si = np.zeros(S.shape) for i in range(len(S)): si[i, i] = 1. / S[i, i] return si def outer_product_sum(A, B=None): """ Computes the sum of the outer products of the rows in A and B P = \Sum {A[i] B[i].T} for i in 0..N Notionally: P = 0 for y in A: P += np.outer(y, y) This is a standard computation for sigma points used in the UKF, ensemble Kalman filter, etc., where A would be the residual of the sigma points and the filter's state or measurement. The computation is vectorized, so it is much faster than the for loop for large A. Parameters ---------- A : np.array, shape (M, N) rows of N-vectors to have the outer product summed B : np.array, shape (M, N) rows of N-vectors to have the outer product summed If it is `None`, it is set to A. Returns ------- P : np.array, shape(N, N) sum of the outer product of the rows of A and B Examples -------- Here sigmas is of shape (M, N), and x is of shape (N). The two sets of code compute the same thing. >>> P = outer_product_sum(sigmas - x) >>> >>> P = 0 >>> for s in sigmas: >>> y = s - x >>> P += np.outer(y, y) """ if B is None: B = A outer = np.einsum('ij,ik->ijk', A, B) return np.sum(outer, axis=0) filterpy-1.4.5/filterpy/common/kinematic.py000066400000000000000000000107731335747705400210520ustar00rootroot00000000000000# -*- coding: utf-8 -*- #pylint: disable=invalid-name """Copyright 2018 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import math import numpy as np from scipy.linalg import block_diag def kinematic_state_transition(order, dt): """ create a state transition matrix of a given order for a given time step `dt`. """ if not(order >= 0 and int(order) == order): raise ValueError("order must be an int >= 0") # hard code common cases for computational efficiency if order == 0: return np.array([[1.]]) if order == 1: return np.array([[1., dt], [0., 1.]]) if order == 2: return np.array([[1., dt, 0.5*dt*dt], [0., 1., dt], [0., 0., 1.]]) # grind it out computationally.... N = order + 1 F = np.zeros((N, N)) # compute highest order row for n in range(N): F[0, n] = float(dt**n) / math.factorial(n) # copy with a shift to get lower order rows for j in range(1, N): F[j, j:] = F[0, 0:-j] return F def kinematic_kf(dim, order, dt=1., dim_z=1, order_by_dim=True): """ Returns a KalmanFilter using newtonian kinematics of arbitrary order for any number of dimensions. For example, a constant velocity filter in 3D space would have order 1 dimension 3. Examples -------- A constant velocity filter in 3D space with delta time = .2 seconds would be created with >>> kf = kinematic_kf(dim=3, order=1, dt=.2) >>> kf.F >>> array([[1. , 0.2, 0. , 0. , 0. , 0. ], [0. , 1. , 0. , 0. , 0. , 0. ], [0. , 0. , 1. , 0.2, 0. , 0. ], [0. , 0. , 0. , 1. , 0. , 0. ], [0. , 0. , 0. , 0. , 1. , 0.2], [0. , 0. , 0. , 0. , 0. , 1. ]]) which will set the state `x` to be interpreted as [x, x', y, y', z, z'].T If you set `order_by_dim` to False, then `x` is ordered as [x y z x' y' z'].T As another example, a 2D constant jerk is created with >> kinematic_kf(2, 3) Assumes that the measurement z is position in each dimension. If this is not true you will have to alter the H matrix by hand. P, Q, R are all set to the Identity matrix. H is assigned assuming the measurement is position, one per dimension `dim`. >>> kf = kinematic_kf(2, 1, dt=3.0) >>> kf.F array([[1., 3., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 3.], [0., 0., 0., 1.]]) Parameters ---------- dim : int, >= 1 number of dimensions (2D space would be dim=2) order : int, >= 0 order of the filter. 2 would be a const acceleration model with a stat dim_z : int, default 1 size of z vector *per* dimension `dim`. Normally should be 1 dt : float, default 1.0 Time step. Used to create the state transition matrix order_by_dim : bool, default=True Defines ordering of variables in the state vector. `True` orders by keeping all derivatives of each dimensions) [x x' x'' y y' y''] whereas `False` interleaves the dimensions [x y z x' y' z' x'' y'' z''] """ from filterpy.kalman import KalmanFilter if dim < 1: raise ValueError("dim must be >= 1") if order < 0: raise ValueError("order must be >= 0") if dim_z < 1: raise ValueError("dim_z must be >= 1") dim_x = order + 1 kf = KalmanFilter(dim_x=dim * dim_x, dim_z=dim) F = kinematic_state_transition(order, dt) if order_by_dim: diag = [F] * dim kf.F = block_diag(*diag) else: kf.F.fill(0.0) for i, x in enumerate(F.ravel()): f = np.eye(dim) * x ix, iy = (i // dim_x) * dim, (i % dim_x) * dim kf.F[ix:ix+dim, iy:iy+dim] = f if order_by_dim: for i in range(dim): kf.H[i, i * dim_x] = 1. else: for i in range(dim): kf.H[i, i] = 1. return kf if __name__ == "__main__": _kf = kinematic_kf(2, 1, dt=3., order_by_dim=False) print(_kf.F) print('\n\n') _kf = kinematic_kf(3, 1, dt=3., order_by_dim=False) print(_kf.F) filterpy-1.4.5/filterpy/common/tests/000077500000000000000000000000001335747705400176665ustar00rootroot00000000000000filterpy-1.4.5/filterpy/common/tests/test_discretization.py000066400000000000000000000051461335747705400243400ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) from filterpy.common import (linear_ode_discretation, Q_discrete_white_noise, kinematic_kf) from numpy import array def near_eq(x,y): return abs(x-y) < 1.e-18 def test_kinematic(): kf = kinematic_kf(1,1) def test_Q_discrete_white_noise(): Q = Q_discrete_white_noise (2) assert Q[0,0] == .25 assert Q[1,0] == .5 assert Q[0,1] == .5 assert Q[1,1] == 1 assert Q.shape == (2,2) def test_linear_ode(): F = array([[0,0,1,0,0,0], [0,0,0,1,0,0], [0,0,0,0,1,0], [0,0,0,0,0,1], [0,0,0,0,0,0], [0,0,0,0,0,0]], dtype=float) L = array ([[0,0], [0,0], [0,0], [0,0], [1,0], [0,1]], dtype=float) q = .2 Q = array([[q, 0],[0, q]]) dt = 0.5 A,Q = linear_ode_discretation(F, L, Q, dt) val = [1, 0, dt, 0, 0.5*dt**2, 0] for i in range(6): assert val[i] == A[0,i] for i in range(6): assert val[i-1] == A[1,i] if i > 0 else A[1,i] == 0 for i in range(6): assert val[i-2] == A[2,i] if i > 1 else A[2,i] == 0 for i in range(6): assert val[i-3] == A[3,i] if i > 2 else A[3,i] == 0 for i in range(6): assert val[i-4] == A[4,i] if i > 3 else A[4,i] == 0 for i in range(6): assert val[i-5] == A[5,i] if i > 4 else A[5,i] == 0 assert near_eq(Q[0,0], (1./20)*(dt**5)*q) assert near_eq(Q[0,1], 0) assert near_eq(Q[0,2], (1/8)*(dt**4)*q) assert near_eq(Q[0,3], 0) assert near_eq(Q[0,4], (1./6)*(dt**3)*q) assert near_eq(Q[0,5], 0) if __name__ == "__main__": test_linear_ode() test_Q_discrete_white_noise() F = array([[0,0,1,0,0,0], [0,0,0,1,0,0], [0,0,0,0,1,0], [0,0,0,0,0,1], [0,0,0,0,0,0], [0,0,0,0,0,0]], dtype=float) L = array ([[0,0], [0,0], [0,0], [0,0], [1,0], [0,1]], dtype=float) q = .2 Q = array([[q, 0],[0, q]]) dt = 1/30 A,Q = linear_ode_discretation(F, L, Q, dt) print(Q)filterpy-1.4.5/filterpy/common/tests/test_helpers.py000066400000000000000000000114101335747705400227360ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from filterpy.common import kinematic_kf, Saver, inv_diagonal, outer_product_sum import numpy as np from filterpy.kalman import (MerweScaledSigmaPoints, UnscentedKalmanFilter, ExtendedKalmanFilter) def test_kinematic_filter(): global kf # make sure the default matrices are shaped correctly for dim_x in range(1,4): for order in range (0, 3): kf = kinematic_kf(dim=dim_x, order=order) kf.predict() kf.update(np.zeros((dim_x, 1))) # H is tricky, make sure it is shaped and assigned correctly kf = kinematic_kf(dim=2, order=2) assert kf.x.shape == (6, 1) assert kf.F.shape == (6, 6) assert kf.P.shape == (6, 6) assert kf.Q.shape == (6, 6) assert kf.R.shape == (2, 2) assert kf.H.shape == (2, 6) H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]], dtype=float) assert np.array_equal(H, kf.H) kf = kinematic_kf(dim=3, order=2, order_by_dim=False) H = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0]], dtype=float) assert np.array_equal(H, kf.H) def test_saver_UKF(): def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) z_std = 0.1 kf.R = np.diag([z_std**2, z_std**2]) # 1 standard kf.x = np.array([-1., 1., -1., 1]) kf.P *= 1. zs = [[i, i] for i in range(40)] s = Saver(kf, skip_private=False, skip_callable=False, ignore=['z_mean']) for z in zs: kf.predict() kf.update(z) #print(kf.x, kf.log_likelihood, kf.P.diagonal()) s.save() s.to_array() def test_saver_kf(): kf = kinematic_kf(3, 2, dt=0.1, dim_z=3) s = Saver(kf) for i in range(10): kf.predict() kf.update([i, i, i]) s.save() # this will assert if the KalmanFilter did not properly assert s.to_array() assert len(s.x) == 10 assert len(s.y) == 10 assert len(s.K) == 10 assert (len(s) == len(s.x)) # Force an exception to occur my malforming K kf = kinematic_kf(3, 2, dt=0.1, dim_z=3) kf.K = 0. s2 = Saver(kf) for i in range(10): kf.predict() kf.update([i, i, i]) s2.save() # this should raise an exception because K is malformed try: s2.to_array() assert False, "Should not have been able to convert s.K into an array" except: pass def test_saver_ekf(): def HJ(x): return np.array([[1, 1]]) def hx(x): return np.array([x[0]]) kf = ExtendedKalmanFilter(2, 1) s = Saver(kf) for i in range(3): kf.predict() kf.update([[i]], HJ, hx) s.save() # this will assert if the KalmanFilter did not properly assert s.to_array() assert len(s.x) == 3 assert len(s.y) == 3 assert len(s.K) == 3 def test_inv_diagonal(): for i in range(10000): n = np.random.randint(1, 50) if i == 0: n = 1 # test 1x1 matrix as special case S = np.diag(np.random.randn(n)) assert np.allclose(inv_diagonal(S), np.linalg.inv(S)) def test_save_properties(): global f, s class Foo(object): aa = 3 def __init__(self): self.x = 7. self.a = None @property def ll(self): self.a = Foo.aa Foo.aa += 1 return self.a f = Foo() assert f.a is None s = Saver(f) s.save() # try to trigger property writing to Foo.a assert s.a[0] == f.a assert s.ll[0] == f.a assert f.a == 3 s.save() assert s.a[1] == f.a assert s.ll[1] == f.a assert f.a == 4 def test_outer_product(): sigmas = np.random.randn(1000000, 2) x = np.random.randn(2) P1 = outer_product_sum(sigmas-x) P2 = 0 for s in sigmas: y = s - x P2 += np.outer(y, y) assert np.allclose(P1, P2) if __name__ == "__main__": #test_repeaters() '''test_save_properties() test_saver_kf() test_saver_ekf() test_inv_diagonal() ITERS = 1000000 #test_mahalanobis() test_kinematic_filter()''' filterpy-1.4.5/filterpy/discrete_bayes/000077500000000000000000000000001335747705400202215ustar00rootroot00000000000000filterpy-1.4.5/filterpy/discrete_bayes/__init__.py000066400000000000000000000010371335747705400223330ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: disable=wildcard-import from __future__ import (absolute_import, division, print_function, unicode_literals) __all__ = ["discrete_bayes"] from .discrete_bayes import * filterpy-1.4.5/filterpy/discrete_bayes/discrete_bayes.py000066400000000000000000000065341335747705400235700ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy as np from scipy.ndimage.filters import convolve from scipy.ndimage.interpolation import shift def normalize(pdf): """Normalize distribution `pdf` in-place so it sums to 1.0. Returns pdf for convienence, so you can write things like: >>> kernel = normalize(randn(7)) Parameters ---------- pdf : ndarray discrete distribution that needs to be converted to a pdf. Converted in-place, i.e., this is modified. Returns ------- pdf : ndarray The converted pdf. """ pdf /= sum(np.asarray(pdf, dtype=float)) return pdf def update(likelihood, prior): """ Computes the posterior of a discrete random variable given a discrete likelihood and prior. In a typical application the likelihood will be the likelihood of a measurement matching your current environment, and the prior comes from discrete_bayes.predict(). Parameters ---------- likelihood : ndarray, dtype=flaot array of likelihood values prior : ndarray, dtype=flaot prior pdf. Returns ------- posterior : ndarray, dtype=float Returns array representing the posterior. Examples -------- .. code-block:: Python # self driving car. Sensor returns values that can be equated to positions # on the road. A real likelihood compuation would be much more complicated # than this example. likelihood = np.ones(len(road)) likelihood[road==z] *= scale_factor prior = predict(posterior, velocity, kernel) posterior = update(likelihood, prior) """ posterior = prior * likelihood return normalize(posterior) def predict(pdf, offset, kernel, mode='wrap', cval=0.): """ Performs the discrete Bayes filter prediction step, generating the prior. `pdf` is a discrete probability distribution expressing our initial belief. `offset` is an integer specifying how much we want to move to the right (negative values means move to the left) We assume there is some noise in that offset, which we express in `kernel`. For example, if offset=3 and kernel=[.1, .7., .2], that means we think there is a 70% chance of moving right by 3, a 10% chance of moving 2 spaces, and a 20% chance of moving by 4. It returns the resulting distribution. If `mode='wrap'`, then the probability distribution is wrapped around the array. If `mode='constant'`, or any other value the pdf is shifted, with `cval` used to fill in missing elements. Examples -------- .. code-block:: Python belief = [.05, .05, .05, .05, .55, .05, .05, .05, .05, .05] prior = predict(belief, offset=2, kernel=[.1, .8, .1]) """ if mode == 'wrap': return convolve(np.roll(pdf, offset), kernel, mode='wrap') return convolve(shift(pdf, offset, cval=cval), kernel, cval=cval, mode='constant') filterpy-1.4.5/filterpy/discrete_bayes/tests/000077500000000000000000000000001335747705400213635ustar00rootroot00000000000000filterpy-1.4.5/filterpy/discrete_bayes/tests/test_discrete_bayes.py000066400000000000000000000022611335747705400257620ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from filterpy.discrete_bayes import predict, update, normalize from numpy.random import randn, randint import numpy as np def _predict(distribution, offset, kernel): """ explicit convolution with wraparound""" N = len(distribution) kN = len(kernel) width = int((kN - 1) / 2) prior = np.zeros(N) for i in range(N): for k in range (kN): index = (i + (width-k) - offset) % N prior[i] += distribution[index] * kernel[k] return prior def test_predictions(): s = 0. for k in range(3, 22, 2): # different kernel sizes for _ in range(1000): a = randn(100) kernel = normalize(randn(k)) move = randint(1, 200) s += sum(predict(a, move, kernel) - _predict(a, move, kernel)) assert s < 1.e-8, "sum of difference = {}".format(s) filterpy-1.4.5/filterpy/examples/000077500000000000000000000000001335747705400170525ustar00rootroot00000000000000filterpy-1.4.5/filterpy/examples/GetRadar.py000066400000000000000000000021361335747705400211170ustar00rootroot00000000000000 """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: disable=invalid-name from __future__ import (absolute_import, division, print_function, unicode_literals) import math from numpy.random import randn def get_radar(dt): """ Simulate radar range to object at 1K altidue and moving at 100m/s. Adds about 5% measurement noise. Returns slant range to the object. Call once for each new measurement at dt time from last call. """ if not hasattr(get_radar, "posp"): get_radar.posp = 0 vel = 100 + .5 * randn() alt = 1000 + 10 * randn() pos = get_radar.posp + vel*dt v = 0 + pos* 0.05*randn() slant_range = math.sqrt(pos**2 + alt**2) + v get_radar.posp = pos return slant_range if __name__ == "__main__": for i in range(100): print(get_radar(0.1)) filterpy-1.4.5/filterpy/examples/README.md000066400000000000000000000004071335747705400203320ustar00rootroot00000000000000These examples are vastly out of date. Do not use! For now look at the code in the test directories. Test code is written to run with py.test on the command line. And, of course, refer to my book: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python filterpy-1.4.5/filterpy/examples/RadarUKF.py000066400000000000000000000034211335747705400210230ustar00rootroot00000000000000# -*- coding: utf-8 -*- from __future__ import (absolute_import, division, print_function, unicode_literals) """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: skip-file """ This is very old code, and no longer runs due to reorgization of the UKF code""" import numpy as np import scipy.linalg as linalg import matplotlib.pyplot as plt from GetRadar import get_radar from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.common import Q_discrete_white_noise def fx(x, dt): """ state transition function for sstate [downrange, vel, altitude]""" F = np.array([[1., dt, 0.], [0., 1., 0.], [0., 0., 1.]]) return np.dot(F, x) def hx(x): """ returns slant range based on downrange distance and altitude""" return (x[0]**2 + x[2]**2)**.5 if __name__ == "__main__": dt = 0.05 radarUKF = UKF(dim_x=3, dim_z=1, dt=dt, kappa=0.) radarUKF.Q *= Q_discrete_white_noise(3, 1, .01) radarUKF.R *= 10 radarUKF.x = np.array([0., 90., 1100.]) radarUKF.P *= 100. t = np.arange(0, 20+dt, dt) n = len(t) xs = [] rs = [] for i in range(n): r = GetRadar(dt) rs.append(r) radarUKF.update(r, hx, fx) xs.append(radarUKF.x) xs = np.asarray(xs) plt.subplot(311) plt.plot(t, xs[:, 0]) plt.title('distance') plt.subplot(312) plt.plot(t, xs[:, 1]) plt.title('velocity') plt.subplot(313) plt.plot(t, xs[:, 2]) plt.title('altitude') filterpy-1.4.5/filterpy/examples/__init__.py000066400000000000000000000004301335747705400211600ustar00rootroot00000000000000# -*- coding: utf-8 -*- #pylint: disable=wildcard-import """ Contains various example, mostly very outdated now.""" from __future__ import (absolute_import, division, print_function, unicode_literals) __all__ = ["radar_sim"] from .radar_sim import * filterpy-1.4.5/filterpy/examples/bearing_only.py000066400000000000000000000037721335747705400221050ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: skip-file import math from numpy import array, asarray from numpy.random import randn import matplotlib.pyplot as plt from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import ScaledUnscentedKalmanFilter as SUKF from filterpy.common import Q_discrete_white_noise """ This is an example of the bearing only problem. You have a platform, usually a ship, that can only get the bearing to a moving target. Assuming platform is stationary, this is a very difficult problem because there are an infinite number of solutions. The literature is filled with this example, along with proposed solutions (usually, platform makes manuevers). This is very old code; it no longer runs due to changes in the UKF """ dt = 0.1 y = 20 platform_pos=(0,20) sf = SUKF(2, 1, dt, alpha=1.e-4, beta=2., kappa=1.) sf.Q = Q_discrete_white_noise(2, dt, .1) f = UKF(2, 1, dt, kappa=0.) f.Q = Q_discrete_white_noise(2, dt, .1) def fx(x,dt): """ state transition function""" # pos = pos + vel # vel = vel return array([x[0]+x[1], x[1]]) def hx(x): """ measurement function - convert position to bearing""" return math.atan2(platform_pos[1],x[0]-platform_pos[0]) xs_scaled = [] xs = [] for i in range(300): angle = hx([i+randn()*.1, 0]) + randn() sf.update(angle, hx, fx) xs_scaled.append(sf.x) f.predict(fx) f.update(angle, hx) xs.append(f.x) xs_scaled = asarray(xs_scaled) xs = asarray(xs) plt.subplot(211) plt.plot(xs_scaled[:,0],label='scaled') plt.plot(xs[:,0], label='Julier') plt.legend(loc=4) plt.subplot(212) plt.plot(xs_scaled[:,1],label='scaled') plt.plot(xs[:,1], label='Julier') plt.legend(loc=4) plt.show() filterpy-1.4.5/filterpy/examples/radar_sim.py000066400000000000000000000022721335747705400213700ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: skip-file from numpy.random import randn class RadarSim(object): """ Simulates the radar signal returns from an object flying at a constant altityude and velocity in 1D. Assumes random process noise - altitude and velocity varies a bit for each call. """ def __init__(self, dt, pos=0., vel=100., alt=1000.): self.dt = dt self.pos = pos self.vel = vel self.alt = alt def get_range(self, process_err_pct=0.05): """ Returns slant range to the object. Call once for each new measurement at dt time from last call. """ vel = self.vel + 5 * randn() alt = self.alt + 10 * randn() self.pos += vel*self.dt err = (self.pos * process_err_pct) * randn() slant_range = (self.pos**2 + alt**2)**.5 + err return slant_range filterpy-1.4.5/filterpy/gh/000077500000000000000000000000001335747705400156325ustar00rootroot00000000000000filterpy-1.4.5/filterpy/gh/__init__.py000066400000000000000000000007201335747705400177420ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: disable=wildcard-import from __future__ import absolute_import __all__ = ["gh_filter"] from .gh_filter import * filterpy-1.4.5/filterpy/gh/gh_filter.py000066400000000000000000000676761335747705400201750ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=C0103, R0913, R0902, C0326, R0903, W1401, too-many-lines # disable snake_case warning, too many arguments, too many attributes, # one space before assignment, too few public methods, anomalous backslash # in string """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy as np from numpy import dot from filterpy.common import pretty_str class GHFilterOrder(object): """ A g-h filter of aspecified order 0, 1, or 2. Strictly speaking, the g-h filter is order 1, and the 2nd order filter is called the g-h-k filter. I'm not aware of any filter name that encompasses orders 0, 1, and 2 under one name, or I would use it. Parameters ---------- x0 : 1D np.array or scalar Initial value for the filter state. Each value can be a scalar or a np.array. You can use a scalar for x0. If order > 0, then 0.0 is assumed for the higher order terms. x[0] is the value being tracked x[1] is the first derivative (for order 1 and 2 filters) x[2] is the second derivative (for order 2 filters) dt : scalar timestep order : int order of the filter. Defines the order of the system 0 - assumes system of form x = a_0 + a_1*t 1 - assumes system of form x = a_0 +a_1*t + a_2*t^2 2 - assumes system of form x = a_0 +a_1*t + a_2*t^2 + a_3*t^3 g : float filter g gain parameter. h : float, optional filter h gain parameter, order 1 and 2 only k : float, optional filter k gain parameter, order 2 only Atrributes ------- x : np.array State of the filter. x[0] is the value being tracked x[1] is the derivative of x[0] (order 1 and 2 only) x[2] is the 2nd derivative of x[0] (order 2 only) This is always an np.array, even for order 0 where you can initialize x0 with a scalar. y : np.array Residual - difference between the measurement and the prediction dt : scalar timestep order : int order of the filter. Defines the order of the system 0 - assumes system of form x = a_0 + a_1*t 1 - assumes system of form x = a_0 +a_1*t + a_2*t^2 2 - assumes system of form x = a_0 +a_1*t + a_2*t^2 + a_3*t^3 g : float filter g gain parameter. h : float filter h gain parameter, order 1 and 2 only k : float filter k gain parameter, order 2 only z : 1D np.array or scalar measurement passed into update() """ def __init__(self, x0, dt, order, g, h=None, k=None): """ Creates a g-h filter of order 0, 1, or 2. """ if order < 0 or order > 2: raise ValueError('order must be between 0 and 2') if np.isscalar(x0): self.x = np.zeros(order+1) self.x[0] = x0 else: self.x = np.copy(x0.astype(float)) self.dt = dt self.order = order self.g = g self.h = h self.k = k self.y = np.zeros(len(self.x)) # residual self.z = np.zeros(len(self.x)) # last measurement def update(self, z, g=None, h=None, k=None): """ Update the filter with measurement z. z must be the same type or treatable as the same type as self.x[0]. """ if self.order == 0: if g is None: g = self.g self.y = z - self.x[0] self.x += dot(g, self.y) elif self.order == 1: if g is None: g = self.g if h is None: h = self.h x = self.x[0] dx = self.x[1] dxdt = dot(dx, self.dt) self.y = z - (x + dxdt) self.x[0] = x + dxdt + g*self.y self.x[1] = dx + h*self.y / self.dt self.z = z else: # order == 2 if g is None: g = self.g if h is None: h = self.h if k is None: k = self.k x = self.x[0] dx = self.x[1] ddx = self.x[2] dxdt = dot(dx, self.dt) T2 = self.dt**2. self.y = z -(x + dxdt +0.5*ddx*T2) self.x[0] = x + dxdt + 0.5*ddx*T2 + g*self.y self.x[1] = dx + ddx*self.dt + h*self.y / self.dt self.x[2] = ddx + 2*k*self.y / (self.dt**2) def __repr__(self): return '\n'.join([ 'GHFilterOrder object', pretty_str('dt', self.dt), pretty_str('order', self.order), pretty_str('x', self.x), pretty_str('g', self.g), pretty_str('h', self.h), pretty_str('k', self.k), pretty_str('y', self.y), pretty_str('z', self.z) ]) class GHFilter(object): """ Implements the g-h filter. The topic is too large to cover in this comment. See my book "Kalman and Bayesian Filters in Python" [1] or Eli Brookner's "Tracking and Kalman Filters Made Easy" [2]. A few basic examples are below, and the tests in ./gh_tests.py may give you more ideas on use. Parameters ---------- x : 1D np.array or scalar Initial value for the filter state. Each value can be a scalar or a np.array. You can use a scalar for x0. If order > 0, then 0.0 is assumed for the higher order terms. x[0] is the value being tracked x[1] is the first derivative (for order 1 and 2 filters) x[2] is the second derivative (for order 2 filters) dx : 1D np.array or scalar Initial value for the derivative of the filter state. dt : scalar time step g : float filter g gain parameter. h : float filter h gain parameter. Attributes ---------- x : 1D np.array or scalar filter state dx : 1D np.array or scalar derivative of the filter state. x_prediction : 1D np.array or scalar predicted filter state dx_prediction : 1D np.array or scalar predicted derivative of the filter state. dt : scalar time step g : float filter g gain parameter. h : float filter h gain parameter. y : np.array, or scalar residual (difference between measurement and prior) z : np.array, or scalar measurement passed into update() Examples -------- Create a basic filter for a scalar value with g=.8, h=.2. Initialize to 0, with a derivative(velocity) of 0. >>> from filterpy.gh import GHFilter >>> f = GHFilter (x=0., dx=0., dt=1., g=.8, h=.2) Incorporate the measurement of 1 >>> f.update(z=1) (0.8, 0.2) Incorporate a measurement of 2 with g=1 and h=0.01 >>> f.update(z=2, g=1, h=0.01) (2.0, 0.21000000000000002) Create a filter with two independent variables. >>> from numpy import array >>> f = GHFilter (x=array([0,1]), dx=array([0,0]), dt=1, g=.8, h=.02) and update with the measurements (2,4) >>> f.update(array([2,4]) (array([ 1.6, 3.4]), array([ 0.04, 0.06])) References ---------- [1] Labbe, "Kalman and Bayesian Filters in Python" http://rlabbe.github.io/Kalman-and-Bayesian-Filters-in-Python [2] Brookner, "Tracking and Kalman Filters Made Easy". John Wiley and Sons, 1998. """ def __init__(self, x, dx, dt, g, h): self.x = x self.dx = dx self.dt = dt self.g = g self.h = h self.dx_prediction = self.dx self.x_prediction = self.x if np.ndim(x) == 0: self.y = 0. # residual self.z = 0. else: self.y = np.zeros(len(x)) self.z = np.zeros(len(x)) def update(self, z, g=None, h=None): """ performs the g-h filter predict and update step on the measurement z. Modifies the member variables listed below, and returns the state of x and dx as a tuple as a convienence. **Modified Members** x filtered state variable dx derivative (velocity) of x residual difference between the measurement and the prediction for x x_prediction predicted value of x before incorporating the measurement z. dx_prediction predicted value of the derivative of x before incorporating the measurement z. Parameters ---------- z : any the measurement g : scalar (optional) Override the fixed self.g value for this update h : scalar (optional) Override the fixed self.h value for this update Returns ------- x filter output for x dx filter output for dx (derivative of x """ if g is None: g = self.g if h is None: h = self.h #prediction step self.dx_prediction = self.dx self.x_prediction = self.x + (self.dx*self.dt) # update step self.y = z - self.x_prediction self.dx = self.dx_prediction + h * self.y / self.dt self.x = self.x_prediction + g * self.y return (self.x, self.dx) def batch_filter(self, data, save_predictions=False, saver=None): """ Given a sequenced list of data, performs g-h filter with a fixed g and h. See update() if you need to vary g and/or h. Uses self.x and self.dx to initialize the filter, but DOES NOT alter self.x and self.dx during execution, allowing you to use this class multiple times without reseting self.x and self.dx. I'm not sure how often you would need to do that, but the capability is there. More exactly, none of the class member variables are modified by this function, in distinct contrast to update(), which changes most of them. Parameters ---------- data : list like contains the data to be filtered. save_predictions : boolean the predictions will be saved and returned if this is true saver : filterpy.common.Saver, optional filterpy.common.Saver object. If provided, saver.save() will be called after every epoch Returns ------- results : np.array shape (n+1, 2), where n=len(data) contains the results of the filter, where results[i,0] is x , and results[i,1] is dx (derivative of x) First entry is the initial values of x and dx as set by __init__. predictions : np.array shape(n), optional the predictions for each step in the filter. Only retured if save_predictions == True """ x = self.x dx = self.dx n = len(data) results = np.zeros((n+1, 2)) results[0, 0] = x results[0, 1] = dx if save_predictions: predictions = np.zeros(n) # optimization to avoid n computations of h / dt h_dt = self.h / self.dt for i, z in enumerate(data): #prediction step x_est = x + (dx * self.dt) # update step residual = z - x_est dx = dx + h_dt * residual # i.e. dx = dx + h * residual / dt x = x_est + self.g * residual results[i+1, 0] = x results[i+1, 1] = dx if save_predictions: predictions[i] = x_est if saver is not None: saver.save() if save_predictions: return results, predictions return results def VRF_prediction(self): """ Returns the Variance Reduction Factor of the prediction step of the filter. The VRF is the normalized variance for the filter, as given in the equation below. .. math:: VRF(\hat{x}_{n+1,n}) = \\frac{VAR(\hat{x}_{n+1,n})}{\sigma^2_x} References ---------- Asquith, "Weight Selection in First Order Linear Filters" Report No RG-TR-69-12, U.S. Army Missle Command. Redstone Arsenal, Al. November 24, 1970. """ g = self.g h = self.h return (2*g**2 + 2*h + g*h) / (g*(4 - 2*g - h)) def VRF(self): """ Returns the Variance Reduction Factor (VRF) of the state variable of the filter (x) and its derivatives (dx, ddx). The VRF is the normalized variance for the filter, as given in the equations below. .. math:: VRF(\hat{x}_{n,n}) = \\frac{VAR(\hat{x}_{n,n})}{\sigma^2_x} VRF(\hat{\dot{x}}_{n,n}) = \\frac{VAR(\hat{\dot{x}}_{n,n})}{\sigma^2_x} VRF(\hat{\ddot{x}}_{n,n}) = \\frac{VAR(\hat{\ddot{x}}_{n,n})}{\sigma^2_x} Returns ------- vrf_x VRF of x state variable vrf_dx VRF of the dx state variable (derivative of x) """ g = self.g h = self.h den = g*(4 - 2*g - h) vx = (2*g**2 + 2*h - 3*g*h) / den vdx = 2*h**2 / (self.dt**2 * den) return (vx, vdx) def __repr__(self): return '\n'.join([ 'GHFilter object', pretty_str('dt', self.dt), pretty_str('g', self.g), pretty_str('h', self.h), pretty_str('x', self.x), pretty_str('dx', self.dx), pretty_str('x_prediction', self.x_prediction), pretty_str('dx_prediction', self.dx_prediction), pretty_str('y', self.y), pretty_str('z', self.z) ]) class GHKFilter(object): """ Implements the g-h-k filter. Parameters ---------- x : 1D np.array or scalar Initial value for the filter state. Each value can be a scalar or a np.array. You can use a scalar for x0. If order > 0, then 0.0 is assumed for the higher order terms. x[0] is the value being tracked x[1] is the first derivative (for order 1 and 2 filters) x[2] is the second derivative (for order 2 filters) dx : 1D np.array or scalar Initial value for the derivative of the filter state. ddx : 1D np.array or scalar Initial value for the second derivative of the filter state. dt : scalar time step g : float filter g gain parameter. h : float filter h gain parameter. k : float filter k gain parameter. Attributes ---------- x : 1D np.array or scalar filter state dx : 1D np.array or scalar derivative of the filter state. ddx : 1D np.array or scalar second derivative of the filter state. x_prediction : 1D np.array or scalar predicted filter state dx_prediction : 1D np.array or scalar predicted derivative of the filter state. ddx_prediction : 1D np.array or scalar second predicted derivative of the filter state. dt : scalar time step g : float filter g gain parameter. h : float filter h gain parameter. k : float filter k gain parameter. y : np.array, or scalar residual (difference between measurement and prior) z : np.array, or scalar measurement passed into update() References ---------- Brookner, "Tracking and Kalman Filters Made Easy". John Wiley and Sons, 1998. """ def __init__(self, x, dx, ddx, dt, g, h, k): self.x = x self.dx = dx self.ddx = ddx self.x_prediction = self.x self.dx_prediction = self.dx self.ddx_prediction = self.ddx self.dt = dt self.g = g self.h = h self.k = k if np.ndim(x) == 0: self.y = 0. # residual self.z = 0. else: self.y = np.zeros(len(x)) self.z = np.zeros(len(x)) def update(self, z, g=None, h=None, k=None): """ Performs the g-h filter predict and update step on the measurement z. On return, self.x, self.dx, self.y, and self.x_prediction will have been updated with the results of the computation. For convienence, self.x and self.dx are returned in a tuple. Parameters ---------- z : scalar the measurement g : scalar (optional) Override the fixed self.g value for this update h : scalar (optional) Override the fixed self.h value for this update k : scalar (optional) Override the fixed self.k value for this update Returns ------- x filter output for x dx filter output for dx (derivative of x """ if g is None: g = self.g if h is None: h = self.h if k is None: k = self.k dt = self.dt dt_sqr = dt**2 #prediction step self.ddx_prediction = self.ddx self.dx_prediction = self.dx + self.ddx*dt self.x_prediction = self.x + self.dx*dt + .5*self.ddx*(dt_sqr) # update step self.y = z - self.x_prediction self.ddx = self.ddx_prediction + 2*k*self.y / dt_sqr self.dx = self.dx_prediction + h * self.y / dt self.x = self.x_prediction + g * self.y return (self.x, self.dx) def batch_filter(self, data, save_predictions=False): """ Performs g-h filter with a fixed g and h. Uses self.x and self.dx to initialize the filter, but DOES NOT alter self.x and self.dx during execution, allowing you to use this class multiple times without reseting self.x and self.dx. I'm not sure how often you would need to do that, but the capability is there. More exactly, none of the class member variables are modified by this function. Parameters ---------- data : list_like contains the data to be filtered. save_predictions : boolean The predictions will be saved and returned if this is true Returns ------- results : np.array shape (n+1, 2), where n=len(data) contains the results of the filter, where results[i,0] is x , and results[i,1] is dx (derivative of x) First entry is the initial values of x and dx as set by __init__. predictions : np.array shape(n), or None the predictions for each step in the filter. Only returned if save_predictions == True """ x = self.x dx = self.dx n = len(data) results = np.zeros((n+1, 2)) results[0, 0] = x results[0, 1] = dx if save_predictions: predictions = np.zeros(n) # optimization to avoid n computations of h / dt h_dt = self.h / self.dt for i, z in enumerate(data): #prediction step x_est = x + (dx*self.dt) # update step residual = z - x_est dx = dx + h_dt * residual # i.e. dx = dx + h * residual / dt x = x_est + self.g * residual results[i+1, 0] = x results[i+1, 1] = dx if save_predictions: predictions[i] = x_est if save_predictions: return results, predictions return results def VRF_prediction(self): """ Returns the Variance Reduction Factor for x of the prediction step of the filter. This implements the equation .. math:: VRF(\hat{x}_{n+1,n}) = \\frac{VAR(\hat{x}_{n+1,n})}{\sigma^2_x} References ---------- Asquith and Woods, "Total Error Minimization in First and Second Order Prediction Filters" Report No RE-TR-70-17, U.S. Army Missle Command. Redstone Arsenal, Al. November 24, 1970. """ g = self.g h = self.h k = self.k gh2 = 2*g + h return ((g*k*(gh2-4)+ h*(g*gh2+2*h)) / (2*k - (g*(h+k)*(gh2-4)))) def bias_error(self, dddx): """ Returns the bias error given the specified constant jerk(dddx) Parameters ---------- dddx : type(self.x) 3rd derivative (jerk) of the state variable x. References ---------- Asquith and Woods, "Total Error Minimization in First and Second Order Prediction Filters" Report No RE-TR-70-17, U.S. Army Missle Command. Redstone Arsenal, Al. November 24, 1970. """ return -self.dt**3 * dddx / (2*self.k) def VRF(self): """ Returns the Variance Reduction Factor (VRF) of the state variable of the filter (x) and its derivatives (dx, ddx). The VRF is the normalized variance for the filter, as given in the equations below. .. math:: VRF(\hat{x}_{n,n}) = \\frac{VAR(\hat{x}_{n,n})}{\sigma^2_x} VRF(\hat{\dot{x}}_{n,n}) = \\frac{VAR(\hat{\dot{x}}_{n,n})}{\sigma^2_x} VRF(\hat{\ddot{x}}_{n,n}) = \\frac{VAR(\hat{\ddot{x}}_{n,n})}{\sigma^2_x} Returns ------- vrf_x : type(x) VRF of x state variable vrf_dx : type(x) VRF of the dx state variable (derivative of x) vrf_ddx : type(x) VRF of the ddx state variable (second derivative of x) """ g = self.g h = self.h k = self.k # common subexpressions in the equations pulled out for efficiency, # they don't 'mean' anything. hg4 = 4- 2*g - h ghk = g*h + g*k - 2*k vx = (2*h*(2*(g**2) + 2*h - 3*g*h) - 2*g*k*hg4) / (2*k - g*(h+k) * hg4) vdx = (2*(h**3) - 4*(h**2)*k + 4*(k**2)*(2-g)) / (2*hg4*ghk) vddx = 8*h*(k**2) / ((self.dt**4)*hg4*ghk) return (vx, vdx, vddx) def __repr__(self): return '\n'.join([ 'GHFilter object', pretty_str('dt', self.dt), pretty_str('g', self.g), pretty_str('h', self.h), pretty_str('k', self.k), pretty_str('x', self.x), pretty_str('dx', self.dx), pretty_str('ddx', self.ddx), pretty_str('x_prediction', self.x_prediction), pretty_str('dx_prediction', self.dx_prediction), pretty_str('ddx_prediction', self.dx_prediction), pretty_str('y', self.y), pretty_str('z', self.z) ]) def optimal_noise_smoothing(g): """ provides g,h,k parameters for optimal smoothing of noise for a given value of g. This is due to Polge and Bhagavan[1]. Parameters ---------- g : float value for g for which we will optimize for Returns ------- (g,h,k) : (float, float, float) values for g,h,k that provide optimal smoothing of noise Examples -------- .. code-block:: Python from filterpy.gh import GHKFilter, optimal_noise_smoothing g,h,k = optimal_noise_smoothing(g) f = GHKFilter(0,0,0,1,g,h,k) f.update(1.) References ---------- [1] Polge and Bhagavan. "A Study of the g-h-k Tracking Filter". Report No. RE-CR-76-1. University of Alabama in Huntsville. July, 1975 """ h = ((2*g**3 - 4*g**2) + (4*g**6 -64*g**5 + 64*g**4)**.5) / (8*(1-g)) k = (h*(2-g) - g**2) / g return (g, h, k) def least_squares_parameters(n): """ An order 1 least squared filter can be computed by a g-h filter by varying g and h over time according to the formulas below, where the first measurement is at n=0, the second is at n=1, and so on: .. math:: h_n = \\frac{6}{(n+2)(n+1)} g_n = \\frac{2(2n+1)}{(n+2)(n+1)} Parameters ---------- n : int the nth measurement, starting at 0 (i.e. first measurement has n==0) Returns ------- (g,h) : (float, float) g and h parameters for this time step for the least-squares filter Examples -------- .. code-block:: Python from filterpy.gh import GHFilter, least_squares_parameters lsf = GHFilter (0, 0, 1, 0, 0) z = 10 for i in range(10): g,h = least_squares_parameters(i) lsf.update(z, g, h) """ den = (n+2)*(n+1) g = (2*(2*n + 1)) / den h = 6 / den return (g, h) def critical_damping_parameters(theta, order=2): """ Computes values for g and h (and k for g-h-k filter) for a critically damped filter. The idea here is to create a filter that reduces the influence of old data as new data comes in. This allows the filter to track a moving target better. This goes by different names. It may be called the discounted least-squares g-h filter, a fading-memory polynomal filter of order 1, or a critically damped g-h filter. In a normal least-squares filter we compute the error for each point as .. math:: \epsilon_t = (z-\\hat{x})^2 For a crically damped filter we reduce the influence of each error by .. math:: \\theta^{t-i} where .. math:: 0 <= \\theta <= 1 In other words the last error is scaled by theta, the next to last by theta squared, the next by theta cubed, and so on. Parameters ---------- theta : float, 0 <= theta <= 1 scaling factor for previous terms order : int, 2 (default) or 3 order of filter to create the parameters for. g and h will be calculated for the order 2, and g, h, and k for order 3. Returns ------- g : scalar optimal value for g in the g-h or g-h-k filter h : scalar optimal value for h in the g-h or g-h-k filter k : scalar optimal value for g in the g-h-k filter Examples -------- .. code-block:: Python from filterpy.gh import GHFilter, critical_damping_parameters g,h = critical_damping_parameters(0.3) critical_filter = GHFilter(0, 0, 1, g, h) References ---------- Brookner, "Tracking and Kalman Filters Made Easy". John Wiley and Sons, 1998. Polge and Bhagavan. "A Study of the g-h-k Tracking Filter". Report No. RE-CR-76-1. University of Alabama in Huntsville. July, 1975 """ if theta < 0 or theta > 1: raise ValueError('theta must be between 0 and 1') if order == 2: return (1. - theta**2, (1. - theta)**2) if order == 3: return (1. - theta**3, 1.5*(1.-theta**2)*(1.-theta), .5*(1 - theta)**3) raise ValueError('bad order specified: {}'.format(order)) def benedict_bornder_constants(g, critical=False): """ Computes the g,h constants for a Benedict-Bordner filter, which minimizes transient errors for a g-h filter. Returns the values g,h for a specified g. Strictly speaking, only h is computed, g is returned unchanged. The default formula for the Benedict-Bordner allows ringing. We can "nearly" critically damp it; ringing will be reduced, but not entirely eliminated at the cost of reduced performance. Parameters ---------- g : float scaling factor g for the filter critical : boolean, default False Attempts to critically damp the filter. Returns ------- g : float scaling factor g (same as the g that was passed in) h : float scaling factor h that minimizes the transient errors Examples -------- .. code-block:: Python from filterpy.gh import GHFilter, benedict_bornder_constants g, h = benedict_bornder_constants(.855) f = GHFilter(0, 0, 1, g, h) References ---------- Brookner, "Tracking and Kalman Filters Made Easy". John Wiley and Sons, 1998. """ g_sqr = g**2 if critical: return (g, 0.8 * (2. - g_sqr - 2*(1-g_sqr)**.5) / g_sqr) return (g, g_sqr / (2.-g)) filterpy-1.4.5/filterpy/gh/tests/000077500000000000000000000000001335747705400167745ustar00rootroot00000000000000filterpy-1.4.5/filterpy/gh/tests/test_gh.py000066400000000000000000000071371335747705400210130ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) from filterpy.common import Saver from filterpy.gh import (GHFilter, GHKFilter, least_squares_parameters, optimal_noise_smoothing, GHFilterOrder) from numpy import array from numpy.random import randn import matplotlib.pyplot as plt def test_least_squares(): """ there is an alternative form for computing h for the least squares. It works for all but the very first term (n=0). Use it to partially test the output of least_squares_parameters(). This test does not test that g is correct""" for n in range (1, 100): g,h = least_squares_parameters(n) h2 = 4 - 2*g - (4*(g-2)**2 - 3*g**2)**.5 assert abs(h2-h) < 1.e-12 def test_1d_array(): f1 = GHFilter (0, 0, 1, .8, .2) f2 = GHFilter (array([0]), array([0]), 1, .8, .2) str(f1) str(f2) # test both give same answers, and that we can # use a scalar for the measurment for i in range(1,10): f1.update(i) f2.update(i) assert f1.x == f2.x[0] assert f1.dx == f2.dx[0] assert f1.VRF() == f2.VRF() # test using an array for the measurement s1 = Saver(f1) s2 = Saver(f2) for i in range(1,10): f1.update(i) f2.update(array([i])) s1.save() s2.save() assert f1.x == f2.x[0] assert f1.dx == f2.dx[0] assert f1.VRF() == f2.VRF() s1.to_array() s2.to_array() def test_2d_array(): """ test using 2 independent variables for the state variable. """ f = GHFilter(array([0,1]), array([0,0]), 1, .8, .2) f0 = GHFilter(0, 0, 1, .8, .2) f1 = GHFilter(1, 0, 1, .8, .2) # test using scalar in update (not normal, but possible) for i in range (1,10): f.update (i) f0.update(i) f1.update(i) assert f.x[0] == f0.x assert f.x[1] == f1.x assert f.dx[0] == f0.dx assert f.dx[1] == f1.dx # test using array for update (typical scenario) f = GHFilter(array([0,1]), array([0,0]), 1, .8, .2) f0 = GHFilter(0, 0, 1, .8, .2) f1 = GHFilter(1, 0, 1, .8, .2) for i in range (1,10): f.update (array([i, i+3])) f0.update(i) f1.update(i+3) assert f.x[0] == f0.x assert f.x[1] == f1.x assert f.dx[0] == f0.dx assert f.dx[1] == f1.dx assert f.VRF() == f0.VRF() assert f.VRF() == f1.VRF() def optimal_test(): def fx(x): return .1*x**2 + 3*x -4 g,h,k = optimal_noise_smoothing(.2) f = GHKFilter(-4,0,0,1,g,h,k) ys = [] zs = [] for i in range(100): z = fx(i) + randn()*10 f.update(z) ys.append(f.x) zs.append(z) plt.plot(ys) plt.plot(zs) def test_GHFilterOrder(): def fx(x): return 2*x+1 f1 = GHFilterOrder(x0=array([0,0]), dt=1, order=1, g=.6, h=.02) f2 = GHFilter(x=0, dx=0, dt=1, g=.6, h=.02) for i in range(100): z = fx(i) + randn() f1.update(z) f2.update(z) assert abs(f1.x[0]-f2.x) < 1.e-18 if __name__ == "__main__": optimal_test() test_least_squares() test_1d_array() test_2d_array() test_GHFilterOrder() print('all passed') filterpy-1.4.5/filterpy/hinfinity/000077500000000000000000000000001335747705400172355ustar00rootroot00000000000000filterpy-1.4.5/filterpy/hinfinity/__init__.py000066400000000000000000000010441335747705400213450ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: disable=wildcard-import from __future__ import (absolute_import, division, print_function, unicode_literals) __all__ = ["hinfinity_filter"] from .hinfinity_filter import * filterpy-1.4.5/filterpy/hinfinity/hinfinity_filter.py000066400000000000000000000177051335747705400231670ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=C0103, R0913, R0902, C0326, R0914 # disable snake_case warning, too many arguments, too many attributes, # one space before assignment, too many local variables """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import absolute_import, division import copy import warnings import numpy as np from numpy import dot, zeros, eye import scipy.linalg as linalg from filterpy.common import pretty_str class HInfinityFilter(object): """ H-Infinity filter. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter. Parameters ---------- dim_x : int Number of state variables for the Kalman filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. This is used to set the default size of `P`, `Q`, and `u` dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x, y), `dim_z` would be 2. dim_u : int Number of control inputs for the Gu part of the prediction step. gamma : float .. warning:: I do not believe this code is correct. DO NOT USE THIS. In particular, note that predict does not update the covariance matrix. """ def __init__(self, dim_x, dim_z, dim_u, gamma): warnings.warn("This code is likely incorrect. DO NOT USE.", DeprecationWarning) self.dim_x = dim_x self.dim_z = dim_z self.dim_u = dim_u self.gamma = gamma self.x = zeros((dim_x, 1)) # state self.B = 0 # control transition matrix self.F = eye(dim_x) # state transition matrix self.H = zeros((dim_z, dim_x)) # Measurement function self.P = eye(dim_x) # Uncertainty covariance. self.Q = eye(dim_x) self._V_inv = zeros((dim_z, dim_z)) # inverse measurement noise self._V = zeros((dim_z, dim_z)) # measurement noise self.W = zeros((dim_x, dim_x)) # process uncertainty # gain and residual are computed during the innovation step. We # save them so that in case you want to inspect them for various # purposes self.K = 0 # H-infinity gain self.y = zeros((dim_z, 1)) self.z = zeros((dim_z, 1)) # identity matrix. Do not alter this. self._I = np.eye(dim_x) def update(self, z): """ Add a new measurement `z` to the H-Infinity filter. If `z` is None, nothing is changed. Parameters ---------- z : ndarray measurement for this update. """ if z is None: return # rename for readability and a tiny extra bit of speed I = self._I gamma = self.gamma Q = self.Q H = self.H P = self.P x = self.x V_inv = self._V_inv F = self.F W = self.W # common subexpression H.T * V^-1 HTVI = dot(H.T, V_inv) L = linalg.inv(I - gamma * dot(Q, P) + dot(HTVI, H).dot(P)) # common subexpression P*L PL = dot(P, L) K = dot(F, PL).dot(HTVI) self.y = z - dot(H, x) # x = x + Ky # predict new x with residual scaled by the H-Infinity gain self.x = self.x + dot(K, self.y) self.P = dot(F, PL).dot(F.T) + W # force P to be symmetric self.P = (self.P + self.P.T) / 2 # pylint: disable=bare-except try: self.z = np.copy(z) except: self.z = copy.deepcopy(z) def predict(self, u=0): """ Predict next position. Parameters ---------- u : ndarray Optional control vector. If non-zero, it is multiplied by `B` to create the control input into the system. """ # x = Fx + Bu self.x = dot(self.F, self.x) + dot(self.B, u) def batch_filter(self, Zs,update_first=False, saver=False): """ Batch processes a sequences of measurements. Parameters ---------- Zs : list-like list of measurements at each time step `self.dt` Missing measurements must be represented by 'None'. update_first : bool, default=False, optional, controls whether the order of operations is update followed by predict, or predict followed by update. saver : filterpy.common.Saver, optional filterpy.common.Saver object. If provided, saver.save() will be called after every epoch Returns ------- means: ndarray ((n, dim_x, 1)) array of the state for each time step. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance: ndarray((n, dim_x, dim_x)) array of the covariances for each time step. In other words `covariance[k, :, :]` is the covariance at step `k`. """ n = np.size(Zs, 0) # mean estimates from H-Infinity Filter means = zeros((n, self.dim_x, 1)) # state covariances from H-Infinity Filter covariances = zeros((n, self.dim_x, self.dim_x)) if update_first: for i, z in enumerate(Zs): self.update(z) means[i, :] = self.x covariances[i, :, :] = self.P self.predict() if saver is not None: saver.save() else: for i, z in enumerate(Zs): self.predict() self.update(z) means[i, :] = self.x covariances[i, :, :] = self.P if saver is not None: saver.save() return (means, covariances) def get_prediction(self, u=0): """ Predicts the next state of the filter and returns it. Does not alter the state of the filter. Parameters ---------- u : ndarray optional control input Returns ------- x : ndarray State vector of the prediction. """ return dot(self.F, self.x) + dot(self.B, u) def residual_of(self, z): """ returns the residual for the given measurement (z). Does not alter the state of the filter. """ return z - dot(self.H, self.x) def measurement_of_state(self, x): """ Helper function that converts a state into a measurement. Parameters ---------- x : ndarray H-Infinity state vector Returns ------- z : ndarray measurement corresponding to the given state """ return dot(self.H, x) @property def V(self): """ measurement noise matrix""" return self._V @V.setter def V(self, value): """ measurement noise matrix""" if np.isscalar(value): self._V = np.array([[value]], dtype=float) else: self._V = value self._V_inv = linalg.inv(self._V) def __repr__(self): return '\n'.join([ 'HInfinityFilter object', pretty_str('dim_x', self.dim_x), pretty_str('dim_z', self.dim_z), pretty_str('dim_u', self.dim_u), pretty_str('gamma', self.dim_u), pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('F', self.F), pretty_str('Q', self.Q), pretty_str('V', self.V), pretty_str('W', self.W), pretty_str('K', self.K), pretty_str('y', self.y), ]) filterpy-1.4.5/filterpy/hinfinity/tests/000077500000000000000000000000001335747705400203775ustar00rootroot00000000000000filterpy-1.4.5/filterpy/hinfinity/tests/test_hinfinity.py000066400000000000000000000022651335747705400240160ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2014 Roger R Labbe Jr. filterpy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) from numpy import array import matplotlib.pyplot as plt from filterpy.hinfinity import HInfinityFilter def test_Hinfinity(): dt = 0.1 f = HInfinityFilter(2, 1, 0, gamma=.4) f.F = array([[1., dt], [0., 1.]]) f.H = array([[0., 1.]]) f.x = array([[0., 0.]]).T #f.G = array([[dt**2 / 2, dt]]).T f.P = 0.01 f.W = array([[0.0003, 0.005], [0.0050, 0.100]])/ 1000 f.V = 0.01 f.Q = 0.01 xs = [] vs = [] for i in range(1,40): f.update (5) print(f.x.T) xs.append(f.x[0,0]) vs.append(f.x[1,0]) f.predict() plt.subplot(211) plt.plot(xs) plt.subplot(212) plt.plot(vs) if __name__ == "__main__": test_Hinfinity()filterpy-1.4.5/filterpy/kalman/000077500000000000000000000000001335747705400164775ustar00rootroot00000000000000filterpy-1.4.5/filterpy/kalman/.gitignore000066400000000000000000000000001335747705400204550ustar00rootroot00000000000000filterpy-1.4.5/filterpy/kalman/CubatureKalmanFilter.py000066400000000000000000000316321335747705400231220ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-arguments """Copyright 2016 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division) from copy import deepcopy from math import log, exp, sqrt import sys import numpy as np from numpy import eye, zeros, dot, isscalar, outer from scipy.linalg import inv, cholesky from filterpy.stats import logpdf from filterpy.common import pretty_str, outer_product_sum def spherical_radial_sigmas(x, P): r""" Creates cubature points for the the specified state and covariance according to [1]. Parameters ---------- x: ndarray (column vector) examples: np.array([[1.], [2.]]) P : scalar, or np.array Covariance of the filter. References ---------- .. [1] Arasaratnam, I, Haykin, S. "Cubature Kalman Filters," IEEE Transactions on Automatic Control, 2009, pp 1254-1269, vol 54, No 6 """ n, _ = P.shape x = x.flatten() sigmas = np.empty((2*n, n)) U = cholesky(P) * sqrt(n) for k in range(n): sigmas[k] = x + U[k] sigmas[n+k] = x - U[k] return sigmas def ckf_transform(Xs, Q): """ Compute mean and covariance of array of cubature points. Parameters ---------- Xs : ndarray Cubature points Q : ndarray Noise covariance Returns ------- mean : ndarray mean of the cubature points variance: ndarray covariance matrix of the cubature points """ m, n = Xs.shape x = sum(Xs, 0)[:, None] / m P = np.zeros((n, n)) xf = x.flatten() for k in range(m): P += np.outer(Xs[k], Xs[k]) - np.outer(xf, xf) P *= 1 / m P += Q return x, P class CubatureKalmanFilter(object): # pylint: disable=too-many-instance-attributes # pylint: disable=C0103 r""" Implements the Cubuture Kalman filter (UKF) as defined by Ienkaran Arasaratnam and Simon Haykin in [1] You will have to set the following attributes after constructing this object for the filter to perform properly. Parameters ---------- dim_x : int Number of state variables for the filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. dt : float Time between steps in seconds. hx : function(x) Measurement function. Converts state vector x into a measurement vector of shape (dim_z). fx : function(x, dt) function that returns the state x transformed by the state transistion function. dt is the time step in seconds. x_mean_fn : callable (sigma_points, weights), optional Function that computes the mean of the provided sigma points and weights. Use this if your state variable contains nonlinear values such as angles which cannot be summed. .. code-block:: Python def state_mean(sigmas, Wm): x = np.zeros(3) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] x[1] += s[1] * Wm[i] sum_sin += sin(s[2])*Wm[i] sum_cos += cos(s[2])*Wm[i] x[2] = atan2(sum_sin, sum_cos) return x z_mean_fn : callable (sigma_points, weights), optional Same as x_mean_fn, except it is called for sigma points which form the measurements after being passed through hx(). residual_x : callable (x, y), optional residual_z : callable (x, y), optional Function that computes the residual (difference) between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. One is for the state variable, the other is for the measurement state. .. code-block:: Python def residual(a, b): y = a[0] - b[0] if y > np.pi: y -= 2*np.pi if y < -np.pi: y = 2*np.pi return y Attributes ---------- x : numpy.array(dim_x) state estimate vector P : numpy.array(dim_x, dim_x) covariance estimate matrix x_prior : numpy.array(dim_x, 1) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_prior : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x, 1) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only R : numpy.array(dim_z, dim_z) measurement noise matrix Q : numpy.array(dim_x, dim_x) process noise matrix K : numpy.array Kalman gain. Read only. y : numpy.array innovation residual. Read only. z : ndarray Last measurement used in update(). Read only. log_likelihood : float log-likelihood of the last measurement. Read only. likelihood : float likelihood of last measurment. Read only. Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. mahalanobis : float mahalanobis distance of the innovation. Read only. References ---------- .. [1] Arasaratnam, I, Haykin, S. "Cubature Kalman Filters," IEEE Transactions on Automatic Control, 2009, pp 1254-1269, vol 54, No 6 """ def __init__(self, dim_x, dim_z, dt, hx, fx, x_mean_fn=None, z_mean_fn=None, residual_x=None, residual_z=None): self.Q = eye(dim_x) self.R = eye(dim_z) self.x = zeros(dim_x) self.P = eye(dim_x) self.K = 0 self.dim_x = dim_x self.dim_z = dim_z self._dt = dt self._num_sigmas = 2*dim_x self.hx = hx self.fx = fx self.x_mean = x_mean_fn self.z_mean = z_mean_fn self.y = 0 self.z = np.array([[None]*self.dim_z]).T self.S = np.zeros((dim_z, dim_z)) # system uncertainty self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty if residual_x is None: self.residual_x = np.subtract else: self.residual_x = residual_x if residual_z is None: self.residual_z = np.subtract else: self.residual_z = residual_z # sigma points transformed through f(x) and h(x) # variables for efficiency so we don't recreate every update self.sigmas_f = zeros((2*self.dim_x, self.dim_x)) self.sigmas_h = zeros((2*self.dim_x, self.dim_z)) # Only computed only if requested via property self._log_likelihood = log(sys.float_info.min) self._likelihood = sys.float_info.min self._mahalanobis = None # these will always be a copy of x,P after predict() is called self.x_prior = self.x.copy() self.P_prior = self.P.copy() # these will always be a copy of x,P after update() is called self.x_post = self.x.copy() self.P_post = self.P.copy() def predict(self, dt=None, fx_args=()): r""" Performs the predict step of the CKF. On return, self.x and self.P contain the predicted state (x) and covariance (P). Important: this MUST be called before update() is called for the first time. Parameters ---------- dt : double, optional If specified, the time step to be used for this prediction. self._dt is used if this is not provided. fx_args : tuple, optional, default (,) optional arguments to be passed into fx() after the required state variable. """ if dt is None: dt = self._dt if not isinstance(fx_args, tuple): fx_args = (fx_args,) sigmas = spherical_radial_sigmas(self.x, self.P) # evaluate cubature points for k in range(self._num_sigmas): self.sigmas_f[k] = self.fx(sigmas[k], dt, *fx_args) self.x, self.P = ckf_transform(self.sigmas_f, self.Q) # save prior self.x_prior = self.x.copy() self.P_prior = self.P.copy() def update(self, z, R=None, hx_args=()): """ Update the CKF with the given measurements. On return, self.x and self.P contain the new mean and covariance of the filter. Parameters ---------- z : numpy.array of shape (dim_z) measurement vector R : numpy.array((dim_z, dim_z)), optional Measurement noise. If provided, overrides self.R for this function call. hx_args : tuple, optional, default (,) arguments to be passed into Hx function after the required state variable. """ if z is None: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() return if not isinstance(hx_args, tuple): hx_args = (hx_args,) if R is None: R = self.R elif isscalar(R): R = eye(self.dim_z) * R for k in range(self._num_sigmas): self.sigmas_h[k] = self.hx(self.sigmas_f[k], *hx_args) # mean and covariance of prediction passed through unscented transform zp, self.S = ckf_transform(self.sigmas_h, R) self.SI = inv(self.S) # compute cross variance of the state and the measurements m = self._num_sigmas # literaure uses m for scaling factor xf = self.x.flatten() zpf = zp.flatten() Pxz = outer_product_sum(self.sigmas_f - xf, self.sigmas_h - zpf) / m self.K = dot(Pxz, self.SI) # Kalman gain self.y = self.residual_z(z, zp) # residual self.x = self.x + dot(self.K, self.y) self.P = self.P - dot(self.K, self.S).dot(self.K.T) # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None @property def log_likelihood(self): """ log-likelihood of the last measurement. """ if self._log_likelihood is None: self._log_likelihood = logpdf(x=self.y, cov=self.S) return self._log_likelihood @property def likelihood(self): """ Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. """ if self._likelihood is None: self._likelihood = exp(self.log_likelihood) if self._likelihood == 0: self._likelihood = sys.float_info.min return self._likelihood @property def mahalanobis(self): """" Mahalanobis distance of innovation. E.g. 3 means measurement was 3 standard deviations away from the predicted value. Returns ------- mahalanobis : float """ if self._mahalanobis is None: self._mahalanobis = sqrt(float(dot(dot(self.y.T, self.SI), self.y))) return self._mahalanobis def __repr__(self): return '\n'.join([ 'CubatureKalmanFilter object', pretty_str('dim_x', self.dim_x), pretty_str('dim_z', self.dim_z), pretty_str('dt', self._dt), pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('log-likelihood', self.log_likelihood), pretty_str('likelihood', self.likelihood), pretty_str('mahalanobis', self.mahalanobis) ]) filterpy-1.4.5/filterpy/kalman/EKF.py000066400000000000000000000332641335747705400174660ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name,too-many-instance-attributes, too-many-arguments """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, unicode_literals) from copy import deepcopy from math import log, exp, sqrt import sys import numpy as np from numpy import dot, zeros, eye import scipy.linalg as linalg from filterpy.stats import logpdf from filterpy.common import pretty_str, reshape_z class ExtendedKalmanFilter(object): """ Implements an extended Kalman filter (EKF). You are responsible for setting the various state variables to reasonable values; the defaults will not give you a functional filter. You will have to set the following attributes after constructing this object for the filter to perform properly. Please note that there are various checks in place to ensure that you have made everything the 'correct' size. However, it is possible to provide incorrectly sized arrays such that the linear algebra can not perform an operation. It can also fail silently - you can end up with matrices of a size that allows the linear algebra to work, but are the wrong shape for the problem you are trying to solve. Parameters ---------- dim_x : int Number of state variables for the Kalman filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. This is used to set the default size of P, Q, and u dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. Attributes ---------- x : numpy.array(dim_x, 1) State estimate vector P : numpy.array(dim_x, dim_x) Covariance matrix x_prior : numpy.array(dim_x, 1) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_prior : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x, 1) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only. R : numpy.array(dim_z, dim_z) Measurement noise matrix Q : numpy.array(dim_x, dim_x) Process noise matrix F : numpy.array() State Transition matrix H : numpy.array(dim_x, dim_x) Measurement function y : numpy.array Residual of the update step. Read only. K : numpy.array(dim_x, dim_z) Kalman gain of the update step. Read only. S : numpy.array Systen uncertaintly projected to measurement space. Read only. z : ndarray Last measurement used in update(). Read only. log_likelihood : float log-likelihood of the last measurement. Read only. likelihood : float likelihood of last measurment. Read only. Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. mahalanobis : float mahalanobis distance of the innovation. E.g. 3 means measurement was 3 standard deviations away from the predicted value. Read only. Examples -------- See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python """ def __init__(self, dim_x, dim_z, dim_u=0): self.dim_x = dim_x self.dim_z = dim_z self.dim_u = dim_u self.x = zeros((dim_x, 1)) # state self.P = eye(dim_x) # uncertainty covariance self.B = 0 # control transition matrix self.F = np.eye(dim_x) # state transition matrix self.R = eye(dim_z) # state uncertainty self.Q = eye(dim_x) # process uncertainty self.y = zeros((dim_z, 1)) # residual z = np.array([None]*self.dim_z) self.z = reshape_z(z, self.dim_z, self.x.ndim) # gain and residual are computed during the innovation step. We # save them so that in case you want to inspect them for various # purposes self.K = np.zeros(self.x.shape) # kalman gain self.y = zeros((dim_z, 1)) self.S = np.zeros((dim_z, dim_z)) # system uncertainty self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty # identity matrix. Do not alter this. self._I = np.eye(dim_x) self._log_likelihood = log(sys.float_info.min) self._likelihood = sys.float_info.min self._mahalanobis = None # these will always be a copy of x,P after predict() is called self.x_prior = self.x.copy() self.P_prior = self.P.copy() # these will always be a copy of x,P after update() is called self.x_post = self.x.copy() self.P_post = self.P.copy() def predict_update(self, z, HJacobian, Hx, args=(), hx_args=(), u=0): """ Performs the predict/update innovation of the extended Kalman filter. Parameters ---------- z : np.array measurement for this step. If `None`, only predict step is perfomed. HJacobian : function function which computes the Jacobian of the H matrix (measurement function). Takes state variable (self.x) as input, along with the optional arguments in args, and returns H. Hx : function function which takes as input the state variable (self.x) along with the optional arguments in hx_args, and returns the measurement that would correspond to that state. args : tuple, optional, default (,) arguments to be passed into HJacobian after the required state variable. hx_args : tuple, optional, default (,) arguments to be passed into Hx after the required state variable. u : np.array or scalar optional control vector input to the filter. """ #pylint: disable=too-many-locals if not isinstance(args, tuple): args = (args,) if not isinstance(hx_args, tuple): hx_args = (hx_args,) if np.isscalar(z) and self.dim_z == 1: z = np.asarray([z], float) F = self.F B = self.B P = self.P Q = self.Q R = self.R x = self.x H = HJacobian(x, *args) # predict step x = dot(F, x) + dot(B, u) P = dot(F, P).dot(F.T) + Q # save prior self.x_prior = np.copy(self.x) self.P_prior = np.copy(self.P) # update step PHT = dot(P, H.T) self.S = dot(H, PHT) + R self.SI = linalg.inv(self.S) self.K = dot(PHT, self.SI) self.y = z - Hx(x, *hx_args) self.x = x + dot(self.K, self.y) I_KH = self._I - dot(self.K, H) self.P = dot(I_KH, P).dot(I_KH.T) + dot(self.K, R).dot(self.K.T) # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None def update(self, z, HJacobian, Hx, R=None, args=(), hx_args=(), residual=np.subtract): """ Performs the update innovation of the extended Kalman filter. Parameters ---------- z : np.array measurement for this step. If `None`, posterior is not computed HJacobian : function function which computes the Jacobian of the H matrix (measurement function). Takes state variable (self.x) as input, returns H. Hx : function function which takes as input the state variable (self.x) along with the optional arguments in hx_args, and returns the measurement that would correspond to that state. R : np.array, scalar, or None Optionally provide R to override the measurement noise for this one call, otherwise self.R will be used. args : tuple, optional, default (,) arguments to be passed into HJacobian after the required state variable. for robot localization you might need to pass in information about the map and time of day, so you might have `args=(map_data, time)`, where the signature of HCacobian will be `def HJacobian(x, map, t)` hx_args : tuple, optional, default (,) arguments to be passed into Hx function after the required state variable. residual : function (z, z2), optional Optional function that computes the residual (difference) between the two measurement vectors. If you do not provide this, then the built in minus operator will be used. You will normally want to use the built in unless your residual computation is nonlinear (for example, if they are angles) """ if z is None: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() return if not isinstance(args, tuple): args = (args,) if not isinstance(hx_args, tuple): hx_args = (hx_args,) if R is None: R = self.R elif np.isscalar(R): R = eye(self.dim_z) * R if np.isscalar(z) and self.dim_z == 1: z = np.asarray([z], float) H = HJacobian(self.x, *args) PHT = dot(self.P, H.T) self.S = dot(H, PHT) + R self.K = PHT.dot(linalg.inv(self.S)) hx = Hx(self.x, *hx_args) self.y = residual(z, hx) self.x = self.x + dot(self.K, self.y) # P = (I-KH)P(I-KH)' + KRK' is more numerically stable # and works for non-optimal K vs the equation # P = (I-KH)P usually seen in the literature. I_KH = self._I - dot(self.K, H) self.P = dot(I_KH, self.P).dot(I_KH.T) + dot(self.K, R).dot(self.K.T) # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() def predict_x(self, u=0): """ Predicts the next state of X. If you need to compute the next state yourself, override this function. You would need to do this, for example, if the usual Taylor expansion to generate F is not providing accurate results for you. """ self.x = dot(self.F, self.x) + dot(self.B, u) def predict(self, u=0): """ Predict next state (prior) using the Kalman filter state propagation equations. Parameters ---------- u : np.array Optional control vector. If non-zero, it is multiplied by B to create the control input into the system. """ self.predict_x(u) self.P = dot(self.F, self.P).dot(self.F.T) + self.Q # save prior self.x_prior = np.copy(self.x) self.P_prior = np.copy(self.P) @property def log_likelihood(self): """ log-likelihood of the last measurement. """ if self._log_likelihood is None: self._log_likelihood = logpdf(x=self.y, cov=self.S) return self._log_likelihood @property def likelihood(self): """ Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. """ if self._likelihood is None: self._likelihood = exp(self.log_likelihood) if self._likelihood == 0: self._likelihood = sys.float_info.min return self._likelihood @property def mahalanobis(self): """ Mahalanobis distance of innovation. E.g. 3 means measurement was 3 standard deviations away from the predicted value. Returns ------- mahalanobis : float """ if self._mahalanobis is None: self._mahalanobis = sqrt(float(dot(dot(self.y.T, self.SI), self.y))) return self._mahalanobis def __repr__(self): return '\n'.join([ 'KalmanFilter object', pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('x_prior', self.x_prior), pretty_str('P_prior', self.P_prior), pretty_str('F', self.F), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('S', self.S), pretty_str('likelihood', self.likelihood), pretty_str('log-likelihood', self.log_likelihood), pretty_str('mahalanobis', self.mahalanobis) ]) filterpy-1.4.5/filterpy/kalman/IMM.py000066400000000000000000000206721335747705400175020ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-instance-attributes """ Created on Mon Aug 6 07:53:34 2018 @author: rlabbe """ from __future__ import (absolute_import, division) import numpy as np from numpy import dot, asarray, zeros, outer from filterpy.common import pretty_str class IMMEstimator(object): """ Implements an Interacting Multiple-Model (IMM) estimator. Parameters ---------- filters : (N,) array_like of KalmanFilter objects List of N filters. filters[i] is the ith Kalman filter in the IMM estimator. Each filter must have the same dimension for the state `x` and `P`, otherwise the states of each filter cannot be mixed with each other. mu : (N,) array_like of float mode probability: mu[i] is the probability that filter i is the correct one. M : (N, N) ndarray of float Markov chain transition matrix. M[i,j] is the probability of switching from filter j to filter i. Attributes ---------- x : numpy.array(dim_x, 1) Current state estimate. Any call to update() or predict() updates this variable. P : numpy.array(dim_x, dim_x) Current state covariance matrix. Any call to update() or predict() updates this variable. x_prior : numpy.array(dim_x, 1) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_prior : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x, 1) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only. N : int number of filters in the filter bank mu : (N,) ndarray of float mode probability: mu[i] is the probability that filter i is the correct one. M : (N, N) ndarray of float Markov chain transition matrix. M[i,j] is the probability of switching from filter j to filter i. cbar : (N,) ndarray of float Total probability, after interaction, that the target is in state j. We use it as the # normalization constant. likelihood: (N,) ndarray of float Likelihood of each individual filter's last measurement. omega : (N, N) ndarray of float Mixing probabilitity - omega[i, j] is the probabilility of mixing the state of filter i into filter j. Perhaps more understandably, it weights the states of each filter by: x_j = sum(omega[i,j] * x_i) with a similar weighting for P_j Examples -------- >>> import numpy as np >>> from filterpy.common import kinematic_kf >>> kf1 = kinematic_kf(2, 2) >>> kf2 = kinematic_kf(2, 2) >>> # do some settings of x, R, P etc. here, I'll just use the defaults >>> kf2.Q *= 0 # no prediction error in second filter >>> >>> filters = [kf1, kf2] >>> mu = [0.5, 0.5] # each filter is equally likely at the start >>> trans = np.array([[0.97, 0.03], [0.03, 0.97]]) >>> imm = IMMEstimator(filters, mu, trans) >>> >>> for i in range(100): >>> # make some noisy data >>> x = i + np.random.randn()*np.sqrt(kf1.R[0, 0]) >>> y = i + np.random.randn()*np.sqrt(kf1.R[1, 1]) >>> z = np.array([[x], [y]]) >>> >>> # perform predict/update cycle >>> imm.predict() >>> imm.update(z) >>> print(imm.x.T) For a full explanation and more examples see my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python References ---------- Bar-Shalom, Y., Li, X-R., and Kirubarajan, T. "Estimation with Application to Tracking and Navigation". Wiley-Interscience, 2001. Crassidis, J and Junkins, J. "Optimal Estimation of Dynamic Systems". CRC Press, second edition. 2012. Labbe, R. "Kalman and Bayesian Filters in Python". https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python """ def __init__(self, filters, mu, M): if len(filters) < 2: raise ValueError('filters must contain at least two filters') self.filters = filters self.mu = asarray(mu) / np.sum(mu) self.M = M x_shape = filters[0].x.shape for f in filters: if x_shape != f.x.shape: raise ValueError( 'All filters must have the same state dimension') self.x = zeros(filters[0].x.shape) self.P = zeros(filters[0].P.shape) self.N = len(filters) # number of filters self.likelihood = zeros(self.N) self.omega = zeros((self.N, self.N)) self._compute_mixing_probabilities() # initialize imm state estimate based on current filters self._compute_state_estimate() self.x_prior = self.x.copy() self.P_prior = self.P.copy() self.x_post = self.x.copy() self.P_post = self.P.copy() def update(self, z): """ Add a new measurement (z) to the Kalman filter. If z is None, nothing is changed. Parameters ---------- z : np.array measurement for this update. """ # run update on each filter, and save the likelihood for i, f in enumerate(self.filters): f.update(z) self.likelihood[i] = f.likelihood # update mode probabilities from total probability * likelihood self.mu = self.cbar * self.likelihood self.mu /= np.sum(self.mu) # normalize self._compute_mixing_probabilities() # compute mixed IMM state and covariance and save posterior estimate self._compute_state_estimate() self.x_post = self.x.copy() self.P_post = self.P.copy() def predict(self, u=None): """ Predict next state (prior) using the IMM state propagation equations. Parameters ---------- u : np.array, optional Control vector. If not `None`, it is multiplied by B to create the control input into the system. """ # compute mixed initial conditions xs, Ps = [], [] for i, (f, w) in enumerate(zip(self.filters, self.omega.T)): x = zeros(self.x.shape) for kf, wj in zip(self.filters, w): x += kf.x * wj xs.append(x) P = zeros(self.P.shape) for kf, wj in zip(self.filters, w): y = kf.x - x P += wj * (outer(y, y) + kf.P) Ps.append(P) # compute each filter's prior using the mixed initial conditions for i, f in enumerate(self.filters): # propagate using the mixed state estimate and covariance f.x = xs[i].copy() f.P = Ps[i].copy() f.predict(u) # compute mixed IMM state and covariance and save posterior estimate self._compute_state_estimate() self.x_prior = self.x.copy() self.P_prior = self.P.copy() def _compute_state_estimate(self): """ Computes the IMM's mixed state estimate from each filter using the the mode probability self.mu to weight the estimates. """ self.x.fill(0) for f, mu in zip(self.filters, self.mu): self.x += f.x * mu self.P.fill(0) for f, mu in zip(self.filters, self.mu): y = f.x - self.x self.P += mu * (outer(y, y) + f.P) def _compute_mixing_probabilities(self): """ Compute the mixing probability for each filter. """ self.cbar = dot(self.mu, self.M) for i in range(self.N): for j in range(self.N): self.omega[i, j] = (self.M[i, j]*self.mu[i]) / self.cbar[j] def __repr__(self): return '\n'.join([ 'IMMEstimator object', pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('x_prior', self.x_prior), pretty_str('P_prior', self.P_prior), pretty_str('x_post', self.x_post), pretty_str('P_post', self.P_post), pretty_str('N', self.N), pretty_str('mu', self.mu), pretty_str('M', self.M), pretty_str('cbar', self.cbar), pretty_str('likelihood', self.likelihood), pretty_str('omega', self.omega) ]) filterpy-1.4.5/filterpy/kalman/UKF.py000066400000000000000000000652441335747705400175110ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division) from copy import deepcopy from math import log, exp, sqrt import sys import numpy as np from numpy import eye, zeros, dot, isscalar, outer from scipy.linalg import cholesky from filterpy.kalman import unscented_transform from filterpy.stats import logpdf from filterpy.common import pretty_str class UnscentedKalmanFilter(object): # pylint: disable=too-many-instance-attributes # pylint: disable=invalid-name r""" Implements the Scaled Unscented Kalman filter (UKF) as defined by Simon Julier in [1], using the formulation provided by Wan and Merle in [2]. This filter scales the sigma points to avoid strong nonlinearities. Parameters ---------- dim_x : int Number of state variables for the filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. This is for convience, so everything is sized correctly on creation. If you are using multiple sensors the size of `z` can change based on the sensor. Just provide the appropriate hx function dt : float Time between steps in seconds. hx : function(x) Measurement function. Converts state vector x into a measurement vector of shape (dim_z). fx : function(x,dt) function that returns the state x transformed by the state transistion function. dt is the time step in seconds. points : class Class which computes the sigma points and weights for a UKF algorithm. You can vary the UKF implementation by changing this class. For example, MerweScaledSigmaPoints implements the alpha, beta, kappa parameterization of Van der Merwe, and JulierSigmaPoints implements Julier's original kappa parameterization. See either of those for the required signature of this class if you want to implement your own. sqrt_fn : callable(ndarray), default=None (implies scipy.linalg.cholesky) Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe's dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you. If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing as far as this class is concerned. x_mean_fn : callable (sigma_points, weights), optional Function that computes the mean of the provided sigma points and weights. Use this if your state variable contains nonlinear values such as angles which cannot be summed. .. code-block:: Python def state_mean(sigmas, Wm): x = np.zeros(3) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] x[1] += s[1] * Wm[i] sum_sin += sin(s[2])*Wm[i] sum_cos += cos(s[2])*Wm[i] x[2] = atan2(sum_sin, sum_cos) return x z_mean_fn : callable (sigma_points, weights), optional Same as x_mean_fn, except it is called for sigma points which form the measurements after being passed through hx(). residual_x : callable (x, y), optional residual_z : callable (x, y), optional Function that computes the residual (difference) between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. One is for the state variable, the other is for the measurement state. .. code-block:: Python def residual(a, b): y = a[0] - b[0] if y > np.pi: y -= 2*np.pi if y < -np.pi: y = 2*np.pi return y Attributes ---------- x : numpy.array(dim_x) state estimate vector P : numpy.array(dim_x, dim_x) covariance estimate matrix x_prior : numpy.array(dim_x) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_prior : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only. z : ndarray Last measurement used in update(). Read only. R : numpy.array(dim_z, dim_z) measurement noise matrix Q : numpy.array(dim_x, dim_x) process noise matrix K : numpy.array Kalman gain y : numpy.array innovation residual log_likelihood : scalar Log likelihood of last measurement update. likelihood : float likelihood of last measurment. Read only. Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. mahalanobis : float mahalanobis distance of the measurement. Read only. inv : function, default numpy.linalg.inv If you prefer another inverse function, such as the Moore-Penrose pseudo inverse, set it to that instead: .. code-block:: Python kf.inv = np.linalg.pinv Examples -------- Simple example of a linear order 1 kinematic filter in 2D. There is no need to use a UKF for this example, but it is easy to read. >>> def fx(x, dt): >>> # state transition function - predict next state based >>> # on constant velocity model x = vt + x_0 >>> F = np.array([[1, dt, 0, 0], >>> [0, 1, 0, 0], >>> [0, 0, 1, dt], >>> [0, 0, 0, 1]], dtype=float) >>> return np.dot(F, x) >>> >>> def hx(x): >>> # measurement function - convert state into a measurement >>> # where measurements are [x_pos, y_pos] >>> return np.array([x[0], x[2]]) >>> >>> dt = 0.1 >>> # create sigma points to use in the filter. This is standard for Gaussian processes >>> points = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=-1) >>> >>> kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) >>> kf.x = np.array([-1., 1., -1., 1]) # initial state >>> kf.P *= 0.2 # initial uncertainty >>> z_std = 0.1 >>> kf.R = np.diag([z_std**2, z_std**2]) # 1 standard >>> kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01**2, block_size=2) >>> >>> zs = [[i+randn()*z_std, i+randn()*z_std] for i in range(50)] # measurements >>> for z in zs: >>> kf.predict() >>> kf.update(z) >>> print(kf.x, 'log-likelihood', kf.log_likelihood) For in depth explanations see my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python Also see the filterpy/kalman/tests subdirectory for test code that may be illuminating. References ---------- .. [1] Julier, Simon J. "The scaled unscented transformation," American Control Converence, 2002, pp 4555-4559, vol 6. Online copy: https://www.cs.unc.edu/~welch/kalman/media/pdf/ACC02-IEEE1357.PDF .. [2] E. A. Wan and R. Van der Merwe, “The unscented Kalman filter for nonlinear estimation,” in Proc. Symp. Adaptive Syst. Signal Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000. Online Copy: https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf .. [3] S. Julier, J. Uhlmann, and H. Durrant-Whyte. "A new method for the nonlinear transformation of means and covariances in filters and estimators," IEEE Transactions on Automatic Control, 45(3), pp. 477-482 (March 2000). .. [4] E. A. Wan and R. Van der Merwe, “The Unscented Kalman filter for Nonlinear Estimation,” in Proc. Symp. Adaptive Syst. Signal Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000. https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf .. [5] Wan, Merle "The Unscented Kalman Filter," chapter in *Kalman Filtering and Neural Networks*, John Wiley & Sons, Inc., 2001. .. [6] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models" (Doctoral dissertation) """ def __init__(self, dim_x, dim_z, dt, hx, fx, points, sqrt_fn=None, x_mean_fn=None, z_mean_fn=None, residual_x=None, residual_z=None): """ Create a Kalman filter. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter. """ #pylint: disable=too-many-arguments self.x = zeros(dim_x) self.P = eye(dim_x) self.x_prior = np.copy(self.x) self.P_prior = np.copy(self.P) self.Q = eye(dim_x) self.R = eye(dim_z) self._dim_x = dim_x self._dim_z = dim_z self.points_fn = points self._dt = dt self._num_sigmas = points.num_sigmas() self.hx = hx self.fx = fx self.x_mean = x_mean_fn self.z_mean = z_mean_fn # Only computed only if requested via property self._log_likelihood = log(sys.float_info.min) self._likelihood = sys.float_info.min self._mahalanobis = None if sqrt_fn is None: self.msqrt = cholesky else: self.msqrt = sqrt_fn # weights for the means and covariances. self.Wm, self.Wc = points.Wm, points.Wc if residual_x is None: self.residual_x = np.subtract else: self.residual_x = residual_x if residual_z is None: self.residual_z = np.subtract else: self.residual_z = residual_z # sigma points transformed through f(x) and h(x) # variables for efficiency so we don't recreate every update self.sigmas_f = zeros((self._num_sigmas, self._dim_x)) self.sigmas_h = zeros((self._num_sigmas, self._dim_z)) self.K = np.zeros((dim_x, dim_z)) # Kalman gain self.y = np.zeros((dim_z)) # residual self.z = np.array([[None]*dim_z]).T # measurement self.S = np.zeros((dim_z, dim_z)) # system uncertainty self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty self.inv = np.linalg.inv # these will always be a copy of x,P after predict() is called self.x_prior = self.x.copy() self.P_prior = self.P.copy() # these will always be a copy of x,P after update() is called self.x_post = self.x.copy() self.P_post = self.P.copy() def predict(self, dt=None, UT=None, fx=None, **fx_args): r""" Performs the predict step of the UKF. On return, self.x and self.P contain the predicted state (x) and covariance (P). ' Important: this MUST be called before update() is called for the first time. Parameters ---------- dt : double, optional If specified, the time step to be used for this prediction. self._dt is used if this is not provided. fx : callable f(x, **fx_args), optional State transition function. If not provided, the default function passed in during construction will be used. UT : function(sigmas, Wm, Wc, noise_cov), optional Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. **fx_args : keyword arguments optional keyword arguments to be passed into f(x). """ if dt is None: dt = self._dt if UT is None: UT = unscented_transform # calculate sigma points for given mean and covariance self.compute_process_sigmas(dt, fx, **fx_args) #and pass sigmas through the unscented transform to compute prior self.x, self.P = UT(self.sigmas_f, self.Wm, self.Wc, self.Q, self.x_mean, self.residual_x) # save prior self.x_prior = np.copy(self.x) self.P_prior = np.copy(self.P) def update(self, z, R=None, UT=None, hx=None, **hx_args): """ Update the UKF with the given measurements. On return, self.x and self.P contain the new mean and covariance of the filter. Parameters ---------- z : numpy.array of shape (dim_z) measurement vector R : numpy.array((dim_z, dim_z)), optional Measurement noise. If provided, overrides self.R for this function call. UT : function(sigmas, Wm, Wc, noise_cov), optional Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. **hx_args : keyword argument arguments to be passed into h(x) after x -> h(x, **hx_args) """ if z is None: self.z = np.array([[None]*self._dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() return if hx is None: hx = self.hx if UT is None: UT = unscented_transform if R is None: R = self.R elif isscalar(R): R = eye(self._dim_z) * R # pass prior sigmas through h(x) to get measurement sigmas # the shape of sigmas_h will vary if the shape of z varies, so # recreate each time sigmas_h = [] for s in self.sigmas_f: sigmas_h.append(hx(s, **hx_args)) self.sigmas_h = np.atleast_2d(sigmas_h) # mean and covariance of prediction passed through unscented transform zp, self.S = UT(self.sigmas_h, self.Wm, self.Wc, R, self.z_mean, self.residual_z) self.SI = self.inv(self.S) # compute cross variance of the state and the measurements Pxz = self.cross_variance(self.x, zp, self.sigmas_f, self.sigmas_h) self.K = dot(Pxz, self.SI) # Kalman gain self.y = self.residual_z(z, zp) # residual # update Gaussian state estimate (x, P) self.x = self.x + dot(self.K, self.y) self.P = self.P - dot(self.K, dot(self.S, self.K.T)) # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None def cross_variance(self, x, z, sigmas_f, sigmas_h): """ Compute cross variance of the state `x` and measurement `z`. """ Pxz = zeros((sigmas_f.shape[1], sigmas_h.shape[1])) N = sigmas_f.shape[0] for i in range(N): dx = self.residual_x(sigmas_f[i], x) dz = self.residual_z(sigmas_h[i], z) Pxz += self.Wc[i] * outer(dx, dz) return Pxz def compute_process_sigmas(self, dt, fx=None, **fx_args): """ computes the values of sigmas_f. Normally a user would not call this, but it is useful if you need to call update more than once between calls to predict (to update for multiple simultaneous measurements), so the sigmas correctly reflect the updated state x, P. """ if fx is None: fx = self.fx # calculate sigma points for given mean and covariance sigmas = self.points_fn.sigma_points(self.x, self.P) for i, s in enumerate(sigmas): self.sigmas_f[i] = fx(s, dt, **fx_args) def batch_filter(self, zs, Rs=None, dts=None, UT=None, saver=None): """ Performs the UKF filter over the list of measurement in `zs`. Parameters ---------- zs : list-like list of measurements at each time step `self._dt` Missing measurements must be represented by 'None'. Rs : None, np.array or list-like, default=None optional list of values to use for the measurement error covariance R. If Rs is None then self.R is used for all epochs. If it is a list of matrices or a 3D array where len(Rs) == len(zs), then it is treated as a list of R values, one per epoch. This allows you to have varying R per epoch. dts : None, scalar or list-like, default=None optional value or list of delta time to be passed into predict. If dtss is None then self.dt is used for all epochs. If it is a list where len(dts) == len(zs), then it is treated as a list of dt values, one per epoch. This allows you to have varying epoch durations. UT : function(sigmas, Wm, Wc, noise_cov), optional Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. saver : filterpy.common.Saver, optional filterpy.common.Saver object. If provided, saver.save() will be called after every epoch Returns ------- means: ndarray((n,dim_x,1)) array of the state for each time step after the update. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance: ndarray((n,dim_x,dim_x)) array of the covariances for each time step after the update. In other words `covariance[k,:,:]` is the covariance at step `k`. Examples -------- .. code-block:: Python # this example demonstrates tracking a measurement where the time # between measurement varies, as stored in dts The output is then smoothed # with an RTS smoother. zs = [t + random.randn()*4 for t in range (40)] (mu, cov, _, _) = ukf.batch_filter(zs, dts=dts) (xs, Ps, Ks) = ukf.rts_smoother(mu, cov) """ #pylint: disable=too-many-arguments try: z = zs[0] except TypeError: raise TypeError('zs must be list-like') if self._dim_z == 1: if not(isscalar(z) or (z.ndim == 1 and len(z) == 1)): raise TypeError('zs must be a list of scalars or 1D, 1 element arrays') else: if len(z) != self._dim_z: raise TypeError( 'each element in zs must be a 1D array of length {}'.format(self._dim_z)) z_n = np.size(zs, 0) if Rs is None: Rs = [self.R] * z_n if dts is None: dts = [self._dt] * z_n # mean estimates from Kalman Filter if self.x.ndim == 1: means = zeros((z_n, self._dim_x)) else: means = zeros((z_n, self._dim_x, 1)) # state covariances from Kalman Filter covariances = zeros((z_n, self._dim_x, self._dim_x)) for i, (z, r, dt) in enumerate(zip(zs, Rs, dts)): self.predict(dt=dt, UT=UT) self.update(z, r, UT=UT) means[i, :] = self.x covariances[i, :, :] = self.P if saver is not None: saver.save() return (means, covariances) def rts_smoother(self, Xs, Ps, Qs=None, dts=None, UT=None): """ Runs the Rauch-Tung-Striebal Kalman smoother on a set of means and covariances computed by the UKF. The usual input would come from the output of `batch_filter()`. Parameters ---------- Xs : numpy.array array of the means (state variable x) of the output of a Kalman filter. Ps : numpy.array array of the covariances of the output of a kalman filter. Qs: list-like collection of numpy.array, optional Process noise of the Kalman filter at each time step. Optional, if not provided the filter's self.Q will be used dt : optional, float or array-like of float If provided, specifies the time step of each step of the filter. If float, then the same time step is used for all steps. If an array, then each element k contains the time at step k. Units are seconds. UT : function(sigmas, Wm, Wc, noise_cov), optional Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. Returns ------- x : numpy.ndarray smoothed means P : numpy.ndarray smoothed state covariances K : numpy.ndarray smoother gain at each step Examples -------- .. code-block:: Python zs = [t + random.randn()*4 for t in range (40)] (mu, cov, _, _) = kalman.batch_filter(zs) (x, P, K) = rts_smoother(mu, cov, fk.F, fk.Q) """ #pylint: disable=too-many-locals, too-many-arguments if len(Xs) != len(Ps): raise ValueError('Xs and Ps must have the same length') n, dim_x = Xs.shape if dts is None: dts = [self._dt] * n elif isscalar(dts): dts = [dts] * n if Qs is None: Qs = [self.Q] * n if UT is None: UT = unscented_transform # smoother gain Ks = zeros((n, dim_x, dim_x)) num_sigmas = self._num_sigmas xs, ps = Xs.copy(), Ps.copy() sigmas_f = zeros((num_sigmas, dim_x)) for k in reversed(range(n-1)): # create sigma points from state estimate, pass through state func sigmas = self.points_fn.sigma_points(xs[k], ps[k]) for i in range(num_sigmas): sigmas_f[i] = self.fx(sigmas[i], dts[k]) xb, Pb = UT( sigmas_f, self.Wm, self.Wc, self.Q, self.x_mean, self.residual_x) # compute cross variance Pxb = 0 for i in range(num_sigmas): y = self.residual_x(sigmas_f[i], xb) z = self.residual_x(sigmas[i], Xs[k]) Pxb += self.Wc[i] * outer(z, y) # compute gain K = dot(Pxb, self.inv(Pb)) # update the smoothed estimates xs[k] += dot(K, self.residual_x(xs[k+1], xb)) ps[k] += dot(K, ps[k+1] - Pb).dot(K.T) Ks[k] = K return (xs, ps, Ks) @property def log_likelihood(self): """ log-likelihood of the last measurement. """ if self._log_likelihood is None: self._log_likelihood = logpdf(x=self.y, cov=self.S) return self._log_likelihood @property def likelihood(self): """ Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. """ if self._likelihood is None: self._likelihood = exp(self.log_likelihood) if self._likelihood == 0: self._likelihood = sys.float_info.min return self._likelihood @property def mahalanobis(self): """" Mahalanobis distance of measurement. E.g. 3 means measurement was 3 standard deviations away from the predicted value. Returns ------- mahalanobis : float """ if self._mahalanobis is None: self._mahalanobis = sqrt(float(dot(dot(self.y.T, self.SI), self.y))) return self._mahalanobis def __repr__(self): return '\n'.join([ 'UnscentedKalmanFilter object', pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('x_prior', self.x_prior), pretty_str('P_prior', self.P_prior), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('S', self.S), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('log-likelihood', self.log_likelihood), pretty_str('likelihood', self.likelihood), pretty_str('mahalanobis', self.mahalanobis), pretty_str('sigmas_f', self.sigmas_f), pretty_str('h', self.sigmas_h), pretty_str('Wm', self.Wm), pretty_str('Wc', self.Wc), pretty_str('residual_x', self.residual_x), pretty_str('residual_z', self.residual_z), pretty_str('msqrt', self.msqrt), pretty_str('hx', self.hx), pretty_str('fx', self.fx), pretty_str('x_mean', self.x_mean), pretty_str('z_mean', self.z_mean) ]) filterpy-1.4.5/filterpy/kalman/__init__.py000066400000000000000000000015641335747705400206160ustar00rootroot00000000000000# -*- coding: utf-8 -*- #pylint: disable=wildcard-import """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) from .EKF import * from .ensemble_kalman_filter import * from .fading_memory import * from .fixed_lag_smoother import FixedLagSmoother from .kalman_filter import * from .IMM import * from .unscented_transform import unscented_transform from .information_filter import * from .mmae import * from .sigma_points import * from .square_root import * from .UKF import * from .CubatureKalmanFilter import * filterpy-1.4.5/filterpy/kalman/ensemble_kalman_filter.py000066400000000000000000000216021335747705400235340ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-arguments, too-many-instance-attributes # pylint: disable=attribute-defined-outside-init """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) from copy import deepcopy import numpy as np from numpy import array, zeros, eye, dot from numpy.random import multivariate_normal from filterpy.common import pretty_str, outer_product_sum class EnsembleKalmanFilter(object): """ This implements the ensemble Kalman filter (EnKF). The EnKF uses an ensemble of hundreds to thousands of state vectors that are randomly sampled around the estimate, and adds perturbations at each update and predict step. It is useful for extremely large systems such as found in hydrophysics. As such, this class is admittedly a toy as it is far too slow with large N. There are many versions of this sort of this filter. This formulation is due to Crassidis and Junkins [1]. It works with both linear and nonlinear systems. Parameters ---------- x : np.array(dim_x) state mean P : np.array((dim_x, dim_x)) covariance of the state dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. dt : float time step in seconds N : int number of sigma points (ensembles). Must be greater than 1. K : np.array Kalman gain hx : function hx(x) Measurement function. May be linear or nonlinear - converts state x into a measurement. Return must be an np.array of the same dimensionality as the measurement vector. fx : function fx(x, dt) State transition function. May be linear or nonlinear. Projects state x into the next time period. Returns the projected state x. Attributes ---------- x : numpy.array(dim_x, 1) State estimate P : numpy.array(dim_x, dim_x) State covariance matrix x_prior : numpy.array(dim_x, 1) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_prior : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x, 1) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only. z : numpy.array Last measurement used in update(). Read only. R : numpy.array(dim_z, dim_z) Measurement noise matrix Q : numpy.array(dim_x, dim_x) Process noise matrix fx : callable (x, dt) State transition function hx : callable (x) Measurement function. Convert state `x` into a measurement K : numpy.array(dim_x, dim_z) Kalman gain of the update step. Read only. inv : function, default numpy.linalg.inv If you prefer another inverse function, such as the Moore-Penrose pseudo inverse, set it to that instead: kf.inv = np.linalg.pinv Examples -------- .. code-block:: Python def hx(x): return np.array([x[0]]) F = np.array([[1., 1.], [0., 1.]]) def fx(x, dt): return np.dot(F, x) x = np.array([0., 1.]) P = np.eye(2) * 100. dt = 0.1 f = EnsembleKalmanFilter(x=x, P=P, dim_z=1, dt=dt, N=8, hx=hx, fx=fx) std_noise = 3. f.R *= std_noise**2 f.Q = Q_discrete_white_noise(2, dt, .01) while True: z = read_sensor() f.predict() f.update(np.asarray([z])) See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python References ---------- - [1] John L Crassidis and John L. Junkins. "Optimal Estimation of Dynamic Systems. CRC Press, second edition. 2012. pp, 257-9. """ def __init__(self, x, P, dim_z, dt, N, hx, fx): if dim_z <= 0: raise ValueError('dim_z must be greater than zero') if N <= 0: raise ValueError('N must be greater than zero') dim_x = len(x) self.dim_x = dim_x self.dim_z = dim_z self.dt = dt self.N = N self.hx = hx self.fx = fx self.K = zeros((dim_x, dim_z)) self.z = array([[None] * self.dim_z]).T self.S = zeros((dim_z, dim_z)) # system uncertainty self.SI = zeros((dim_z, dim_z)) # inverse system uncertainty self.initialize(x, P) self.Q = eye(dim_x) # process uncertainty self.R = eye(dim_z) # state uncertainty self.inv = np.linalg.inv # used to create error terms centered at 0 mean for # state and measurement self._mean = zeros(dim_x) self._mean_z = zeros(dim_z) def initialize(self, x, P): """ Initializes the filter with the specified mean and covariance. Only need to call this if you are using the filter to filter more than one set of data; this is called by __init__ Parameters ---------- x : np.array(dim_z) state mean P : np.array((dim_x, dim_x)) covariance of the state """ if x.ndim != 1: raise ValueError('x must be a 1D array') self.sigmas = multivariate_normal(mean=x, cov=P, size=self.N) self.x = x self.P = P # these will always be a copy of x,P after predict() is called self.x_prior = self.x.copy() self.P_prior = self.P.copy() # these will always be a copy of x,P after update() is called self.x_post = self.x.copy() self.P_post = self.P.copy() def update(self, z, R=None): """ Add a new measurement (z) to the kalman filter. If z is None, nothing is changed. Parameters ---------- z : np.array measurement for this update. R : np.array, scalar, or None Optionally provide R to override the measurement noise for this one call, otherwise self.R will be used. """ if z is None: self.z = array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() return if R is None: R = self.R if np.isscalar(R): R = eye(self.dim_z) * R N = self.N dim_z = len(z) sigmas_h = zeros((N, dim_z)) # transform sigma points into measurement space for i in range(N): sigmas_h[i] = self.hx(self.sigmas[i]) z_mean = np.mean(sigmas_h, axis=0) P_zz = (outer_product_sum(sigmas_h - z_mean) / (N-1)) + R P_xz = outer_product_sum( self.sigmas - self.x, sigmas_h - z_mean) / (N - 1) self.S = P_zz self.SI = self.inv(self.S) self.K = dot(P_xz, self.SI) e_r = multivariate_normal(self._mean_z, R, N) for i in range(N): self.sigmas[i] += dot(self.K, z + e_r[i] - sigmas_h[i]) self.x = np.mean(self.sigmas, axis=0) self.P = self.P - dot(dot(self.K, self.S), self.K.T) # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() def predict(self): """ Predict next position. """ N = self.N for i, s in enumerate(self.sigmas): self.sigmas[i] = self.fx(s, self.dt) e = multivariate_normal(self._mean, self.Q, N) self.sigmas += e self.x = np.mean(self.sigmas, axis=0) self.P = outer_product_sum(self.sigmas - self.x) / (N - 1) # save prior self.x_prior = np.copy(self.x) self.P_prior = np.copy(self.P) def __repr__(self): return '\n'.join([ 'EnsembleKalmanFilter object', pretty_str('dim_x', self.dim_x), pretty_str('dim_z', self.dim_z), pretty_str('dt', self.dt), pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('x_prior', self.x_prior), pretty_str('P_prior', self.P_prior), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('K', self.K), pretty_str('S', self.S), pretty_str('sigmas', self.sigmas), pretty_str('hx', self.hx), pretty_str('fx', self.fx) ]) filterpy-1.4.5/filterpy/kalman/fading_memory.py000066400000000000000000000342111335747705400216720ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-arguments, too-many-instance-attributes """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, unicode_literals) from copy import deepcopy from math import log, exp, sqrt import sys import warnings import numpy as np from numpy import dot, zeros, eye import scipy.linalg as linalg from filterpy.stats import logpdf from filterpy.common import pretty_str class FadingKalmanFilter(object): """ Fading memory Kalman filter. This implements a linear Kalman filter with a fading memory effect controlled by `alpha`. This is obsolete. The class KalmanFilter now incorporates the `alpha` attribute, and should be used instead. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter. Parameters ---------- alpha : float, >= 1 alpha controls how much you want the filter to forget past measurements. alpha==1 yields identical performance to the Kalman filter. A typical application might use 1.01 dim_x : int Number of state variables for the Kalman filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. This is used to set the default size of P, Q, and u dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. dim_u : int (optional) size of the control input, if it is being used. Default value of 0 indicates it is not used. Attributes ---------- You will have to assign reasonable values to all of these before running the filter. All must have dtype of float x : ndarray (dim_x, 1), default = [0,0,0...0] state of the filter P : ndarray (dim_x, dim_x), default identity matrix covariance matrix x_prior : numpy.array(dim_x, 1) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_prior : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x, 1) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only. z : ndarray Last measurement used in update(). Read only. Q : ndarray (dim_x, dim_x), default identity matrix Process uncertainty matrix R : ndarray (dim_z, dim_z), default identity matrix measurement uncertainty H : ndarray (dim_z, dim_x) measurement function F : ndarray (dim_x, dim_x) state transistion matrix B : ndarray (dim_x, dim_u), default 0 control transition matrix y : numpy.array Residual of the update step. Read only. K : numpy.array(dim_x, dim_z) Kalman gain of the update step. Read only. S : numpy.array System uncertainty (P projected to measurement space). Read only. S : numpy.array Inverse system uncertainty. Read only. log_likelihood : float log-likelihood of the last measurement. Read only. likelihood : float likelihood of last measurement. Read only. Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. mahalanobis : float mahalanobis distance of the innovation. Read only. Examples -------- See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python """ def __init__(self, alpha, dim_x, dim_z, dim_u=0): warnings.warn( "Use KalmanFilter class instead; it also provides the alpha attribute", DeprecationWarning) assert alpha >= 1 assert dim_x > 0 assert dim_z > 0 assert dim_u >= 0 self.alpha_sq = alpha**2 self.dim_x = dim_x self.dim_z = dim_z self.dim_u = dim_u self.x = zeros((dim_x, 1)) # state self.P = eye(dim_x) # uncertainty covariance self.Q = eye(dim_x) # process uncertainty self.B = 0. # control transition matrix self.F = np.eye(dim_x) # state transition matrix self.H = zeros((dim_z, dim_x)) # Measurement function self.R = eye(dim_z) # state uncertainty self.z = np.array([[None]*dim_z]).T # gain and residual are computed during the innovation step. We # save them so that in case you want to inspect them for various # purposes self.K = 0 # kalman gain self.y = zeros((dim_z, 1)) self.S = np.zeros((dim_z, dim_z)) # system uncertainty (measurement space) self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty # identity matrix. Do not alter this. self.I = np.eye(dim_x) # Only computed only if requested via property self._log_likelihood = log(sys.float_info.min) self._likelihood = sys.float_info.min self._mahalanobis = None # these will always be a copy of x,P after predict() is called self.x_prior = self.x.copy() self.P_prior = self.P.copy() # these will always be a copy of x,P after update() is called self.x_post = self.x.copy() self.P_post = self.P.copy() def update(self, z, R=None): """ Add a new measurement (z) to the kalman filter. If z is None, nothing is changed. Parameters ---------- z : np.array measurement for this update. R : np.array, scalar, or None Optionally provide R to override the measurement noise for this one call, otherwise self.R will be used. """ if z is None: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() return if R is None: R = self.R elif np.isscalar(R): R = eye(self.dim_z) * R # y = z - Hx # error (residual) between measurement and prediction self.y = z - dot(self.H, self.x) PHT = dot(self.P, self.H.T) # S = HPH' + R # project system uncertainty into measurement space self.S = dot(self.H, PHT) + R self.SI = linalg.inv(self.S) # K = PH'inv(S) # map system uncertainty into kalman gain self.K = PHT.dot(self.SI) # x = x + Ky # predict new x with residual scaled by the kalman gain self.x = self.x + dot(self.K, self.y) # P = (I-KH)P(I-KH)' + KRK' I_KH = self.I - dot(self.K, self.H) self.P = dot(I_KH, self.P).dot(I_KH.T) + dot(self.K, R).dot(self.K.T) # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None def predict(self, u=0): """ Predict next position. Parameters ---------- u : np.array Optional control vector. If non-zero, it is multiplied by B to create the control input into the system. """ # x = Fx + Bu self.x = dot(self.F, self.x) + dot(self.B, u) # P = FPF' + Q self.P = self.alpha_sq * dot(self.F, self.P).dot(self.F.T) + self.Q # save prior self.x_prior = self.x.copy() self.P_prior = self.P.copy() def batch_filter(self, zs, Rs=None, update_first=False): """ Batch processes a sequences of measurements. Parameters ---------- zs : list-like list of measurements at each time step `self.dt` Missing measurements must be represented by 'None'. Rs : list-like, optional optional list of values to use for the measurement error covariance; a value of None in any position will cause the filter to use `self.R` for that time step. update_first : bool, optional, controls whether the order of operations is update followed by predict, or predict followed by update. Default is predict->update. Returns ------- means: np.array((n,dim_x,1)) array of the state for each time step after the update. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance: np.array((n,dim_x,dim_x)) array of the covariances for each time step after the update. In other words `covariance[k,:,:]` is the covariance at step `k`. means_predictions: np.array((n,dim_x,1)) array of the state for each time step after the predictions. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance_predictions: np.array((n,dim_x,dim_x)) array of the covariances for each time step after the prediction. In other words `covariance[k,:,:]` is the covariance at step `k`. """ n = np.size(zs, 0) if Rs is None: Rs = [None] * n #pylint: disable=bad-whitespace # mean estimates from Kalman Filter means = zeros((n, self.dim_x, 1)) means_p = zeros((n, self.dim_x, 1)) # state covariances from Kalman Filter covariances = zeros((n, self.dim_x, self.dim_x)) covariances_p = zeros((n, self.dim_x, self.dim_x)) if update_first: for i, (z, r) in enumerate(zip(zs, Rs)): self.update(z, r) means[i, :] = self.x covariances[i, :, :] = self.P self.predict() means_p[i, :] = self.x covariances_p[i, :, :] = self.P else: for i, (z, r) in enumerate(zip(zs, Rs)): self.predict() means_p[i, :] = self.x covariances_p[i, :, :] = self.P self.update(z, r) means[i, :] = self.x covariances[i, :, :] = self.P return (means, covariances, means_p, covariances_p) def get_prediction(self, u=0): """ Predicts the next state of the filter and returns it. Does not alter the state of the filter. Parameters ---------- u : np.array optional control input Returns ------- (x, P) State vector and covariance array of the prediction. """ x = dot(self.F, self.x) + dot(self.B, u) P = self.alpha_sq * dot(self.F, self.P).dot(self.F.T) + self.Q return (x, P) def residual_of(self, z): """ returns the residual for the given measurement (z). Does not alter the state of the filter. """ return z - dot(self.H, self.x) def measurement_of_state(self, x): """ Helper function that converts a state into a measurement. Parameters ---------- x : np.array kalman state vector Returns ------- z : np.array measurement corresponding to the given state """ return dot(self.H, x) @property def alpha(self): """ scaling factor for fading memory""" return sqrt(self.alpha_sq) @property def log_likelihood(self): """ log-likelihood of the last measurement. """ if self._log_likelihood is None: self._log_likelihood = logpdf(x=self.y, cov=self.S) return self._log_likelihood @property def likelihood(self): """ Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. """ if self._likelihood is None: self._likelihood = exp(self.log_likelihood) if self._likelihood == 0: self._likelihood = sys.float_info.min return self._likelihood @property def mahalanobis(self): """" Mahalanobis distance of innovation. E.g. 3 means measurement was 3 standard deviations away from the predicted value. Returns ------- mahalanobis : float """ if self._mahalanobis is None: self._mahalanobis = sqrt(float(dot(dot(self.y.T, self.SI), self.y))) return self._mahalanobis def __repr__(self): return '\n'.join([ 'FadingKalmanFilter object', pretty_str('dim_x', self.x), pretty_str('dim_z', self.x), pretty_str('dim_u', self.dim_u), pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('F', self.F), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('H', self.H), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('S', self.S), pretty_str('B', self.B), pretty_str('likelihood', self.likelihood), pretty_str('log-likelihood', self.log_likelihood), pretty_str('mahalanobis', self.mahalanobis), pretty_str('alpha', self.alpha) ]) filterpy-1.4.5/filterpy/kalman/fixed_lag_smoother.py000066400000000000000000000236551335747705400227260ustar00rootroot00000000000000# -*- coding: utf-8 -*- #pylint: disable=too-many-instance-attributes, too-many-locals, invalid-name """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy as np from numpy import dot, zeros, eye from scipy.linalg import inv from filterpy.common import pretty_str class FixedLagSmoother(object): """ Fixed Lag Kalman smoother. Computes a smoothed sequence from a set of measurements based on the fixed lag Kalman smoother. At time k, for a lag N, the fixed-lag smoother computes the state estimate for time k-N based on all measurements made between times k-N and k. This yields a pretty good smoothed result with O(N) extra computations performed for each measurement. In other words, if N=4 this will consume about 5x the number of computations as a basic Kalman filter. However, the loops contain only 3 dot products, so it will be much faster than this sounds as the main Kalman filter loop involves transposes and inverses, as well as many more matrix multiplications. Implementation based on Wikipedia article as it existed on November 18, 2014. Examples -------- .. code-block:: Python from filterpy.kalman import FixedLagSmoother fls = FixedLagSmoother(dim_x=2, dim_z=1) fls.x = np.array([[0.], [.5]]) fls.F = np.array([[1.,1.], [0.,1.]]) fls.H = np.array([[1.,0.]]) fls.P *= 200 fls.R *= 5. fls.Q *= 0.001 zs = [...some measurements...] xhatsmooth, xhat = fls.smooth_batch(zs, N=4) See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python References ---------- Wikipedia http://en.wikipedia.org/wiki/Kalman_filter#Fixed-lag_smoother Simon, Dan. "Optimal State Estimation," John Wiley & Sons pp 274-8 (2006). | | """ def __init__(self, dim_x, dim_z, N=None): """ Create a fixed lag Kalman filter smoother. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter. Parameters ---------- dim_x : int Number of state variables for the Kalman filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. This is used to set the default size of P, Q, and u dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. N : int, optional If provided, the size of the lag. Not needed if you are only using smooth_batch() function. Required if calling smooth() """ self.dim_x = dim_x self.dim_z = dim_z self.N = N self.x = zeros((dim_x, 1)) # state self.x_s = zeros((dim_x, 1)) # smoothed state self.P = eye(dim_x) # uncertainty covariance self.Q = eye(dim_x) # process uncertainty self.F = eye(dim_x) # state transition matrix self.H = eye(dim_z, dim_x) # Measurement function self.R = eye(dim_z) # state uncertainty self.K = zeros((dim_x, 1)) # kalman gain self.y = zeros((dim_z, 1)) self.B = 0. self.S = zeros((dim_z, dim_z)) # identity matrix. Do not alter this. self._I = np.eye(dim_x) self.count = 0 if N is not None: self.xSmooth = [] def smooth(self, z, u=None): """ Smooths the measurement using a fixed lag smoother. On return, self.xSmooth is populated with the N previous smoothed estimates, where self.xSmooth[k] is the kth time step. self.x merely contains the current Kalman filter output of the most recent measurement, and is not smoothed at all (beyond the normal Kalman filter processing). self.xSmooth grows in length on each call. If you run this 1 million times, it will contain 1 million elements. Sure, we could minimize this, but then this would make the caller's code much more cumbersome. This also means that you cannot use this filter to track more than one data set; as data will be hopelessly intermingled. If you want to filter something else, create a new FixedLagSmoother object. Parameters ---------- z : ndarray or scalar measurement to be smoothed u : ndarray, optional If provided, control input to the filter """ # take advantage of the fact that np.array are assigned by reference. H = self.H R = self.R F = self.F P = self.P x = self.x Q = self.Q B = self.B N = self.N k = self.count # predict step of normal Kalman filter x_pre = dot(F, x) if u is not None: x_pre += dot(B, u) P = dot(F, P).dot(F.T) + Q # update step of normal Kalman filter self.y = z - dot(H, x_pre) self.S = dot(H, P).dot(H.T) + R SI = inv(self.S) K = dot(P, H.T).dot(SI) x = x_pre + dot(K, self.y) I_KH = self._I - dot(K, H) P = dot(I_KH, P).dot(I_KH.T) + dot(K, R).dot(K.T) self.xSmooth.append(x_pre.copy()) #compute invariants HTSI = dot(H.T, SI) F_LH = (F - dot(K, H)).T if k >= N: PS = P.copy() # smoothed P for step i for i in range(N): K = dot(PS, HTSI) # smoothed gain PS = dot(PS, F_LH) # smoothed covariance si = k-i self.xSmooth[si] = self.xSmooth[si] + dot(K, self.y) else: # Some sources specify starting the fix lag smoother only # after N steps have passed, some don't. I am getting far # better results by starting only at step N. self.xSmooth[k] = x.copy() self.count += 1 self.x = x self.P = P def smooth_batch(self, zs, N, us=None): """ batch smooths the set of measurements using a fixed lag smoother. I consider this function a somewhat pedalogical exercise; why would you not use a RTS smoother if you are able to batch process your data? Hint: RTS is a much better smoother, and faster besides. Use it. This is a batch processor, so it does not alter any of the object's data. In particular, self.x is NOT modified. All date is returned by the function. Parameters ---------- zs : ndarray of measurements iterable list (usually ndarray, but whatever works for you) of measurements that you want to smooth, one per time step. N : int size of fixed lag in time steps us : ndarray, optional If provided, control input to the filter for each time step Returns ------- (xhat_smooth, xhat) : ndarray, ndarray xhat_smooth is the output of the N step fix lag smoother xhat is the filter output of the standard Kalman filter """ # take advantage of the fact that np.array are assigned by reference. H = self.H R = self.R F = self.F P = self.P x = self.x Q = self.Q B = self.B if x.ndim == 1: xSmooth = zeros((len(zs), self.dim_x)) xhat = zeros((len(zs), self.dim_x)) else: xSmooth = zeros((len(zs), self.dim_x, 1)) xhat = zeros((len(zs), self.dim_x, 1)) for k, z in enumerate(zs): # predict step of normal Kalman filter x_pre = dot(F, x) if us is not None: x_pre += dot(B, us[k]) P = dot(F, P).dot(F.T) + Q # update step of normal Kalman filter y = z - dot(H, x_pre) S = dot(H, P).dot(H.T) + R SI = inv(S) K = dot(P, H.T).dot(SI) x = x_pre + dot(K, y) I_KH = self._I - dot(K, H) P = dot(I_KH, P).dot(I_KH.T) + dot(K, R).dot(K.T) xhat[k] = x.copy() xSmooth[k] = x_pre.copy() #compute invariants HTSI = dot(H.T, SI) F_LH = (F - dot(K, H)).T if k >= N: PS = P.copy() # smoothed P for step i for i in range(N): K = dot(PS, HTSI) # smoothed gain PS = dot(PS, F_LH) # smoothed covariance si = k-i xSmooth[si] = xSmooth[si] + dot(K, y) else: # Some sources specify starting the fix lag smoother only # after N steps have passed, some don't. I am getting far # better results by starting only at step N. xSmooth[k] = xhat[k] return xSmooth, xhat def __repr__(self): return '\n'.join([ 'FixedLagSmoother object', pretty_str('dim_x', self.x), pretty_str('dim_z', self.x), pretty_str('N', self.N), pretty_str('x', self.x), pretty_str('x_s', self.x_s), pretty_str('P', self.P), pretty_str('F', self.F), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('H', self.H), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('S', self.S), pretty_str('B', self.B), ]) filterpy-1.4.5/filterpy/kalman/information_filter.py000066400000000000000000000311321335747705400227430ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-instance-attributes """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division) from copy import deepcopy import math import sys import numpy as np from numpy import dot, zeros, eye from filterpy.stats import logpdf from filterpy.common import pretty_str, reshape_z class InformationFilter(object): """ Create a linear Information filter. Information filters compute the inverse of the Kalman filter, allowing you to easily denote having no information at initialization. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter. Parameters ---------- dim_x : int Number of state variables for the filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. This is used to set the default size of P, Q, and u dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. dim_u : int (optional) size of the control input, if it is being used. Default value of 0 indicates it is not used. self.compute_log_likelihood = compute_log_likelihood self.log_likelihood = math.log(sys.float_info.min) Attributes ---------- x : numpy.array(dim_x, 1) State estimate vector P_inv : numpy.array(dim_x, dim_x) inverse state covariance matrix x_prior : numpy.array(dim_x, 1) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_inv_prior : numpy.array(dim_x, dim_x) Inverse prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x, 1) Posterior (updated) state estimate. Read Only. P_inv_post : numpy.array(dim_x, dim_x) Inverse posterior (updated) state covariance matrix. Read Only. z : ndarray Last measurement used in update(). Read only. R_inv : numpy.array(dim_z, dim_z) inverse of measurement noise matrix Q : numpy.array(dim_x, dim_x) Process noise matrix H : numpy.array(dim_z, dim_x) Measurement function y : numpy.array Residual of the update step. Read only. K : numpy.array(dim_x, dim_z) Kalman gain of the update step. Read only. S : numpy.array Systen uncertaintly projected to measurement space. Read only. log_likelihood : float log-likelihood of the last measurement. Read only. likelihood : float likelihood of last measurment. Read only. Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. inv : function, default numpy.linalg.inv If you prefer another inverse function, such as the Moore-Penrose pseudo inverse, set it to that instead: kf.inv = np.linalg.pinv Examples -------- See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python """ def __init__(self, dim_x, dim_z, dim_u=0, compute_log_likelihood=True): if dim_x < 1: raise ValueError('dim_x must be 1 or greater') if dim_z < 1: raise ValueError('dim_z must be 1 or greater') if dim_u < 0: raise ValueError('dim_u must be 0 or greater') self.dim_x = dim_x self.dim_z = dim_z self.dim_u = dim_u self.x = zeros((dim_x, 1)) # state self.P_inv = eye(dim_x) # uncertainty covariance self.Q = eye(dim_x) # process uncertainty self.B = 0. # control transition matrix self._F = 0. # state transition matrix self._F_inv = 0. # state transition matrix self.H = np.zeros((dim_z, dim_x)) # Measurement function self.R_inv = eye(dim_z) # state uncertainty self.z = np.array([[None]*self.dim_z]).T # gain and residual are computed during the innovation step. We # save them so that in case you want to inspect them for various # purposes self.K = 0. # kalman gain self.y = zeros((dim_z, 1)) self.z = zeros((dim_z, 1)) self.S = 0. # system uncertainty in measurement space # identity matrix. Do not alter this. self._I = np.eye(dim_x) self._no_information = False self.compute_log_likelihood = compute_log_likelihood self.log_likelihood = math.log(sys.float_info.min) self.likelihood = sys.float_info.min self.inv = np.linalg.inv # save priors and posteriors self.x_prior = np.copy(self.x) self.P_inv_prior = np.copy(self.P_inv) self.x_post = np.copy(self.x) self.P_inv_post = np.copy(self.P_inv) def update(self, z, R_inv=None): """ Add a new measurement (z) to the kalman filter. If z is None, nothing is changed. Parameters ---------- z : np.array measurement for this update. R : np.array, scalar, or None Optionally provide R to override the measurement noise for this one call, otherwise self.R will be used. """ if z is None: self.z = None self.x_post = self.x.copy() self.P_inv_post = self.P_inv.copy() return if R_inv is None: R_inv = self.R_inv elif np.isscalar(R_inv): R_inv = eye(self.dim_z) * R_inv # rename for readability and a tiny extra bit of speed H = self.H H_T = H.T P_inv = self.P_inv x = self.x if self._no_information: self.x = dot(P_inv, x) + dot(H_T, R_inv).dot(z) self.P_inv = P_inv + dot(H_T, R_inv).dot(H) self.log_likelihood = math.log(sys.float_info.min) self.likelihood = sys.float_info.min else: # y = z - Hx # error (residual) between measurement and prediction self.y = z - dot(H, x) # S = HPH' + R # project system uncertainty into measurement space self.S = P_inv + dot(H_T, R_inv).dot(H) self.K = dot(self.inv(self.S), H_T).dot(R_inv) # x = x + Ky # predict new x with residual scaled by the kalman gain self.x = x + dot(self.K, self.y) self.P_inv = P_inv + dot(H_T, R_inv).dot(H) self.z = np.copy(reshape_z(z, self.dim_z, np.ndim(self.x))) if self.compute_log_likelihood: self.log_likelihood = logpdf(x=self.y, cov=self.S) self.likelihood = math.exp(self.log_likelihood) if self.likelihood == 0: self.likelihood = sys.float_info.min # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_inv_post = self.P_inv.copy() def predict(self, u=0): """ Predict next position. Parameters ---------- u : ndarray Optional control vector. If non-zero, it is multiplied by B to create the control input into the system. """ # x = Fx + Bu A = dot(self._F_inv.T, self.P_inv).dot(self._F_inv) #pylint: disable=bare-except try: AI = self.inv(A) invertable = True if self._no_information: try: self.x = dot(self.inv(self.P_inv), self.x) except: self.x = dot(0, self.x) self._no_information = False except: invertable = False self._no_information = True if invertable: self.x = dot(self._F, self.x) + dot(self.B, u) self.P_inv = self.inv(AI + self.Q) # save priors self.P_inv_prior = np.copy(self.P_inv) self.x_prior = np.copy(self.x) else: I_PF = self._I - dot(self.P_inv, self._F_inv) FTI = self.inv(self._F.T) FTIX = dot(FTI, self.x) AQI = self.inv(A + self.Q) self.x = dot(FTI, dot(I_PF, AQI).dot(FTIX)) # save priors self.x_prior = np.copy(self.x) self.P_inv_prior = np.copy(AQI) def batch_filter(self, zs, Rs=None, update_first=False, saver=None): """ Batch processes a sequences of measurements. Parameters ---------- zs : list-like list of measurements at each time step `self.dt` Missing measurements must be represented by 'None'. Rs : list-like, optional optional list of values to use for the measurement error covariance; a value of None in any position will cause the filter to use `self.R` for that time step. update_first : bool, optional, controls whether the order of operations is update followed by predict, or predict followed by update. Default is predict->update. saver : filterpy.common.Saver, optional filterpy.common.Saver object. If provided, saver.save() will be called after every epoch Returns ------- means: np.array((n,dim_x,1)) array of the state for each time step. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance: np.array((n,dim_x,dim_x)) array of the covariances for each time step. In other words `covariance[k,:,:]` is the covariance at step `k`. """ raise NotImplementedError("this is not implemented yet") #pylint: disable=unreachable, no-member # this is a copy of the code from kalman_filter, it has not been # turned into the information filter yet. DO NOT USE. n = np.size(zs, 0) if Rs is None: Rs = [None] * n # mean estimates from Kalman Filter means = zeros((n, self.dim_x, 1)) # state covariances from Kalman Filter covariances = zeros((n, self.dim_x, self.dim_x)) if update_first: for i, (z, r) in enumerate(zip(zs, Rs)): self.update(z, r) means[i, :] = self.x covariances[i, :, :] = self._P self.predict() if saver is not None: saver.save() else: for i, (z, r) in enumerate(zip(zs, Rs)): self.predict() self.update(z, r) means[i, :] = self.x covariances[i, :, :] = self._P if saver is not None: saver.save() return (means, covariances) @property def F(self): """State Transition matrix""" return self._F @F.setter def F(self, value): """State Transition matrix""" self._F = value self._F_inv = self.inv(self._F) @property def P(self): """State covariance matrix""" return self.inv(self.P_inv) def __repr__(self): return '\n'.join([ 'InformationFilter object', pretty_str('dim_x', self.dim_x), pretty_str('dim_z', self.dim_z), pretty_str('dim_u', self.dim_u), pretty_str('x', self.x), pretty_str('P_inv', self.P_inv), pretty_str('x_prior', self.x_prior), pretty_str('P_inv_prior', self.P_inv_prior), pretty_str('F', self.F), pretty_str('_F_inv', self._F_inv), pretty_str('Q', self.Q), pretty_str('R_inv', self.R_inv), pretty_str('H', self.H), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('z', self.z), pretty_str('S', self.S), pretty_str('B', self.B), pretty_str('log-likelihood', self.log_likelihood), pretty_str('likelihood', self.likelihood), pretty_str('inv', self.inv) ]) filterpy-1.4.5/filterpy/kalman/kalman_filter.py000066400000000000000000001533711335747705400216730ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-arguments, too-many-branches, # pylint: disable=too-many-locals, too-many-instance-attributes, too-many-lines """ This module implements the linear Kalman filter in both an object oriented and procedural form. The KalmanFilter class implements the filter by storing the various matrices in instance variables, minimizing the amount of bookkeeping you have to do. All Kalman filters operate with a predict->update cycle. The predict step, implemented with the method or function predict(), uses the state transition matrix F to predict the state in the next time period (epoch). The state is stored as a gaussian (x, P), where x is the state (column) vector, and P is its covariance. Covariance matrix Q specifies the process covariance. In Bayesian terms, this prediction is called the *prior*, which you can think of colloquially as the estimate prior to incorporating the measurement. The update step, implemented with the method or function `update()`, incorporates the measurement z with covariance R, into the state estimate (x, P). The class stores the system uncertainty in S, the innovation (residual between prediction and measurement in measurement space) in y, and the Kalman gain in k. The procedural form returns these variables to you. In Bayesian terms this computes the *posterior* - the estimate after the information from the measurement is incorporated. Whether you use the OO form or procedural form is up to you. If matrices such as H, R, and F are changing each epoch, you'll probably opt to use the procedural form. If they are unchanging, the OO form is perhaps easier to use since you won't need to keep track of these matrices. This is especially useful if you are implementing banks of filters or comparing various KF designs for performance; a trivial coding bug could lead to using the wrong sets of matrices. This module also offers an implementation of the RTS smoother, and other helper functions, such as log likelihood computations. The Saver class allows you to easily save the state of the KalmanFilter class after every update This module expects NumPy arrays for all values that expect arrays, although in a few cases, particularly method parameters, it will accept types that convert to NumPy arrays, such as lists of lists. These exceptions are documented in the method or function. Examples -------- The following example constructs a constant velocity kinematic filter, filters noisy data, and plots the results. It also demonstrates using the Saver class to save the state of the filter at each epoch. .. code-block:: Python import matplotlib.pyplot as plt import numpy as np from filterpy.kalman import KalmanFilter from filterpy.common import Q_discrete_white_noise, Saver r_std, q_std = 2., 0.003 cv = KalmanFilter(dim_x=2, dim_z=1) cv.x = np.array([[0., 1.]]) # position, velocity cv.F = np.array([[1, dt],[ [0, 1]]) cv.R = np.array([[r_std^^2]]) f.H = np.array([[1., 0.]]) f.P = np.diag([.1^^2, .03^^2) f.Q = Q_discrete_white_noise(2, dt, q_std**2) saver = Saver(cv) for z in range(100): cv.predict() cv.update([z + randn() * r_std]) saver.save() # save the filter's state saver.to_array() plt.plot(saver.x[:, 0]) # plot all of the priors plt.plot(saver.x_prior[:, 0]) # plot mahalanobis distance plt.figure() plt.plot(saver.mahalanobis) This code implements the same filter using the procedural form x = np.array([[0., 1.]]) # position, velocity F = np.array([[1, dt],[ [0, 1]]) R = np.array([[r_std^^2]]) H = np.array([[1., 0.]]) P = np.diag([.1^^2, .03^^2) Q = Q_discrete_white_noise(2, dt, q_std**2) for z in range(100): x, P = predict(x, P, F=F, Q=Q) x, P = update(x, P, z=[z + randn() * r_std], R=R, H=H) xs.append(x[0, 0]) plt.plot(xs) For more examples see the test subdirectory, or refer to the book cited below. In it I both teach Kalman filtering from basic principles, and teach the use of this library in great detail. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. Copyright 2014-2018 Roger R Labbe Jr. """ from __future__ import absolute_import, division from copy import deepcopy from math import log, exp, sqrt import sys import warnings import numpy as np from numpy import dot, zeros, eye, isscalar, shape import numpy.linalg as linalg from filterpy.stats import logpdf from filterpy.common import pretty_str, reshape_z class KalmanFilter(object): r""" Implements a Kalman filter. You are responsible for setting the various state variables to reasonable values; the defaults will not give you a functional filter. For now the best documentation is my free book Kalman and Bayesian Filters in Python [2]_. The test files in this directory also give you a basic idea of use, albeit without much description. In brief, you will first construct this object, specifying the size of the state vector with dim_x and the size of the measurement vector that you will be using with dim_z. These are mostly used to perform size checks when you assign values to the various matrices. For example, if you specified dim_z=2 and then try to assign a 3x3 matrix to R (the measurement noise matrix you will get an assert exception because R should be 2x2. (If for whatever reason you need to alter the size of things midstream just use the underscore version of the matrices to assign directly: your_filter._R = a_3x3_matrix.) After construction the filter will have default matrices created for you, but you must specify the values for each. It’s usually easiest to just overwrite them rather than assign to each element yourself. This will be clearer in the example below. All are of type numpy.array. Examples -------- Here is a filter that tracks position and velocity using a sensor that only reads position. First construct the object with the required dimensionality. .. code:: from filterpy.kalman import KalmanFilter f = KalmanFilter (dim_x=2, dim_z=1) Assign the initial value for the state (position and velocity). You can do this with a two dimensional array like so: .. code:: f.x = np.array([[2.], # position [0.]]) # velocity or just use a one dimensional array, which I prefer doing. .. code:: f.x = np.array([2., 0.]) Define the state transition matrix: .. code:: f.F = np.array([[1.,1.], [0.,1.]]) Define the measurement function: .. code:: f.H = np.array([[1.,0.]]) Define the covariance matrix. Here I take advantage of the fact that P already contains np.eye(dim_x), and just multiply by the uncertainty: .. code:: f.P *= 1000. I could have written: .. code:: f.P = np.array([[1000., 0.], [ 0., 1000.] ]) You decide which is more readable and understandable. Now assign the measurement noise. Here the dimension is 1x1, so I can use a scalar .. code:: f.R = 5 I could have done this instead: .. code:: f.R = np.array([[5.]]) Note that this must be a 2 dimensional array, as must all the matrices. Finally, I will assign the process noise. Here I will take advantage of another FilterPy library function: .. code:: from filterpy.common import Q_discrete_white_noise f.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13) Now just perform the standard predict/update loop: while some_condition_is_true: .. code:: z = get_sensor_reading() f.predict() f.update(z) do_something_with_estimate (f.x) **Procedural Form** This module also contains stand alone functions to perform Kalman filtering. Use these if you are not a fan of objects. **Example** .. code:: while True: z, R = read_sensor() x, P = predict(x, P, F, Q) x, P = update(x, P, z, R, H) See my book Kalman and Bayesian Filters in Python [2]_. You will have to set the following attributes after constructing this object for the filter to perform properly. Please note that there are various checks in place to ensure that you have made everything the 'correct' size. However, it is possible to provide incorrectly sized arrays such that the linear algebra can not perform an operation. It can also fail silently - you can end up with matrices of a size that allows the linear algebra to work, but are the wrong shape for the problem you are trying to solve. Parameters ---------- dim_x : int Number of state variables for the Kalman filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. This is used to set the default size of P, Q, and u dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. dim_u : int (optional) size of the control input, if it is being used. Default value of 0 indicates it is not used. compute_log_likelihood : bool (default = True) Computes log likelihood by default, but this can be a slow computation, so if you never use it you can turn this computation off. Attributes ---------- x : numpy.array(dim_x, 1) Current state estimate. Any call to update() or predict() updates this variable. P : numpy.array(dim_x, dim_x) Current state covariance matrix. Any call to update() or predict() updates this variable. x_prior : numpy.array(dim_x, 1) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_prior : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x, 1) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only. z : numpy.array Last measurement used in update(). Read only. R : numpy.array(dim_z, dim_z) Measurement noise matrix Q : numpy.array(dim_x, dim_x) Process noise matrix F : numpy.array() State Transition matrix H : numpy.array(dim_z, dim_x) Measurement function y : numpy.array Residual of the update step. Read only. K : numpy.array(dim_x, dim_z) Kalman gain of the update step. Read only. S : numpy.array System uncertainty (P projected to measurement space). Read only. SI : numpy.array Inverse system uncertainty. Read only. log_likelihood : float log-likelihood of the last measurement. Read only. likelihood : float likelihood of last measurement. Read only. Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. mahalanobis : float mahalanobis distance of the innovation. Read only. inv : function, default numpy.linalg.inv If you prefer another inverse function, such as the Moore-Penrose pseudo inverse, set it to that instead: kf.inv = np.linalg.pinv This is only used to invert self.S. If you know it is diagonal, you might choose to set it to filterpy.common.inv_diagonal, which is several times faster than numpy.linalg.inv for diagonal matrices. alpha : float Fading memory setting. 1.0 gives the normal Kalman filter, and values slightly larger than 1.0 (such as 1.02) give a fading memory effect - previous measurements have less influence on the filter's estimates. This formulation of the Fading memory filter (there are many) is due to Dan Simon [1]_. References ---------- .. [1] Dan Simon. "Optimal State Estimation." John Wiley & Sons. p. 208-212. (2006) .. [2] Roger Labbe. "Kalman and Bayesian Filters in Python" https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python """ def __init__(self, dim_x, dim_z, dim_u=0): if dim_x < 1: raise ValueError('dim_x must be 1 or greater') if dim_z < 1: raise ValueError('dim_z must be 1 or greater') if dim_u < 0: raise ValueError('dim_u must be 0 or greater') self.dim_x = dim_x self.dim_z = dim_z self.dim_u = dim_u self.x = zeros((dim_x, 1)) # state self.P = eye(dim_x) # uncertainty covariance self.Q = eye(dim_x) # process uncertainty self.B = None # control transition matrix self.F = eye(dim_x) # state transition matrix self.H = zeros((dim_z, dim_x)) # Measurement function self.R = eye(dim_z) # state uncertainty self._alpha_sq = 1. # fading memory control self.M = np.zeros((dim_z, dim_z)) # process-measurement cross correlation self.z = np.array([[None]*self.dim_z]).T # gain and residual are computed during the innovation step. We # save them so that in case you want to inspect them for various # purposes self.K = np.zeros((dim_x, dim_z)) # kalman gain self.y = zeros((dim_z, 1)) self.S = np.zeros((dim_z, dim_z)) # system uncertainty self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty # identity matrix. Do not alter this. self._I = np.eye(dim_x) # these will always be a copy of x,P after predict() is called self.x_prior = self.x.copy() self.P_prior = self.P.copy() # these will always be a copy of x,P after update() is called self.x_post = self.x.copy() self.P_post = self.P.copy() # Only computed only if requested via property self._log_likelihood = log(sys.float_info.min) self._likelihood = sys.float_info.min self._mahalanobis = None self.inv = np.linalg.inv def predict(self, u=None, B=None, F=None, Q=None): """ Predict next state (prior) using the Kalman filter state propagation equations. Parameters ---------- u : np.array Optional control vector. If not `None`, it is multiplied by B to create the control input into the system. B : np.array(dim_x, dim_z), or None Optional control transition matrix; a value of None will cause the filter to use `self.B`. F : np.array(dim_x, dim_x), or None Optional state transition matrix; a value of None will cause the filter to use `self.F`. Q : np.array(dim_x, dim_x), scalar, or None Optional process noise matrix; a value of None will cause the filter to use `self.Q`. """ if B is None: B = self.B if F is None: F = self.F if Q is None: Q = self.Q elif isscalar(Q): Q = eye(self.dim_x) * Q # x = Fx + Bu if B is not None and u is not None: self.x = dot(F, self.x) + dot(B, u) else: self.x = dot(F, self.x) # P = FPF' + Q self.P = self._alpha_sq * dot(dot(F, self.P), F.T) + Q # save prior self.x_prior = self.x.copy() self.P_prior = self.P.copy() def update(self, z, R=None, H=None): """ Add a new measurement (z) to the Kalman filter. If z is None, nothing is computed. However, x_post and P_post are updated with the prior (x_prior, P_prior), and self.z is set to None. Parameters ---------- z : (dim_z, 1): array_like measurement for this update. z can be a scalar if dim_z is 1, otherwise it must be convertible to a column vector. R : np.array, scalar, or None Optionally provide R to override the measurement noise for this one call, otherwise self.R will be used. H : np.array, or None Optionally provide H to override the measurement function for this one call, otherwise self.H will be used. """ # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None if z is None: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() self.y = zeros((self.dim_z, 1)) return z = reshape_z(z, self.dim_z, self.x.ndim) if R is None: R = self.R elif isscalar(R): R = eye(self.dim_z) * R if H is None: H = self.H # y = z - Hx # error (residual) between measurement and prediction self.y = z - dot(H, self.x) # common subexpression for speed PHT = dot(self.P, H.T) # S = HPH' + R # project system uncertainty into measurement space self.S = dot(H, PHT) + R self.SI = self.inv(self.S) # K = PH'inv(S) # map system uncertainty into kalman gain self.K = dot(PHT, self.SI) # x = x + Ky # predict new x with residual scaled by the kalman gain self.x = self.x + dot(self.K, self.y) # P = (I-KH)P(I-KH)' + KRK' # This is more numerically stable # and works for non-optimal K vs the equation # P = (I-KH)P usually seen in the literature. I_KH = self._I - dot(self.K, H) self.P = dot(dot(I_KH, self.P), I_KH.T) + dot(dot(self.K, R), self.K.T) # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() def predict_steadystate(self, u=0, B=None): """ Predict state (prior) using the Kalman filter state propagation equations. Only x is updated, P is left unchanged. See update_steadstate() for a longer explanation of when to use this method. Parameters ---------- u : np.array Optional control vector. If non-zero, it is multiplied by B to create the control input into the system. B : np.array(dim_x, dim_z), or None Optional control transition matrix; a value of None will cause the filter to use `self.B`. """ if B is None: B = self.B # x = Fx + Bu if B is not None: self.x = dot(self.F, self.x) + dot(B, u) else: self.x = dot(self.F, self.x) # save prior self.x_prior = self.x.copy() self.P_prior = self.P.copy() def update_steadystate(self, z): """ Add a new measurement (z) to the Kalman filter without recomputing the Kalman gain K, the state covariance P, or the system uncertainty S. You can use this for LTI systems since the Kalman gain and covariance converge to a fixed value. Precompute these and assign them explicitly, or run the Kalman filter using the normal predict()/update(0 cycle until they converge. The main advantage of this call is speed. We do significantly less computation, notably avoiding a costly matrix inversion. Use in conjunction with predict_steadystate(), otherwise P will grow without bound. Parameters ---------- z : (dim_z, 1): array_like measurement for this update. z can be a scalar if dim_z is 1, otherwise it must be convertible to a column vector. Examples -------- >>> cv = kinematic_kf(dim=3, order=2) # 3D const velocity filter >>> # let filter converge on representative data, then save k and P >>> for i in range(100): >>> cv.predict() >>> cv.update([i, i, i]) >>> saved_k = np.copy(cv.K) >>> saved_P = np.copy(cv.P) later on: >>> cv = kinematic_kf(dim=3, order=2) # 3D const velocity filter >>> cv.K = np.copy(saved_K) >>> cv.P = np.copy(saved_P) >>> for i in range(100): >>> cv.predict_steadystate() >>> cv.update_steadystate([i, i, i]) """ # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None if z is None: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() self.y = zeros((self.dim_z, 1)) return z = reshape_z(z, self.dim_z, self.x.ndim) # y = z - Hx # error (residual) between measurement and prediction self.y = z - dot(self.H, self.x) # x = x + Ky # predict new x with residual scaled by the kalman gain self.x = self.x + dot(self.K, self.y) self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None def update_correlated(self, z, R=None, H=None): """ Add a new measurement (z) to the Kalman filter assuming that process noise and measurement noise are correlated as defined in the `self.M` matrix. If z is None, nothing is changed. Parameters ---------- z : (dim_z, 1): array_like measurement for this update. z can be a scalar if dim_z is 1, otherwise it must be convertible to a column vector. R : np.array, scalar, or None Optionally provide R to override the measurement noise for this one call, otherwise self.R will be used. H : np.array, or None Optionally provide H to override the measurement function for this one call, otherwise self.H will be used. """ # set to None to force recompute self._log_likelihood = None self._likelihood = None self._mahalanobis = None if z is None: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self.P_post = self.P.copy() self.y = zeros((self.dim_z, 1)) return z = reshape_z(z, self.dim_z, self.x.ndim) if R is None: R = self.R elif isscalar(R): R = eye(self.dim_z) * R # rename for readability and a tiny extra bit of speed if H is None: H = self.H # handle special case: if z is in form [[z]] but x is not a column # vector dimensions will not match if self.x.ndim == 1 and shape(z) == (1, 1): z = z[0] if shape(z) == (): # is it scalar, e.g. z=3 or z=np.array(3) z = np.asarray([z]) # y = z - Hx # error (residual) between measurement and prediction self.y = z - dot(H, self.x) # common subexpression for speed PHT = dot(self.P, H.T) # project system uncertainty into measurement space self.S = dot(H, PHT) + dot(H, self.M) + dot(self.M.T, H.T) + R self.SI = self.inv(self.S) # K = PH'inv(S) # map system uncertainty into kalman gain self.K = dot(PHT + self.M, self.SI) # x = x + Ky # predict new x with residual scaled by the kalman gain self.x = self.x + dot(self.K, self.y) self.P = self.P - dot(self.K, dot(H, self.P) + self.M.T) self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() def batch_filter(self, zs, Fs=None, Qs=None, Hs=None, Rs=None, Bs=None, us=None, update_first=False, saver=None): """ Batch processes a sequences of measurements. Parameters ---------- zs : list-like list of measurements at each time step `self.dt`. Missing measurements must be represented by `None`. Fs : None, list-like, default=None optional value or list of values to use for the state transition matrix F. If Fs is None then self.F is used for all epochs. Otherwise it must contain a list-like list of F's, one for each epoch. This allows you to have varying F per epoch. Qs : None, np.array or list-like, default=None optional value or list of values to use for the process error covariance Q. If Qs is None then self.Q is used for all epochs. Otherwise it must contain a list-like list of Q's, one for each epoch. This allows you to have varying Q per epoch. Hs : None, np.array or list-like, default=None optional list of values to use for the measurement matrix H. If Hs is None then self.H is used for all epochs. If Hs contains a single matrix, then it is used as H for all epochs. Otherwise it must contain a list-like list of H's, one for each epoch. This allows you to have varying H per epoch. Rs : None, np.array or list-like, default=None optional list of values to use for the measurement error covariance R. If Rs is None then self.R is used for all epochs. Otherwise it must contain a list-like list of R's, one for each epoch. This allows you to have varying R per epoch. Bs : None, np.array or list-like, default=None optional list of values to use for the control transition matrix B. If Bs is None then self.B is used for all epochs. Otherwise it must contain a list-like list of B's, one for each epoch. This allows you to have varying B per epoch. us : None, np.array or list-like, default=None optional list of values to use for the control input vector; If us is None then None is used for all epochs (equivalent to 0, or no control input). Otherwise it must contain a list-like list of u's, one for each epoch. update_first : bool, optional, default=False controls whether the order of operations is update followed by predict, or predict followed by update. Default is predict->update. saver : filterpy.common.Saver, optional filterpy.common.Saver object. If provided, saver.save() will be called after every epoch Returns ------- means : np.array((n,dim_x,1)) array of the state for each time step after the update. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance : np.array((n,dim_x,dim_x)) array of the covariances for each time step after the update. In other words `covariance[k,:,:]` is the covariance at step `k`. means_predictions : np.array((n,dim_x,1)) array of the state for each time step after the predictions. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance_predictions : np.array((n,dim_x,dim_x)) array of the covariances for each time step after the prediction. In other words `covariance[k,:,:]` is the covariance at step `k`. Examples -------- .. code-block:: Python # this example demonstrates tracking a measurement where the time # between measurement varies, as stored in dts. This requires # that F be recomputed for each epoch. The output is then smoothed # with an RTS smoother. zs = [t + random.randn()*4 for t in range (40)] Fs = [np.array([[1., dt], [0, 1]] for dt in dts] (mu, cov, _, _) = kf.batch_filter(zs, Fs=Fs) (xs, Ps, Ks) = kf.rts_smoother(mu, cov, Fs=Fs) """ #pylint: disable=too-many-statements n = np.size(zs, 0) if Fs is None: Fs = [self.F] * n if Qs is None: Qs = [self.Q] * n if Hs is None: Hs = [self.H] * n if Rs is None: Rs = [self.R] * n if Bs is None: Bs = [self.B] * n if us is None: us = [0] * n # mean estimates from Kalman Filter if self.x.ndim == 1: means = zeros((n, self.dim_x)) means_p = zeros((n, self.dim_x)) else: means = zeros((n, self.dim_x, 1)) means_p = zeros((n, self.dim_x, 1)) # state covariances from Kalman Filter covariances = zeros((n, self.dim_x, self.dim_x)) covariances_p = zeros((n, self.dim_x, self.dim_x)) if update_first: for i, (z, F, Q, H, R, B, u) in enumerate(zip(zs, Fs, Qs, Hs, Rs, Bs, us)): self.update(z, R=R, H=H) means[i, :] = self.x covariances[i, :, :] = self.P self.predict(u=u, B=B, F=F, Q=Q) means_p[i, :] = self.x covariances_p[i, :, :] = self.P if saver is not None: saver.save() else: for i, (z, F, Q, H, R, B, u) in enumerate(zip(zs, Fs, Qs, Hs, Rs, Bs, us)): self.predict(u=u, B=B, F=F, Q=Q) means_p[i, :] = self.x covariances_p[i, :, :] = self.P self.update(z, R=R, H=H) means[i, :] = self.x covariances[i, :, :] = self.P if saver is not None: saver.save() return (means, covariances, means_p, covariances_p) def rts_smoother(self, Xs, Ps, Fs=None, Qs=None, inv=np.linalg.inv): """ Runs the Rauch-Tung-Striebal Kalman smoother on a set of means and covariances computed by a Kalman filter. The usual input would come from the output of `KalmanFilter.batch_filter()`. Parameters ---------- Xs : numpy.array array of the means (state variable x) of the output of a Kalman filter. Ps : numpy.array array of the covariances of the output of a kalman filter. Fs : list-like collection of numpy.array, optional State transition matrix of the Kalman filter at each time step. Optional, if not provided the filter's self.F will be used Qs : list-like collection of numpy.array, optional Process noise of the Kalman filter at each time step. Optional, if not provided the filter's self.Q will be used inv : function, default numpy.linalg.inv If you prefer another inverse function, such as the Moore-Penrose pseudo inverse, set it to that instead: kf.inv = np.linalg.pinv Returns ------- x : numpy.ndarray smoothed means P : numpy.ndarray smoothed state covariances K : numpy.ndarray smoother gain at each step Pp : numpy.ndarray Predicted state covariances Examples -------- .. code-block:: Python zs = [t + random.randn()*4 for t in range (40)] (mu, cov, _, _) = kalman.batch_filter(zs) (x, P, K, Pp) = rts_smoother(mu, cov, kf.F, kf.Q) """ if len(Xs) != len(Ps): raise ValueError('length of Xs and Ps must be the same') n = Xs.shape[0] dim_x = Xs.shape[1] if Fs is None: Fs = [self.F] * n if Qs is None: Qs = [self.Q] * n # smoother gain K = zeros((n, dim_x, dim_x)) x, P, Pp = Xs.copy(), Ps.copy(), Ps.copy() for k in range(n-2, -1, -1): Pp[k] = dot(dot(Fs[k+1], P[k]), Fs[k+1].T) + Qs[k+1] #pylint: disable=bad-whitespace K[k] = dot(dot(P[k], Fs[k+1].T), inv(Pp[k])) x[k] += dot(K[k], x[k+1] - dot(Fs[k+1], x[k])) P[k] += dot(dot(K[k], P[k+1] - Pp[k]), K[k].T) return (x, P, K, Pp) def get_prediction(self, u=0): """ Predicts the next state of the filter and returns it without altering the state of the filter. Parameters ---------- u : np.array optional control input Returns ------- (x, P) : tuple State vector and covariance array of the prediction. """ x = dot(self.F, self.x) + dot(self.B, u) P = self._alpha_sq * dot(dot(self.F, self.P), self.F.T) + self.Q return (x, P) def get_update(self, z=None): """ Computes the new estimate based on measurement `z` and returns it without altering the state of the filter. Parameters ---------- z : (dim_z, 1): array_like measurement for this update. z can be a scalar if dim_z is 1, otherwise it must be convertible to a column vector. Returns ------- (x, P) : tuple State vector and covariance array of the update. """ if z is None: return self.x, self.P z = reshape_z(z, self.dim_z, self.x.ndim) R = self.R H = self.H P = self.P x = self.x # error (residual) between measurement and prediction y = z - dot(H, x) # common subexpression for speed PHT = dot(P, H.T) # project system uncertainty into measurement space S = dot(H, PHT) + R # map system uncertainty into kalman gain K = dot(PHT, self.inv(S)) # predict new x with residual scaled by the kalman gain x = x + dot(K, y) # P = (I-KH)P(I-KH)' + KRK' I_KH = self._I - dot(K, H) P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T) return x, P def residual_of(self, z): """ Returns the residual for the given measurement (z). Does not alter the state of the filter. """ return z - dot(self.H, self.x_prior) def measurement_of_state(self, x): """ Helper function that converts a state into a measurement. Parameters ---------- x : np.array kalman state vector Returns ------- z : (dim_z, 1): array_like measurement for this update. z can be a scalar if dim_z is 1, otherwise it must be convertible to a column vector. """ return dot(self.H, x) @property def log_likelihood(self): """ log-likelihood of the last measurement. """ if self._log_likelihood is None: self._log_likelihood = logpdf(x=self.y, cov=self.S) return self._log_likelihood @property def likelihood(self): """ Computed from the log-likelihood. The log-likelihood can be very small, meaning a large negative value such as -28000. Taking the exp() of that results in 0.0, which can break typical algorithms which multiply by this value, so by default we always return a number >= sys.float_info.min. """ if self._likelihood is None: self._likelihood = exp(self.log_likelihood) if self._likelihood == 0: self._likelihood = sys.float_info.min return self._likelihood @property def mahalanobis(self): """" Mahalanobis distance of measurement. E.g. 3 means measurement was 3 standard deviations away from the predicted value. Returns ------- mahalanobis : float """ if self._mahalanobis is None: self._mahalanobis = sqrt(float(dot(dot(self.y.T, self.SI), self.y))) return self._mahalanobis @property def alpha(self): """ Fading memory setting. 1.0 gives the normal Kalman filter, and values slightly larger than 1.0 (such as 1.02) give a fading memory effect - previous measurements have less influence on the filter's estimates. This formulation of the Fading memory filter (there are many) is due to Dan Simon [1]_. """ return self._alpha_sq**.5 def log_likelihood_of(self, z): """ log likelihood of the measurement `z`. This should only be called after a call to update(). Calling after predict() will yield an incorrect result.""" if z is None: return log(sys.float_info.min) return logpdf(z, dot(self.H, self.x), self.S) @alpha.setter def alpha(self, value): if not np.isscalar(value) or value < 1: raise ValueError('alpha must be a float greater than 1') self._alpha_sq = value**2 def __repr__(self): return '\n'.join([ 'KalmanFilter object', pretty_str('dim_x', self.dim_x), pretty_str('dim_z', self.dim_z), pretty_str('dim_u', self.dim_u), pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('x_prior', self.x_prior), pretty_str('P_prior', self.P_prior), pretty_str('x_post', self.x_post), pretty_str('P_post', self.P_post), pretty_str('F', self.F), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('H', self.H), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('S', self.S), pretty_str('SI', self.SI), pretty_str('M', self.M), pretty_str('B', self.B), pretty_str('z', self.z), pretty_str('log-likelihood', self.log_likelihood), pretty_str('likelihood', self.likelihood), pretty_str('mahalanobis', self.mahalanobis), pretty_str('alpha', self.alpha), pretty_str('inv', self.inv) ]) def test_matrix_dimensions(self, z=None, H=None, R=None, F=None, Q=None): """ Performs a series of asserts to check that the size of everything is what it should be. This can help you debug problems in your design. If you pass in H, R, F, Q those will be used instead of this object's value for those matrices. Testing `z` (the measurement) is problamatic. x is a vector, and can be implemented as either a 1D array or as a nx1 column vector. Thus Hx can be of different shapes. Then, if Hx is a single value, it can be either a 1D array or 2D vector. If either is true, z can reasonably be a scalar (either '3' or np.array('3') are scalars under this definition), a 1D, 1 element array, or a 2D, 1 element array. You are allowed to pass in any combination that works. """ if H is None: H = self.H if R is None: R = self.R if F is None: F = self.F if Q is None: Q = self.Q x = self.x P = self.P assert x.ndim == 1 or x.ndim == 2, \ "x must have one or two dimensions, but has {}".format(x.ndim) if x.ndim == 1: assert x.shape[0] == self.dim_x, \ "Shape of x must be ({},{}), but is {}".format( self.dim_x, 1, x.shape) else: assert x.shape == (self.dim_x, 1), \ "Shape of x must be ({},{}), but is {}".format( self.dim_x, 1, x.shape) assert P.shape == (self.dim_x, self.dim_x), \ "Shape of P must be ({},{}), but is {}".format( self.dim_x, self.dim_x, P.shape) assert Q.shape == (self.dim_x, self.dim_x), \ "Shape of P must be ({},{}), but is {}".format( self.dim_x, self.dim_x, P.shape) assert F.shape == (self.dim_x, self.dim_x), \ "Shape of F must be ({},{}), but is {}".format( self.dim_x, self.dim_x, F.shape) assert np.ndim(H) == 2, \ "Shape of H must be (dim_z, {}), but is {}".format( P.shape[0], shape(H)) assert H.shape[1] == P.shape[0], \ "Shape of H must be (dim_z, {}), but is {}".format( P.shape[0], H.shape) # shape of R must be the same as HPH' hph_shape = (H.shape[0], H.shape[0]) r_shape = shape(R) if H.shape[0] == 1: # r can be scalar, 1D, or 2D in this case assert r_shape == () or r_shape == (1,) or r_shape == (1, 1), \ "R must be scalar or one element array, but is shaped {}".format( r_shape) else: assert r_shape == hph_shape, \ "shape of R should be {} but it is {}".format(hph_shape, r_shape) if z is not None: z_shape = shape(z) else: z_shape = (self.dim_z, 1) # H@x must have shape of z Hx = dot(H, x) if z_shape == (): # scalar or np.array(scalar) assert Hx.ndim == 1 or shape(Hx) == (1, 1), \ "shape of z should be {}, not {} for the given H".format( shape(Hx), z_shape) elif shape(Hx) == (1,): assert z_shape[0] == 1, 'Shape of z must be {} for the given H'.format(shape(Hx)) else: assert (z_shape == shape(Hx) or (len(z_shape) == 1 and shape(Hx) == (z_shape[0], 1))), \ "shape of z should be {}, not {} for the given H".format( shape(Hx), z_shape) if np.ndim(Hx) > 1 and shape(Hx) != (1, 1): assert shape(Hx) == z_shape, \ 'shape of z should be {} for the given H, but it is {}'.format( shape(Hx), z_shape) def update(x, P, z, R, H=None, return_all=False): """ Add a new measurement (z) to the Kalman filter. If z is None, nothing is changed. This can handle either the multidimensional or unidimensional case. If all parameters are floats instead of arrays the filter will still work, and return floats for x, P as the result. update(1, 2, 1, 1, 1) # univariate update(x, P, 1 Parameters ---------- x : numpy.array(dim_x, 1), or float State estimate vector P : numpy.array(dim_x, dim_x), or float Covariance matrix z : (dim_z, 1): array_like measurement for this update. z can be a scalar if dim_z is 1, otherwise it must be convertible to a column vector. R : numpy.array(dim_z, dim_z), or float Measurement noise matrix H : numpy.array(dim_x, dim_x), or float, optional Measurement function. If not provided, a value of 1 is assumed. return_all : bool, default False If true, y, K, S, and log_likelihood are returned, otherwise only x and P are returned. Returns ------- x : numpy.array Posterior state estimate vector P : numpy.array Posterior covariance matrix y : numpy.array or scalar Residua. Difference between measurement and state in measurement space K : numpy.array Kalman gain S : numpy.array System uncertainty in measurement space log_likelihood : float log likelihood of the measurement """ #pylint: disable=bare-except if z is None: if return_all: return x, P, None, None, None, None return x, P if H is None: H = np.array([1]) if np.isscalar(H): H = np.array([H]) Hx = np.atleast_1d(dot(H, x)) z = reshape_z(z, Hx.shape[0], x.ndim) # error (residual) between measurement and prediction y = z - Hx # project system uncertainty into measurement space S = dot(dot(H, P), H.T) + R # map system uncertainty into kalman gain try: K = dot(dot(P, H.T), linalg.inv(S)) except: # can't invert a 1D array, annoyingly K = dot(dot(P, H.T), 1./S) # predict new x with residual scaled by the kalman gain x = x + dot(K, y) # P = (I-KH)P(I-KH)' + KRK' KH = dot(K, H) try: I_KH = np.eye(KH.shape[0]) - KH except: I_KH = np.array([1 - KH]) P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T) if return_all: # compute log likelihood log_likelihood = logpdf(z, dot(H, x), S) return x, P, y, K, S, log_likelihood return x, P def update_steadystate(x, z, K, H=None): """ Add a new measurement (z) to the Kalman filter. If z is None, nothing is changed. Parameters ---------- x : numpy.array(dim_x, 1), or float State estimate vector z : (dim_z, 1): array_like measurement for this update. z can be a scalar if dim_z is 1, otherwise it must be convertible to a column vector. K : numpy.array, or float Kalman gain matrix H : numpy.array(dim_x, dim_x), or float, optional Measurement function. If not provided, a value of 1 is assumed. Returns ------- x : numpy.array Posterior state estimate vector Examples -------- This can handle either the multidimensional or unidimensional case. If all parameters are floats instead of arrays the filter will still work, and return floats for x, P as the result. >>> update_steadystate(1, 2, 1) # univariate >>> update_steadystate(x, P, z, H) """ if z is None: return x if H is None: H = np.array([1]) if np.isscalar(H): H = np.array([H]) Hx = np.atleast_1d(dot(H, x)) z = reshape_z(z, Hx.shape[0], x.ndim) # error (residual) between measurement and prediction y = z - Hx # estimate new x with residual scaled by the kalman gain return x + dot(K, y) def predict(x, P, F=1, Q=0, u=0, B=1, alpha=1.): """ Predict next state (prior) using the Kalman filter state propagation equations. Parameters ---------- x : numpy.array State estimate vector P : numpy.array Covariance matrix F : numpy.array() State Transition matrix Q : numpy.array, Optional Process noise matrix u : numpy.array, Optional, default 0. Control vector. If non-zero, it is multiplied by B to create the control input into the system. B : numpy.array, optional, default 0. Control transition matrix. alpha : float, Optional, default=1.0 Fading memory setting. 1.0 gives the normal Kalman filter, and values slightly larger than 1.0 (such as 1.02) give a fading memory effect - previous measurements have less influence on the filter's estimates. This formulation of the Fading memory filter (there are many) is due to Dan Simon Returns ------- x : numpy.array Prior state estimate vector P : numpy.array Prior covariance matrix """ if np.isscalar(F): F = np.array(F) x = dot(F, x) + dot(B, u) P = (alpha * alpha) * dot(dot(F, P), F.T) + Q return x, P def predict_steadystate(x, F=1, u=0, B=1): """ Predict next state (prior) using the Kalman filter state propagation equations. This steady state form only computes x, assuming that the covariance is constant. Parameters ---------- x : numpy.array State estimate vector P : numpy.array Covariance matrix F : numpy.array() State Transition matrix u : numpy.array, Optional, default 0. Control vector. If non-zero, it is multiplied by B to create the control input into the system. B : numpy.array, optional, default 0. Control transition matrix. Returns ------- x : numpy.array Prior state estimate vector """ if np.isscalar(F): F = np.array(F) x = dot(F, x) + dot(B, u) return x def batch_filter(x, P, zs, Fs, Qs, Hs, Rs, Bs=None, us=None, update_first=False, saver=None): """ Batch processes a sequences of measurements. Parameters ---------- zs : list-like list of measurements at each time step. Missing measurements must be represented by None. Fs : list-like list of values to use for the state transition matrix matrix. Qs : list-like list of values to use for the process error covariance. Hs : list-like list of values to use for the measurement matrix. Rs : list-like list of values to use for the measurement error covariance. Bs : list-like, optional list of values to use for the control transition matrix; a value of None in any position will cause the filter to use `self.B` for that time step. us : list-like, optional list of values to use for the control input vector; a value of None in any position will cause the filter to use 0 for that time step. update_first : bool, optional controls whether the order of operations is update followed by predict, or predict followed by update. Default is predict->update. saver : filterpy.common.Saver, optional filterpy.common.Saver object. If provided, saver.save() will be called after every epoch Returns ------- means : np.array((n,dim_x,1)) array of the state for each time step after the update. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance : np.array((n,dim_x,dim_x)) array of the covariances for each time step after the update. In other words `covariance[k,:,:]` is the covariance at step `k`. means_predictions : np.array((n,dim_x,1)) array of the state for each time step after the predictions. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance_predictions : np.array((n,dim_x,dim_x)) array of the covariances for each time step after the prediction. In other words `covariance[k,:,:]` is the covariance at step `k`. Examples -------- .. code-block:: Python zs = [t + random.randn()*4 for t in range (40)] Fs = [kf.F for t in range (40)] Hs = [kf.H for t in range (40)] (mu, cov, _, _) = kf.batch_filter(zs, Rs=R_list, Fs=Fs, Hs=Hs, Qs=None, Bs=None, us=None, update_first=False) (xs, Ps, Ks) = kf.rts_smoother(mu, cov, Fs=Fs, Qs=None) """ n = np.size(zs, 0) dim_x = x.shape[0] # mean estimates from Kalman Filter if x.ndim == 1: means = zeros((n, dim_x)) means_p = zeros((n, dim_x)) else: means = zeros((n, dim_x, 1)) means_p = zeros((n, dim_x, 1)) # state covariances from Kalman Filter covariances = zeros((n, dim_x, dim_x)) covariances_p = zeros((n, dim_x, dim_x)) if us is None: us = [0.] * n Bs = [0.] * n if update_first: for i, (z, F, Q, H, R, B, u) in enumerate(zip(zs, Fs, Qs, Hs, Rs, Bs, us)): x, P = update(x, P, z, R=R, H=H) means[i, :] = x covariances[i, :, :] = P x, P = predict(x, P, u=u, B=B, F=F, Q=Q) means_p[i, :] = x covariances_p[i, :, :] = P if saver is not None: saver.save() else: for i, (z, F, Q, H, R, B, u) in enumerate(zip(zs, Fs, Qs, Hs, Rs, Bs, us)): x, P = predict(x, P, u=u, B=B, F=F, Q=Q) means_p[i, :] = x covariances_p[i, :, :] = P x, P = update(x, P, z, R=R, H=H) means[i, :] = x covariances[i, :, :] = P if saver is not None: saver.save() return (means, covariances, means_p, covariances_p) def rts_smoother(Xs, Ps, Fs, Qs): """ Runs the Rauch-Tung-Striebal Kalman smoother on a set of means and covariances computed by a Kalman filter. The usual input would come from the output of `KalmanFilter.batch_filter()`. Parameters ---------- Xs : numpy.array array of the means (state variable x) of the output of a Kalman filter. Ps : numpy.array array of the covariances of the output of a kalman filter. Fs : list-like collection of numpy.array State transition matrix of the Kalman filter at each time step. Qs : list-like collection of numpy.array, optional Process noise of the Kalman filter at each time step. Returns ------- x : numpy.ndarray smoothed means P : numpy.ndarray smoothed state covariances K : numpy.ndarray smoother gain at each step pP : numpy.ndarray predicted state covariances Examples -------- .. code-block:: Python zs = [t + random.randn()*4 for t in range (40)] (mu, cov, _, _) = kalman.batch_filter(zs) (x, P, K, pP) = rts_smoother(mu, cov, kf.F, kf.Q) """ if len(Xs) != len(Ps): raise ValueError('length of Xs and Ps must be the same') n = Xs.shape[0] dim_x = Xs.shape[1] # smoother gain K = zeros((n, dim_x, dim_x)) x, P, pP = Xs.copy(), Ps.copy(), Ps.copy() for k in range(n-2, -1, -1): pP[k] = dot(dot(Fs[k], P[k]), Fs[k].T) + Qs[k] #pylint: disable=bad-whitespace K[k] = dot(dot(P[k], Fs[k].T), linalg.inv(pP[k])) x[k] += dot(K[k], x[k+1] - dot(Fs[k], x[k])) P[k] += dot(dot(K[k], P[k+1] - pP[k]), K[k].T) return (x, P, K, pP) filterpy-1.4.5/filterpy/kalman/mmae.py000066400000000000000000000144251335747705400177760ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name,too-many-instance-attributes """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import absolute_import, division from copy import deepcopy import numpy as np from filterpy.common import pretty_str class MMAEFilterBank(object): """ Implements the fixed Multiple Model Adaptive Estimator (MMAE). This is a bank of independent Kalman filters. This estimator computes the likelihood that each filter is the correct one, and blends their state estimates weighted by their likelihood to produce the state estimate. Parameters ---------- filters : list of Kalman filters List of Kalman filters. p : list-like of floats Initial probability that each filter is the correct one. In general you'd probably set each element to 1./len(p). dim_x : float number of random variables in the state X H : Measurement matrix Attributes ---------- x : numpy.array(dim_x, 1) Current state estimate. Any call to update() or predict() updates this variable. P : numpy.array(dim_x, dim_x) Current state covariance matrix. Any call to update() or predict() updates this variable. x_prior : numpy.array(dim_x, 1) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_prior : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x, 1) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only. z : ndarray Last measurement used in update(). Read only. filters : list of Kalman filters List of Kalman filters. Examples -------- ..code: ca = make_ca_filter(dt, noise_factor=0.6) cv = make_ca_filter(dt, noise_factor=0.6) cv.F[:,2] = 0 # remove acceleration term cv.P[2,2] = 0 cv.Q[2,2] = 0 filters = [cv, ca] bank = MMAEFilterBank(filters, p=(0.5, 0.5), dim_x=3) for z in zs: bank.predict() bank.update(z) Also, see my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python References ---------- Zarchan and Musoff. "Fundamentals of Kalman filtering: A Practical Approach." AIAA, third edition. """ def __init__(self, filters, p, dim_x, H=None): if len(filters) != len(p): raise ValueError('length of filters and p must be the same') if dim_x < 1: raise ValueError('dim_x must be >= 1') self.filters = filters self.p = np.asarray(p) self.dim_x = dim_x if H is None: self.H = None else: self.H = np.copy(H) # try to form a reasonable initial values, but good luck! try: self.z = np.copy(filters[0].z) self.x = np.copy(filters[0].x) self.P = np.copy(filters[0].P) except AttributeError: self.z = 0 self.x = None self.P = None # these will always be a copy of x,P after predict() is called self.x_prior = self.x.copy() self.P_prior = self.P.copy() # these will always be a copy of x,P after update() is called self.x_post = self.x.copy() self.P_post = self.P.copy() def predict(self, u=0): """ Predict next position using the Kalman filter state propagation equations for each filter in the bank. Parameters ---------- u : np.array Optional control vector. If non-zero, it is multiplied by B to create the control input into the system. """ for f in self.filters: f.predict(u) # save prior self.x_prior = self.x.copy() self.P_prior = self.P.copy() def update(self, z, R=None, H=None): """ Add a new measurement (z) to the Kalman filter. If z is None, nothing is changed. Parameters ---------- z : np.array measurement for this update. R : np.array, scalar, or None Optionally provide R to override the measurement noise for this one call, otherwise self.R will be used. H : np.array, or None Optionally provide H to override the measurement function for this one call, otherwise self.H will be used. """ if H is None: H = self.H # new probability is recursively defined as prior * likelihood for i, f in enumerate(self.filters): f.update(z, R, H) self.p[i] *= f.likelihood self.p /= sum(self.p) # normalize # compute estimated state and covariance of the bank of filters. self.P = np.zeros(self.filters[0].P.shape) # state can be in form [x,y,z,...] or [[x, y, z,...]].T is_row_vector = (self.filters[0].x.ndim == 1) if is_row_vector: self.x = np.zeros(self.dim_x) for f, p in zip(self.filters, self.p): self.x += np.dot(f.x, p) else: self.x = np.zeros((self.dim_x, 1)) for f, p in zip(self.filters, self.p): self.x = np.zeros((self.dim_x, 1)) self.x += np.dot(f.x, p) for x, f, p in zip(self.x, self.filters, self.p): y = f.x - x self.P += p*(np.outer(y, y) + f.P) # save measurement and posterior state self.z = deepcopy(z) self.x_post = self.x.copy() self.P_post = self.P.copy() def __repr__(self): return '\n'.join([ 'MMAEFilterBank object', pretty_str('dim_x', self.dim_x), pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('log-p', self.p), ]) filterpy-1.4.5/filterpy/kalman/sigma_points.py000066400000000000000000000375761335747705400215670ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-instance-attributes """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import division import numpy as np from scipy.linalg import cholesky from filterpy.common import pretty_str class MerweScaledSigmaPoints(object): """ Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1] for the UnscentedKalmanFilter class.. It parametizes the sigma points using alpha, beta, kappa terms, and is the version seen in most publications. Unless you know better, this should be your default choice. Parameters ---------- n : int Dimensionality of the state. 2n+1 weights will be generated. alpha : float Determins the spread of the sigma points around the mean. Usually a small positive value (1e-3) according to [3]. beta : float Incorporates prior knowledge of the distribution of the mean. For Gaussian x beta=2 is optimal, according to [3]. kappa : float, default=0.0 Secondary scaling parameter usually set to 0 according to [4], or to 3-n according to [5]. sqrt_method : function(ndarray), default=scipy.linalg.cholesky Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe's dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you. If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing. subtract : callable (x, y), optional Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. Attributes ---------- Wm : np.array weight for each sigma point for the mean Wc : np.array weight for each sigma point for the covariance Examples -------- See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python References ---------- .. [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models" (Doctoral dissertation) """ def __init__(self, n, alpha, beta, kappa, sqrt_method=None, subtract=None): #pylint: disable=too-many-arguments self.n = n self.alpha = alpha self.beta = beta self.kappa = kappa if sqrt_method is None: self.sqrt = cholesky else: self.sqrt = sqrt_method if subtract is None: self.subtract = np.subtract else: self.subtract = subtract self._compute_weights() def num_sigmas(self): """ Number of sigma points for each variable in the state x""" return 2*self.n + 1 def sigma_points(self, x, P): """ Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. Returns tuple of the sigma points and weights. Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I Parameters ---------- x : An array-like object of the means of length n Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2]) P : scalar, or np.array Covariance of the filter. If scalar, is treated as eye(n)*P. Returns ------- sigmas : np.array, of size (n, 2n+1) Two dimensional array of sigma points. Each column contains all of the sigmas for one dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n} """ if self.n != np.size(x): raise ValueError("expected size(x) {}, but size is {}".format( self.n, np.size(x))) n = self.n if np.isscalar(x): x = np.asarray([x]) if np.isscalar(P): P = np.eye(n)*P else: P = np.atleast_2d(P) lambda_ = self.alpha**2 * (n + self.kappa) - n U = self.sqrt((lambda_ + n)*P) sigmas = np.zeros((2*n+1, n)) sigmas[0] = x for k in range(n): # pylint: disable=bad-whitespace sigmas[k+1] = self.subtract(x, -U[k]) sigmas[n+k+1] = self.subtract(x, U[k]) return sigmas def _compute_weights(self): """ Computes the weights for the scaled unscented Kalman filter. """ n = self.n lambda_ = self.alpha**2 * (n +self.kappa) - n c = .5 / (n + lambda_) self.Wc = np.full(2*n + 1, c) self.Wm = np.full(2*n + 1, c) self.Wc[0] = lambda_ / (n + lambda_) + (1 - self.alpha**2 + self.beta) self.Wm[0] = lambda_ / (n + lambda_) def __repr__(self): return '\n'.join([ 'MerweScaledSigmaPoints object', pretty_str('n', self.n), pretty_str('alpha', self.alpha), pretty_str('beta', self.beta), pretty_str('kappa', self.kappa), pretty_str('Wm', self.Wm), pretty_str('Wc', self.Wc), pretty_str('subtract', self.subtract), pretty_str('sqrt', self.sqrt) ]) class JulierSigmaPoints(object): """ Generates sigma points and weights according to Simon J. Julier and Jeffery K. Uhlmann's original paper[1]. It parametizes the sigma points using kappa. Parameters ---------- n : int Dimensionality of the state. 2n+1 weights will be generated. kappa : float, default=0. Scaling factor that can reduce high order errors. kappa=0 gives the standard unscented filter. According to [Julier], if you set kappa to 3-dim_x for a Gaussian x you will minimize the fourth order errors in x and P. sqrt_method : function(ndarray), default=scipy.linalg.cholesky Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe's dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you. If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing. subtract : callable (x, y), optional Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y Attributes ---------- Wm : np.array weight for each sigma point for the mean Wc : np.array weight for each sigma point for the covariance References ---------- .. [1] Julier, Simon J.; Uhlmann, Jeffrey "A New Extension of the Kalman Filter to Nonlinear Systems". Proc. SPIE 3068, Signal Processing, Sensor Fusion, and Target Recognition VI, 182 (July 28, 1997) """ def __init__(self, n, kappa=0., sqrt_method=None, subtract=None): self.n = n self.kappa = kappa if sqrt_method is None: self.sqrt = cholesky else: self.sqrt = sqrt_method if subtract is None: self.subtract = np.subtract else: self.subtract = subtract self._compute_weights() def num_sigmas(self): """ Number of sigma points for each variable in the state x""" return 2*self.n + 1 def sigma_points(self, x, P): r""" Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. kappa is an arbitrary constant. Returns sigma points. Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I Parameters ---------- x : array-like object of the means of length n Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2]) P : scalar, or np.array Covariance of the filter. If scalar, is treated as eye(n)*P. kappa : float Scaling factor. Returns ------- sigmas : np.array, of size (n, 2n+1) 2D array of sigma points :math:`\chi`. Each column contains all of the sigmas for one dimension in the problem space. They are ordered as: .. math:: :nowrap: \begin{eqnarray} \chi[0] = &x \\ \chi[1..n] = &x + [\sqrt{(n+\kappa)P}]_k \\ \chi[n+1..2n] = &x - [\sqrt{(n+\kappa)P}]_k \end{eqnarray} """ if self.n != np.size(x): raise ValueError("expected size(x) {}, but size is {}".format( self.n, np.size(x))) n = self.n if np.isscalar(x): x = np.asarray([x]) n = np.size(x) # dimension of problem if np.isscalar(P): P = np.eye(n) * P else: P = np.atleast_2d(P) sigmas = np.zeros((2*n+1, n)) # implements U'*U = (n+kappa)*P. Returns lower triangular matrix. # Take transpose so we can access with U[i] U = self.sqrt((n + self.kappa) * P) sigmas[0] = x for k in range(n): # pylint: disable=bad-whitespace sigmas[k+1] = self.subtract(x, -U[k]) sigmas[n+k+1] = self.subtract(x, U[k]) return sigmas def _compute_weights(self): """ Computes the weights for the unscented Kalman filter. In this formulation the weights for the mean and covariance are the same. """ n = self.n k = self.kappa self.Wm = np.full(2*n+1, .5 / (n + k)) self.Wm[0] = k / (n+k) self.Wc = self.Wm def __repr__(self): return '\n'.join([ 'JulierSigmaPoints object', pretty_str('n', self.n), pretty_str('kappa', self.kappa), pretty_str('Wm', self.Wm), pretty_str('Wc', self.Wc), pretty_str('subtract', self.subtract), pretty_str('sqrt', self.sqrt) ]) class SimplexSigmaPoints(object): """ Generates sigma points and weights according to the simplex method presented in [1]. Parameters ---------- n : int Dimensionality of the state. n+1 weights will be generated. sqrt_method : function(ndarray), default=scipy.linalg.cholesky Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing. subtract : callable (x, y), optional Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. Attributes ---------- Wm : np.array weight for each sigma point for the mean Wc : np.array weight for each sigma point for the covariance References ---------- .. [1] Phillippe Moireau and Dominique Chapelle "Reduced-Order Unscented Kalman Filtering with Application to Parameter Identification in Large-Dimensional Systems" DOI: 10.1051/cocv/2010006 """ def __init__(self, n, alpha=1, sqrt_method=None, subtract=None): self.n = n self.alpha = alpha if sqrt_method is None: self.sqrt = cholesky else: self.sqrt = sqrt_method if subtract is None: self.subtract = np.subtract else: self.subtract = subtract self._compute_weights() def num_sigmas(self): """ Number of sigma points for each variable in the state x""" return self.n + 1 def sigma_points(self, x, P): """ Computes the implex sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. Returns tuple of the sigma points and weights. Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I Parameters ---------- x : An array-like object of the means of length n Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2]) P : scalar, or np.array Covariance of the filter. If scalar, is treated as eye(n)*P. Returns ------- sigmas : np.array, of size (n, n+1) Two dimensional array of sigma points. Each column contains all of the sigmas for one dimension in the problem space. Ordered by Xi_0, Xi_{1..n} """ if self.n != np.size(x): raise ValueError("expected size(x) {}, but size is {}".format( self.n, np.size(x))) n = self.n if np.isscalar(x): x = np.asarray([x]) x = x.reshape(-1, 1) if np.isscalar(P): P = np.eye(n) * P else: P = np.atleast_2d(P) U = self.sqrt(P) lambda_ = n / (n + 1) Istar = np.array([[-1/np.sqrt(2*lambda_), 1/np.sqrt(2*lambda_)]]) for d in range(2, n+1): row = np.ones((1, Istar.shape[1] + 1)) * 1. / np.sqrt(lambda_*d*(d + 1)) row[0, -1] = -d / np.sqrt(lambda_ * d * (d + 1)) Istar = np.r_[np.c_[Istar, np.zeros((Istar.shape[0]))], row] I = np.sqrt(n)*Istar scaled_unitary = U.dot(I) sigmas = self.subtract(x, -scaled_unitary) return sigmas.T def _compute_weights(self): """ Computes the weights for the scaled unscented Kalman filter. """ n = self.n c = 1. / (n + 1) self.Wm = np.full(n + 1, c) self.Wc = self.Wm def __repr__(self): return '\n'.join([ 'SimplexSigmaPoints object', pretty_str('n', self.n), pretty_str('alpha', self.alpha), pretty_str('Wm', self.Wm), pretty_str('Wc', self.Wc), pretty_str('subtract', self.subtract), pretty_str('sqrt', self.sqrt) ]) filterpy-1.4.5/filterpy/kalman/square_root.py000066400000000000000000000235451335747705400214250ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-instance-attributes """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division) from copy import deepcopy import numpy as np from numpy import dot, zeros, eye from scipy.linalg import cholesky, qr, pinv from filterpy.common import pretty_str class SquareRootKalmanFilter(object): """ Create a Kalman filter which uses a square root implementation. This uses the square root of the state covariance matrix, which doubles the numerical precision of the filter, Therebuy reducing the effect of round off errors. It is likely that you do not need to use this algorithm; we understand divergence issues very well now. However, if you expect the covariance matrix P to vary by 20 or more orders of magnitude then perhaps this will be useful to you, as the square root will vary by 10 orders of magnitude. From my point of view this is merely a 'reference' algorithm; I have not used this code in real world software. Brown[1] has a useful discussion of when you might need to use the square root form of this algorithm. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter. Parameters ---------- dim_x : int Number of state variables for the Kalman filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. This is used to set the default size of P, Q, and u dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. dim_u : int (optional) size of the control input, if it is being used. Default value of 0 indicates it is not used. Attributes ---------- x : numpy.array(dim_x, 1) State estimate P : numpy.array(dim_x, dim_x) State covariance matrix x_prior : numpy.array(dim_x, 1) Prior (predicted) state estimate. The *_prior and *_post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only. P_prior : numpy.array(dim_x, dim_x) Prior (predicted) state covariance matrix. Read Only. x_post : numpy.array(dim_x, 1) Posterior (updated) state estimate. Read Only. P_post : numpy.array(dim_x, dim_x) Posterior (updated) state covariance matrix. Read Only. z : numpy.array Last measurement used in update(). Read only. R : numpy.array(dim_z, dim_z) Measurement noise matrix Q : numpy.array(dim_x, dim_x) Process noise matrix F : numpy.array() State Transition matrix H : numpy.array(dim_z, dim_x) Measurement function y : numpy.array Residual of the update step. Read only. K : numpy.array(dim_x, dim_z) Kalman gain of the update step. Read only. S : numpy.array Systen uncertaintly projected to measurement space. Read only. Examples -------- See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python References ---------- [1] Robert Grover Brown. Introduction to Random Signals and Applied Kalman Filtering. Wiley and sons, 2012. """ def __init__(self, dim_x, dim_z, dim_u=0): if dim_z < 1: raise ValueError('dim_x must be 1 or greater') if dim_z < 1: raise ValueError('dim_x must be 1 or greater') if dim_u < 0: raise ValueError('dim_x must be 0 or greater') self.dim_x = dim_x self.dim_z = dim_z self.dim_u = dim_u self.x = zeros((dim_x, 1)) # state self._P = eye(dim_x) # uncertainty covariance self._P1_2 = eye(dim_x) # sqrt uncertainty covariance self._Q = eye(dim_x) # sqrt process uncertainty self._Q1_2 = eye(dim_x) # sqrt process uncertainty self.B = 0. # control transition matrix self.F = np.eye(dim_x) # state transition matrix self.H = np.zeros((dim_z, dim_x)) # Measurement function self._R1_2 = eye(dim_z) # sqrt state uncertainty self._R = eye(dim_z) # state uncertainty self.z = np.array([[None]*self.dim_z]).T self.K = 0. self.S = 0. # Residual is computed during the innovation (update) step. We # save it so that in case you want to inspect it for various # purposes self.y = zeros((dim_z, 1)) # identity matrix. self._I = np.eye(dim_x) self.M = np.zeros((dim_z + dim_x, dim_z + dim_x)) # copy prior and posterior self.x_prior = np.copy(self.x) self._P1_2_prior = np.copy(self._P1_2) self.x_post = np.copy(self.x) self._P1_2_post = np.copy(self._P1_2) def update(self, z, R2=None): """ Add a new measurement (z) to the kalman filter. If z is None, nothing is changed. Parameters ---------- z : np.array measurement for this update. R2 : np.array, scalar, or None Sqrt of meaaurement noize. Optionally provide to override the measurement noise for this one call, otherwise self.R2 will be used. """ if z is None: self.z = np.array([[None]*self.dim_z]).T self.x_post = self.x.copy() self._P1_2_post = np.copy(self._P1_2) return if R2 is None: R2 = self._R1_2 elif np.isscalar(R2): R2 = eye(self.dim_z) * R2 # rename for convienance dim_z = self.dim_z M = self.M M[0:dim_z, 0:dim_z] = R2.T M[dim_z:, 0:dim_z] = dot(self.H, self._P1_2).T M[dim_z:, dim_z:] = self._P1_2.T _, self.S = qr(M) self.K = self.S[0:dim_z, dim_z:].T N = self.S[0:dim_z, 0:dim_z].T # y = z - Hx # error (residual) between measurement and prediction self.y = z - dot(self.H, self.x) # x = x + Ky # predict new x with residual scaled by the kalman gain self.x += dot(self.K, pinv(N)).dot(self.y) self._P1_2 = self.S[dim_z:, dim_z:].T self.z = deepcopy(z) self.x_post = self.x.copy() self._P1_2_post = np.copy(self._P1_2) def predict(self, u=0): """ Predict next state (prior) using the Kalman filter state propagation equations. Parameters ---------- u : np.array, optional Optional control vector. If non-zero, it is multiplied by B to create the control input into the system. """ # x = Fx + Bu self.x = dot(self.F, self.x) + dot(self.B, u) # P = FPF' + Q _, P2 = qr(np.hstack([dot(self.F, self._P1_2), self._Q1_2]).T) self._P1_2 = P2[:self.dim_x, :self.dim_x].T # copy prior self.x_prior = np.copy(self.x) self._P1_2_prior = np.copy(self._P1_2) def residual_of(self, z): """ returns the residual for the given measurement (z). Does not alter the state of the filter. """ return z - dot(self.H, self.x) def measurement_of_state(self, x): """ Helper function that converts a state into a measurement. Parameters ---------- x : np.array kalman state vector Returns ------- z : np.array measurement corresponding to the given state """ return dot(self.H, x) @property def Q(self): """ Process uncertainty""" return dot(self._Q1_2.T, self._Q1_2) @property def Q1_2(self): """ Sqrt Process uncertainty""" return self._Q1_2 @Q.setter def Q(self, value): """ Process uncertainty""" self._Q = value self._Q1_2 = cholesky(self._Q, lower=True) @property def P(self): """ covariance matrix""" return dot(self._P1_2.T, self._P1_2) @property def P_prior(self): """ covariance matrix of the prior""" return dot(self._P1_2_prior.T, self._P1_2_prior) @property def P_post(self): """ covariance matrix of the posterior""" return dot(self._P1_2_prior.T, self._P1_2_prior) @property def P1_2(self): """ sqrt of covariance matrix""" return self._P1_2 @P.setter def P(self, value): """ covariance matrix""" self._P = value self._P1_2 = cholesky(self._P, lower=True) @property def R(self): """ measurement uncertainty""" return dot(self._R1_2.T, self._R1_2) @property def R1_2(self): """ sqrt of measurement uncertainty""" return self._R1_2 @R.setter def R(self, value): """ measurement uncertainty""" self._R = value self._R1_2 = cholesky(self._R, lower=True) def __repr__(self): return '\n'.join([ 'SquareRootKalmanFilter object', pretty_str('dim_x', self.dim_x), pretty_str('dim_z', self.dim_z), pretty_str('dim_u', self.dim_u), pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('F', self.F), pretty_str('Q', self.Q), pretty_str('R', self.R), pretty_str('H', self.H), pretty_str('K', self.K), pretty_str('y', self.y), pretty_str('S', self.S), pretty_str('M', self.M), pretty_str('B', self.B), ]) filterpy-1.4.5/filterpy/kalman/tests/000077500000000000000000000000001335747705400176415ustar00rootroot00000000000000filterpy-1.4.5/filterpy/kalman/tests/test_ckf.py000066400000000000000000000031101335747705400220100ustar00rootroot00000000000000# -*- coding: utf-8 -*- """ Created on Sun Jun 26 08:03:07 2016 @author: rlabbe """ from filterpy.common import Saver from filterpy.kalman import CubatureKalmanFilter as CKF from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import MerweScaledSigmaPoints import numpy as np from numpy.random import randn from pytest import approx from scipy.spatial.distance import mahalanobis as scipy_mahalanobis def test_1d(): def fx(x, dt): F = np.array([[1., dt], [0, 1]]) return np.dot(F, x) def hx(x): return x[0:1] ckf = CKF(dim_x=2, dim_z=1, dt=0.1, hx=hx, fx=fx) ckf.x = np.array([[1.], [2.]]) ckf.P = np.array([[1, 1.1], [1.1, 3]]) ckf.R = np.eye(1) * .05 ckf.Q = np.array([[0., 0], [0., .001]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) s = Saver(kf) for i in range(50): z = np.array([[i+randn()*0.1]]) ckf.predict() ckf.update(z) kf.predict() kf.update(z[0]) assert abs(ckf.x[0] - kf.x[0]) < 1e-10 assert abs(ckf.x[1] - kf.x[1]) < 1e-10 s.save() # test mahalanobis a = np.zeros(kf.y.shape) maha = scipy_mahalanobis(a, kf.y, kf.SI) assert kf.mahalanobis == approx(maha) s.to_array() if __name__ == "__main__": test_1d() filterpy-1.4.5/filterpy/kalman/tests/test_ekf.py000066400000000000000000000053151335747705400220230ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import matplotlib.pyplot as plt from math import sqrt import numpy as np from filterpy.kalman import ExtendedKalmanFilter from numpy import array, eye, asarray from filterpy.common import Saver from filterpy.examples import RadarSim from pytest import approx from scipy.spatial.distance import mahalanobis as scipy_mahalanobis DO_PLOT = False def test_ekf(): def H_of(x): """ compute Jacobian of H matrix for state x """ horiz_dist = x[0] altitude = x[2] denom = sqrt(horiz_dist**2 + altitude**2) return array([[horiz_dist/denom, 0., altitude/denom]]) def hx(x): """ takes a state variable and returns the measurement that would correspond to that state. """ return sqrt(x[0]**2 + x[2]**2) dt = 0.05 proccess_error = 0.05 rk = ExtendedKalmanFilter(dim_x=3, dim_z=1) rk.F = eye(3) + array ([[0, 1, 0], [0, 0, 0], [0, 0, 0]])*dt def fx(x, dt): return np.dot(rk.F, x) rk.x = array([-10., 90., 1100.]) rk.R *= 10 rk.Q = array([[0, 0, 0], [0, 1, 0], [0, 0, 1]]) * 0.001 rk.P *= 50 rs = [] xs = [] radar = RadarSim(dt) ps = [] pos = [] s = Saver(rk) for i in range(int(20/dt)): z = radar.get_range(proccess_error) pos.append(radar.pos) rk.update(asarray([z]), H_of, hx, R=hx(rk.x)*proccess_error) ps.append(rk.P) rk.predict() xs.append(rk.x) rs.append(z) s.save() # test mahalanobis a = np.zeros(rk.y.shape) maha = scipy_mahalanobis(a, rk.y, rk.SI) assert rk.mahalanobis == approx(maha) s.to_array() xs = asarray(xs) ps = asarray(ps) rs = asarray(rs) p_pos = ps[:, 0, 0] p_vel = ps[:, 1, 1] p_alt = ps[:, 2, 2] pos = asarray(pos) if DO_PLOT: plt.subplot(311) plt.plot(xs[:, 0]) plt.ylabel('position') plt.subplot(312) plt.plot(xs[:, 1]) plt.ylabel('velocity') plt.subplot(313) #plt.plot(xs[:,2]) #plt.ylabel('altitude') plt.plot(p_pos) plt.plot(-p_pos) plt.plot(xs[:, 0] - pos) if __name__ == '__main__': test_ekf()filterpy-1.4.5/filterpy/kalman/tests/test_enkf.py000066400000000000000000000064341335747705400222040ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from numpy.random import randn import numpy as np import matplotlib.pyplot as plt from filterpy.kalman import EnsembleKalmanFilter as EnKF from filterpy.common import Q_discrete_white_noise, Saver from math import cos, sin DO_PLOT = False def test_1d_const_vel(): def hx(x): return np.array([x[0]]) F = np.array([[1., 1.],[0., 1.]]) def fx(x, dt): return np.dot(F, x) x = np.array([0., 1.]) P = np.eye(2)* 100. f = EnKF(x=x, P=P, dim_z=1, dt=1., N=8, hx=hx, fx=fx) std_noise = 10. f.R *= std_noise**2 f.Q = Q_discrete_white_noise(2, 1., .001) measurements = [] results = [] ps = [] zs = [] s = Saver(f) for t in range (0,100): # create measurement = t plus white noise z = t + randn()*std_noise zs.append(z) f.predict() f.update(np.asarray([z])) # save data results.append (f.x[0]) measurements.append(z) ps.append(3*(f.P[0,0]**.5)) s.save() s.to_array() results = np.asarray(results) ps = np.asarray(ps) if DO_PLOT: plt.plot(results, label='EnKF') plt.plot(measurements, c='r', label='z') plt.plot (results-ps, c='k',linestyle='--', label='3$\sigma$') plt.plot(results+ps, c='k', linestyle='--') plt.legend(loc='best') #print(ps) return f def test_circle(): def hx(x): return np.array([x[0], x[3]]) F = np.array([[1., 1., .5, 0., 0., 0.], [0., 1., 1., 0., 0., 0.], [0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 1., .5], [0., 0., 0., 0., 1., 1.], [0., 0., 0., 0., 0., 1.]]) def fx(x, dt): return np.dot(F, x) x = np.array([50., 0., 0, 0, .0, 0.]) P = np.eye(6)* 100. f = EnKF(x=x, P=P, dim_z=2, dt=1., N=30, hx=hx, fx=fx) std_noise = .1 f.R *= std_noise**2 f.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .001) f.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .001) measurements = [] results = [] zs = [] for t in range (0,300): a = t / 300000 x = cos(a) * 50. y = sin(a) * 50. # create measurement = t plus white noise z = np.array([x,y]) zs.append(z) f.predict() f.update(z) # save data results.append (f.x) measurements.append(z) #test that __repr__ doesn't assert str(f) results = np.asarray(results) measurements = np.asarray(measurements) if DO_PLOT: plt.plot(results[:,0], results[:,2], label='EnKF') plt.plot(measurements[:,0], measurements[:,1], c='r', label='z') #plt.plot (results-ps, c='k',linestyle='--', label='3$\sigma$') #plt.plot(results+ps, c='k', linestyle='--') plt.legend(loc='best') plt.axis('equal') #print(ps) if __name__ == '__main__': DO_PLOT = True test_circle () test_1d_const_vel() #test_noisy_1d()filterpy-1.4.5/filterpy/kalman/tests/test_fls.py000066400000000000000000000057761335747705400220550ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy as np from numpy import random import matplotlib.pyplot as plt from filterpy.kalman import KalmanFilter, FixedLagSmoother DO_PLOT = False def test_fls(): # it is possible for the fixed lag to rarely perform worse than the # kalman filter. Let it happen once in 50 times before we become # alarmed. fail_count = 0 for i in range(50): fail_count = one_run_test_fls() assert fail_count < 2 def test_batch_equals_recursive(): """ ensures that the batch filter and the recursive version both produce the same results. """ N = 4 # size of lag fls = FixedLagSmoother(dim_x=2, dim_z=1, N=N) fls.x = np.array([0., .5]) fls.F = np.array([[1.,1.], [0.,1.]]) fls.H = np.array([[1.,0.]]) fls.P *= 200 fls.R *= 5. fls.Q *= 0.001 nom = np.array([t/2. for t in range (0,40)]) zs = np.array([t + random.randn()*1.1 for t in nom]) xs, x = fls.smooth_batch(zs, N) for k,z in enumerate(zs): fls.smooth(z) xSmooth = np.asarray(fls.xSmooth) xfl = xs[:,0].T[0] res = xSmooth.T[0,0] - xfl assert np.sum(res) < 1.e-12 def one_run_test_fls(): fls = FixedLagSmoother(dim_x=2, dim_z=1) fls.x = np.array([0., .5]) fls.F = np.array([[1.,1.], [0.,1.]]) fls.H = np.array([[1.,0.]]) fls.P *= 200 fls.R *= 5. fls.Q *= 0.001 kf = KalmanFilter(dim_x=2, dim_z=1) kf.x = np.array([0., .5]) kf.F = np.array([[1.,1.], [0.,1.]]) kf.H = np.array([[1.,0.]]) kf.P *= 2000 kf.R *= 1. kf.Q *= 0.001 N = 4 # size of lag nom = np.array([t/2. for t in range (0,40)]) zs = np.array([t + random.randn()*1.1 for t in nom]) xs, x = fls.smooth_batch(zs, N) M, P, _, _ = kf.batch_filter(zs) rts_x, _, _, _ = kf.rts_smoother(M, P) xfl = xs[:,0].T[0] xkf = M[:,0].T[0] fl_res = abs(xfl-nom) kf_res = abs(xkf-nom) if DO_PLOT: plt.cla() plt.plot(zs,'o', alpha=0.5, marker='o', label='zs') plt.plot(x[:,0], label='FLS') plt.plot(xfl, label='FLS S') plt.plot(xkf, label='KF') plt.plot(rts_x[:,0], label='RTS') plt.legend(loc=4) plt.show() print(fl_res) print(kf_res) print('std fixed lag:', np.mean(fl_res[N:])) print('std kalman:', np.mean(kf_res[N:])) return np.mean(fl_res) <= np.mean(kf_res) if __name__ == '__main__': DO_PLOT = True one_run_test_fls() DO_PLOT = False test_fls() test_batch_equals_recursive() filterpy-1.4.5/filterpy/kalman/tests/test_fm.py000066400000000000000000000047061335747705400216630ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy.random as random import numpy as np import matplotlib.pyplot as plt from filterpy.kalman import FadingKalmanFilter from pytest import approx from scipy.spatial.distance import mahalanobis as scipy_mahalanobis DO_PLOT = False def test_noisy_1d(): f = FadingKalmanFilter(3., dim_x=2, dim_z=1) f.x = np.array([[2.], [0.]]) # initial state (location and velocity) f.F = np.array([[1.,1.], [0.,1.]]) # state transition matrix f.H = np.array([[1.,0.]]) # Measurement function f.P *= 1000. # covariance matrix f.R = 5.**2 # state uncertainty f.Q = np.array([[0, 0], [0, 0.0001]]) # process uncertainty measurements = [] results = [] zs = [] for t in range (100): # create measurement = t plus white noise z = t + random.randn() * np.sqrt(f.R) zs.append(z) # perform kalman filtering f.update(z) f.predict() # save data results.append(f.x[0, 0]) measurements.append(z) # test mahalanobis a = np.zeros(f.y.shape) maha = scipy_mahalanobis(a, f.y, f.SI) assert f.mahalanobis == approx(maha) print(z, maha, f.y, f.S) assert maha < 4 # now do a batch run with the stored z values so we can test that # it is working the same as the recursive implementation. # give slightly different P so result is slightly different f.X = np.array([[2.,0]]).T f.P = np.eye(2)*100. m, c, _, _ = f.batch_filter(zs,update_first=False) # plot data if DO_PLOT: p1, = plt.plot(measurements,'r', alpha=0.5) p2, = plt.plot (results,'b') p4, = plt.plot(m[:,0], 'm') p3, = plt.plot ([0, 100],[0, 100], 'g') # perfect result plt.legend([p1,p2, p3, p4], ["noisy measurement", "KF output", "ideal", "batch"], loc=4) plt.show() if __name__ == "__main__": DO_PLOT = True test_noisy_1d()filterpy-1.4.5/filterpy/kalman/tests/test_imm.py000066400000000000000000000164431335747705400220440ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) from math import sin, cos, radians import numpy.random as random import numpy as np from numpy import array from numpy.random import randn import matplotlib.pyplot as plt from filterpy.kalman import IMMEstimator, KalmanFilter from filterpy.common import Q_discrete_white_noise, Saver DO_PLOT = False class NoisySensor(object): def __init__(self, noise_factor=1): self.noise_factor = noise_factor def sense(self, pos): return (pos[0] + randn()*self.noise_factor, pos[1] + randn()*self.noise_factor) def angle_between(x, y): return min(y-x, y-x+360, y-x-360, key=abs) class ManeuveringTarget(object): def __init__(self, x0, y0, v0, heading): self.x = x0 self.y = y0 self.vel = v0 self.hdg = heading self.cmd_vel = v0 self.cmd_hdg = heading self.vel_step = 0 self.hdg_step = 0 self.vel_delta = 0 self.hdg_delta = 0 def update(self): vx = self.vel * cos(radians(90-self.hdg)) vy = self.vel * sin(radians(90-self.hdg)) self.x += vx self.y += vy if self.hdg_step > 0: self.hdg_step -= 1 self.hdg += self.hdg_delta if self.vel_step > 0: self.vel_step -= 1 self.vel += self.vel_delta return (self.x, self.y) def set_commanded_heading(self, hdg_degrees, steps): self.cmd_hdg = hdg_degrees self.hdg_delta = angle_between(self.cmd_hdg, self.hdg) / steps if abs(self.hdg_delta) > 0: self.hdg_step = steps else: self.hdg_step = 0 def set_commanded_speed(self, speed, steps): self.cmd_vel = speed self.vel_delta = (self.cmd_vel - self.vel) / steps if abs(self.vel_delta) > 0: self.vel_step = steps else: self.vel_step = 0 def make_cv_filter(dt, noise_factor): cvfilter = KalmanFilter(dim_x = 2, dim_z=1) cvfilter.x = array([0., 0.]) cvfilter.P *= 3 cvfilter.R *= noise_factor**2 cvfilter.F = array([[1, dt], [0, 1]], dtype=float) cvfilter.H = array([[1, 0]], dtype=float) cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02) return cvfilter def make_ca_filter(dt, noise_factor): cafilter = KalmanFilter(dim_x=3, dim_z=1) cafilter.x = array([0., 0., 0.]) cafilter.P *= 3 cafilter.R *= noise_factor**2 cafilter.Q = Q_discrete_white_noise(dim=3, dt=dt, var=0.02) cafilter.F = array([[1, dt, 0.5*dt*dt], [0, 1, dt], [0, 0, 1]], dtype=float) cafilter.H = array([[1, 0, 0]], dtype=float) return cafilter def generate_data(steady_count, noise_factor): t = ManeuveringTarget(x0=0, y0=0, v0=0.3, heading=0) xs = [] ys = [] for i in range(30): x, y = t.update() xs.append(x) ys.append(y) t.set_commanded_heading(310, 25) t.set_commanded_speed(1, 15) for i in range(steady_count): x, y = t.update() xs.append(x) ys.append(y) ns = NoisySensor(noise_factor=noise_factor) pos = array(list(zip(xs, ys))) zs = array([ns.sense(p) for p in pos]) return pos, zs def test_imm(): """ This test is drawn from Crassidis [1], example 4.6. ** References** [1] Crassidis. "Optimal Estimation of Dynamic Systems", CRC Press, Second edition. """ r = 100. dt = 1. phi_sim = np.array( [[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) gam = np.array([[dt**2/2, 0], [dt, 0], [0, dt**2/2], [0, dt]]) x = np.array([[2000, 0, 10000, -15.]]).T simxs = [] N = 600 for i in range(N): x = np.dot(phi_sim, x) if i >= 400: x += np.dot(gam, np.array([[.075, .075]]).T) simxs.append(x) simxs = np.array(simxs) zs = np.zeros((N, 2)) for i in range(len(zs)): zs[i, 0] = simxs[i, 0] + randn()*r zs[i, 1] = simxs[i, 2] + randn()*r ''' try: # data to test against crassidis' IMM matlab code zs_tmp = np.genfromtxt('c:/users/rlabbe/dropbox/Crassidis/mycode/xx.csv', delimiter=',')[:-1] zs = zs_tmp except: pass ''' ca = KalmanFilter(6, 2) cano = KalmanFilter(6, 2) dt2 = (dt**2)/2 ca.F = np.array( [[1, dt, dt2, 0, 0, 0], [0, 1, dt, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, dt, dt2], [0, 0, 0, 0, 1, dt], [0, 0, 0, 0, 0, 1]]) cano.F = ca.F.copy() ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T cano.x = ca.x.copy() ca.P *= 1.e-12 cano.P *= 1.e-12 ca.R *= r**2 cano.R *= r**2 cano.Q *= 0 q = np.array([[.05, .125, 1./6], [.125, 1/3, .5], [1./6, .5, 1.]])*1.e-3 ca.Q[0:3, 0:3] = q ca.Q[3:6, 3:6] = q ca.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) cano.H = ca.H.copy() filters = [ca, cano] trans = np.array([[0.97, 0.03], [0.03, 0.97]]) bank = IMMEstimator(filters, (0.5, 0.5), trans) # ensure __repr__ doesn't have problems str(bank) s = Saver(bank) ca_s = Saver(ca) cano_s = Saver(cano) for i, z in enumerate(zs): z = np.array([z]).T bank.update(z) bank.predict() s.save() ca_s.save() cano_s.save() if DO_PLOT: s.to_array() ca_s.to_array() cano_s.to_array() plt.figure() plt.subplot(121) plt.plot(s.x[:, 0], s.x[:, 3], 'k') #plt.plot(cvxs[:, 0], caxs[:, 3]) #plt.plot(simxs[:, 0], simxs[:, 2], 'g') plt.scatter(zs[:, 0], zs[:, 1], marker='+', alpha=0.2) plt.subplot(122) plt.plot(s.mu[:, 0]) plt.plot(s.mu[:, 1]) plt.ylim(0, 1) plt.title('probability ratio p(cv)/p(ca)') '''plt.figure() plt.plot(cvxs, label='CV') plt.plot(caxs, label='CA') plt.plot(xs[:, 0], label='GT') plt.legend() plt.figure() plt.plot(xs) plt.plot(xs[:, 0])''' def test_misshapen(): """Ensure we get a ValueError if the filter banks are not designed properly """ ca = KalmanFilter(3, 1) cv = KalmanFilter(2, 1) trans = np.array([[0.97, 0.03], [0.03, 0.97]]) try: IMMEstimator([ca, cv], (0.5, 0.5), trans) assert "IMM should raise ValueError on filter banks with filters of different sizes" except ValueError: pass try: IMMEstimator([], (0.5, 0.5), trans) assert "Should raise ValueError on empty bank" except ValueError: pass if __name__ == '__main__': #test_misshapen() DO_PLOT = True test_imm() filterpy-1.4.5/filterpy/kalman/tests/test_information.py000066400000000000000000000107131335747705400236010ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy.random as random import numpy as np import matplotlib.pyplot as plt from filterpy.common import Saver from filterpy.kalman import KalmanFilter, InformationFilter DO_PLOT = False def test_1d_0P(): global inf f = KalmanFilter (dim_x=2, dim_z=1) inf = InformationFilter (dim_x=2, dim_z=1) f.x = np.array([[2.], [0.]]) # initial state (location and velocity) f.F = (np.array([[1., 1.], [0., 1.]])) # state transition matrix f.H = np.array([[1., 0.]]) # Measurement function f.R = np.array([[5.]]) # state uncertainty f.Q = np.eye(2)*0.0001 # process uncertainty f.P = np.diag([20., 20.]) inf.x = f.x.copy() inf.F = f.F.copy() inf.H = np.array([[1.,0.]]) # Measurement function inf.R_inv *= 1./5 # state uncertainty inf.Q = np.eye(2)*0.0001 inf.P_inv = 0.000000000000000000001 #inf.P_inv = inv(f.P) m = [] r = [] r2 = [] zs = [] for t in range (50): # create measurement = t plus white noise z = t + random.randn()* np.sqrt(5) zs.append(z) # perform kalman filtering f.predict() f.update(z) inf.predict() inf.update(z) try: print(t, inf.P) except: pass # save data r.append (f.x[0,0]) r2.append (inf.x[0,0]) m.append(z) #assert np.allclose(f.x, inf.x), f'{t}: {f.x.T} {inf.x.T}' if DO_PLOT: plt.plot(m) plt.plot(r) plt.plot(r2) def test_1d(): global inf f = KalmanFilter(dim_x=2, dim_z=1) inf = InformationFilter(dim_x=2, dim_z=1) # ensure __repr__ doesn't assert str(inf) f.x = np.array([[2.], [0.]]) # initial state (location and velocity) inf.x = f.x.copy() f.F = (np.array([[1.,1.], [0.,1.]])) # state transition matrix inf.F = f.F.copy() f.H = np.array([[1.,0.]]) # Measurement function inf.H = np.array([[1.,0.]]) # Measurement function f.R *= 5 # state uncertainty inf.R_inv *= 1./5 # state uncertainty f.Q *= 0.0001 # process uncertainty inf.Q *= 0.0001 m = [] r = [] r2 = [] zs = [] s = Saver(inf) for t in range (100): # create measurement = t plus white noise z = t + random.randn()*20 zs.append(z) # perform kalman filtering f.update(z) f.predict() inf.update(z) inf.predict() # save data r.append (f.x[0,0]) r2.append (inf.x[0,0]) m.append(z) print(inf.y) s.save() assert abs(f.x[0,0] - inf.x[0,0]) < 1.e-12 if DO_PLOT: plt.plot(m) plt.plot(r) plt.plot(r2) def test_against_kf(): inv = np.linalg.inv dt = 1.0 IM = np.eye(2) Q = np.array([[.25, 0.5], [0.5, 1]]) F = np.array([[1, dt], [0, 1]]) #QI = inv(Q) P = inv(IM) from filterpy.kalman import InformationFilter from filterpy.common import Q_discrete_white_noise #f = IF2(2, 1) r_std = .2 R = np.array([[r_std*r_std]]) RI = inv(R) '''f.F = F.copy() f.H = np.array([[1, 0.]]) f.RI = RI.copy() f.Q = Q.copy() f.IM = IM.copy()''' kf = KalmanFilter(2, 1) kf.F = F.copy() kf.H = np.array([[1, 0.]]) kf.R = R.copy() kf.Q = Q.copy() f0 = InformationFilter(2, 1) f0.F = F.copy() f0.H = np.array([[1, 0.]]) f0.R_inv = RI.copy() f0.Q = Q.copy() #f.IM = np.zeros((2,2)) for i in range(1, 50): z = i + (np.random.rand() * r_std) f0.predict() #f.predict() kf.predict() f0.update(z) #f.update(z) kf.update(z) print(f0.x.T, kf.x.T) assert np.allclose(f0.x, kf.x) #assert np.allclose(f.x, kf.x) if __name__ == "__main__": DO_PLOT = True #test_1d_0P() test_1d() test_against_kf() filterpy-1.4.5/filterpy/kalman/tests/test_kf.py000066400000000000000000000371031335747705400216560ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import absolute_import, division, print_function import numpy.random as random from numpy.random import randn import numpy as np import matplotlib.pyplot as plt from pytest import approx from filterpy.kalman import KalmanFilter, update, predict, batch_filter from filterpy.common import Q_discrete_white_noise, kinematic_kf, Saver from scipy.linalg import block_diag, norm from scipy.spatial.distance import mahalanobis as scipy_mahalanobis DO_PLOT = False class PosSensor1(object): def __init__(self, pos=(0, 0), vel=(0, 0), noise_std=1.): self.vel = vel self.noise_std = noise_std self.pos = [pos[0], pos[1]] def read(self): self.pos[0] += self.vel[0] self.pos[1] += self.vel[1] return [self.pos[0] + randn() * self.noise_std, self.pos[1] + randn() * self.noise_std] def const_vel_filter(dt, x0=0, x_ndim=1, P_diag=(1., 1.), R_std=1., Q_var=0.0001): """ helper, constructs 1d, constant velocity filter""" f = KalmanFilter(dim_x=2, dim_z=1) if x_ndim == 1: f.x = np.array([x0, 0.]) else: f.x = np.array([[x0, 0.]]).T f.F = np.array([[1., dt], [0., 1.]]) f.H = np.array([[1., 0.]]) f.P = np.diag(P_diag) f.R = np.eye(1) * (R_std**2) f.Q = Q_discrete_white_noise(2, dt, Q_var) return f def const_vel_filter_2d(dt, x_ndim=1, P_diag=(1., 1, 1, 1), R_std=1., Q_var=0.0001): """ helper, constructs 1d, constant velocity filter""" kf = KalmanFilter(dim_x=4, dim_z=2) kf.x = np.array([[0., 0., 0., 0.]]).T kf.P *= np.diag(P_diag) kf.F = np.array([[1., dt, 0., 0.], [0., 1., 0., 0.], [0., 0., 1., dt], [0., 0., 0., 1.]]) kf.H = np.array([[1., 0, 0, 0], [0., 0, 1, 0]]) kf.R *= np.eye(2) * (R_std**2) q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var) kf.Q = block_diag(q, q) return kf def test_noisy_1d(): f = KalmanFilter(dim_x=2, dim_z=1) f.x = np.array([[2.], [0.]]) # initial state (location and velocity) f.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix f.H = np.array([[1., 0.]]) # Measurement function f.P *= 1000. # covariance matrix f.R = 5 # state uncertainty f.Q = 0.0001 # process uncertainty measurements = [] results = [] zs = [] for t in range(100): # create measurement = t plus white noise z = t + random.randn()*20 zs.append(z) # perform kalman filtering f.update(z) f.predict() # save data results.append(f.x[0, 0]) measurements.append(z) # test mahalanobis a = np.zeros(f.y.shape) maha = scipy_mahalanobis(a, f.y, f.SI) assert f.mahalanobis == approx(maha) # now do a batch run with the stored z values so we can test that # it is working the same as the recursive implementation. # give slightly different P so result is slightly different f.x = np.array([[2., 0]]).T f.P = np.eye(2) * 100. s = Saver(f) m, c, _, _ = f.batch_filter(zs, update_first=False, saver=s) s.to_array() assert len(s.x) == len(zs) assert len(s.x) == len(s) # plot data if DO_PLOT: p1, = plt.plot(measurements, 'r', alpha=0.5) p2, = plt.plot(results, 'b') p4, = plt.plot(m[:, 0], 'm') p3, = plt.plot([0, 100], [0, 100], 'g') # perfect result plt.legend([p1, p2, p3, p4], ["noisy measurement", "KF output", "ideal", "batch"], loc=4) plt.show() def test_1d_vel(): from scipy.linalg import inv from numpy import dot global ks dt = 1. std_z = 0.0001 x = np.array([[0.], [0.]]) F = np.array([[1., dt], [0., 1.]]) H = np.array([[1., 0.]]) P = np.eye(2) R = np.eye(1)*std_z**2 Q = np.eye(2)*0.001 measurements = [] xest = [] ks = [] pos = 0. for t in range(20): z = pos + random.randn() * std_z pos += 100 # perform kalman filtering x = dot(F, x) P = dot(dot(F, P), F.T) + Q P2 = P.copy() P2[0, 1] = 0 # force there to be no correlation P2[1, 0] = 0 S = dot(dot(H, P2), H.T) + R K = dot(dot(P2, H.T), inv(S)) y = z - dot(H, x) x = x + dot(K, y) # save data xest.append(x.copy()) measurements.append(z) ks.append(K.copy()) xest = np.array(xest) ks = np.array(ks) # plot data if DO_PLOT: plt.subplot(121) plt.plot(xest[:, 1]) plt.subplot(122) plt.plot(ks[:, 1]) plt.show() def test_noisy_11d(): f = KalmanFilter(dim_x=2, dim_z=1) f.x = np.array([2., 0]) # initial state (location and velocity) f.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix f.H = np.array([[1., 0.]]) # Measurement function f.P *= 1000. # covariance matrix f.R = 5 # state uncertainty f.Q = 0.0001 # process uncertainty measurements = [] results = [] zs = [] for t in range(100): # create measurement = t plus white noise z = t + random.randn()*20 zs.append(z) # perform kalman filtering f.update(z) f.predict() # save data results.append(f.x[0]) measurements.append(z) # test mahalanobis a = np.zeros(f.y.shape) maha = scipy_mahalanobis(a, f.y, f.SI) assert f.mahalanobis == approx(maha) # now do a batch run with the stored z values so we can test that # it is working the same as the recursive implementation. # give slightly different P so result is slightly different f.x = np.array([[2., 0]]).T f.P = np.eye(2) * 100. m, c, _, _ = f.batch_filter(zs, update_first=False) # plot data if DO_PLOT: p1, = plt.plot(measurements, 'r', alpha=0.5) p2, = plt.plot(results, 'b') p4, = plt.plot(m[:, 0], 'm') p3, = plt.plot([0, 100], [0, 100], 'g') # perfect result plt.legend([p1, p2, p3, p4], ["noisy measurement", "KF output", "ideal", "batch"], loc=4) plt.show() def test_batch_filter(): f = KalmanFilter(dim_x=2, dim_z=1) f.x = np.array([2., 0]) # initial state (location and velocity) f.F = np.array([[1., 1.], [0., 1.]]) # state transition matrix f.H = np.array([[1., 0.]]) # Measurement function f.P *= 1000. # covariance matrix f.R = 5 # state uncertainty f.Q = 0.0001 # process uncertainty zs = [None, 1., 2.] m, c, _, _ = f.batch_filter(zs, update_first=False) m, c, _, _ = f.batch_filter(zs, update_first=True) def test_univariate(): f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1) f.x = np.array([[0]]) f.P *= 50 f.H = np.array([[1.]]) f.F = np.array([[1.]]) f.B = np.array([[1.]]) f.Q = .02 f.R *= .1 for i in range(50): f.predict() f.update(i) def test_procedure_form(): dt = 1. std_z = 10.1 x = np.array([[0.], [0.]]) F = np.array([[1., dt], [0., 1.]]) H = np.array([[1., 0.]]) P = np.eye(2) R = np.eye(1)*std_z**2 Q = Q_discrete_white_noise(2, dt, 5.1) kf = KalmanFilter(2, 1) kf.x = x.copy() kf.F = F.copy() kf.H = H.copy() kf.P = P.copy() kf.R = R.copy() kf.Q = Q.copy() measurements = [] xest = [] pos = 0. for t in range(2000): z = pos + random.randn() * std_z pos += 100 # perform kalman filtering x, P = predict(x, P, F, Q) kf.predict() assert norm(x - kf.x) < 1.e-12 x, P, _, _, _, _ = update(x, P, z, R, H, True) kf.update(z) assert norm(x - kf.x) < 1.e-12 # save data xest.append(x.copy()) measurements.append(z) xest = np.asarray(xest) measurements = np.asarray(measurements) # plot data if DO_PLOT: plt.plot(xest[:, 0]) plt.plot(xest[:, 1]) plt.plot(measurements) def test_steadystate(): dim = 7 cv = kinematic_kf(dim=dim, order=5) cv.x[1] = 1.0 for i in range(100): cv.predict() cv.update([i]*dim) for i in range(100): cv.predict_steadystate() cv.update_steadystate([i]*dim) # test mahalanobis a = np.zeros(cv.y.shape) maha = scipy_mahalanobis(a, cv.y, cv.SI) assert cv.mahalanobis == approx(maha) def test_procedural_batch_filter(): f = KalmanFilter(dim_x=2, dim_z=1) f.x = np.array([2., 0]) f.F = np.array([[1., 1.], [0., 1.]]) f.H = np.array([[1., 0.]]) f.P = np.eye(2) * 1000. f.R = np.eye(1) * 5 f.Q = Q_discrete_white_noise(2, 1., 0.0001) f.test_matrix_dimensions() x = np.array([2., 0]) F = np.array([[1., 1.], [0., 1.]]) H = np.array([[1., 0.]]) P = np.eye(2) * 1000. R = np.eye(1) * 5 Q = Q_discrete_white_noise(2, 1., 0.0001) zs = [13., None, 1., 2.] * 10 m, c, _, _ = f.batch_filter(zs, update_first=False) n = len(zs) mp, cp, _, _ = batch_filter(x, P, zs, [F]*n, [Q]*n, [H]*n, [R]*n) for x1, x2 in zip(m, mp): assert np.allclose(x1, x2) for p1, p2 in zip(c, cp): assert np.allclose(p1, p2) def proc_form(): """ This is for me to run against the class_form() function to see which, if either, runs faster. They within a few ms of each other on my machine with Python 3.5.1""" dt = 1. std_z = 10.1 x = np.array([[0.], [0.]]) F = np.array([[1., dt], [0., 1.]]) H = np.array([[1., 0.]]) P = np.eye(2) R = np.eye(1)*std_z**2 Q = Q_discrete_white_noise(2, dt, 5.1) pos = 0. for t in range(2000): z = pos + random.randn() * std_z pos += 100 # perform kalman filtering x, P = predict(x, P, F, Q) x, P, _ = update(z, R, x, P, H) def class_form(): dt = 1. std_z = 10.1 f = const_vel_filter(dt, x0=2, R_std=std_z, Q_std=5.1) pos = 0. for t in range(2000): z = pos + random.randn() * std_z pos += 100 f.predict() f.update(z) def test_z_dim(): f = const_vel_filter(1.0, x0=2, R_std=1., Q_var=5.1) f.test_matrix_dimensions() f.update(3.) assert f.x.shape == (2,) f.update([3]) assert f.x.shape == (2,) f.update(np.array([[3]])) assert f.x.shape == (2,) try: f.update(np.array([[[3]]])) assert False, "filter should have asserted that [[[3]]] is not a valid form for z" except: pass f = const_vel_filter_2d(1.0, R_std=1., Q_var=5.1) try: f.update(3) assert False, "filter should have asserted that 3 is not a valid form for z" except: pass try: f.update([3]) assert False, "filter should have asserted that [3] is not a valid form for z" except: pass try: f.update([3, 3]) assert False, "filter should have asserted that [3] is not a valid form for z" except: pass try: f.update([[3, 3]]) assert False, "filter should have asserted that [3] is not a valid form for z" except: pass f = const_vel_filter_2d(1.0, R_std=1., Q_var=5.1) f.update([[3], [3]]) f.update(np.array([[3], [3]])) # now make sure test_matrix_dimensions() is working f.test_matrix_dimensions() try: f.H = 3 f.test_matrix_dimensions() assert False, "test_matrix_dimensions should have asserted on shape of H" except: pass f = const_vel_filter_2d(1.0, R_std=1., Q_var=5.1) try: f.R = 3 f.test_matrix_dimensions() assert False, "test_matrix_dimensions should have asserted on shape of R" except: pass try: f.R = [3] f.test_matrix_dimensions() assert False, "test_matrix_dimensions should have asserted on shape of R" except: pass try: f.R = [3, 4.] f.test_matrix_dimensions() assert False, "test_matrix_dimensions should have asserted on shape of R" except: pass f.R = np.diag([3, 4.]) f.test_matrix_dimensions() f = const_vel_filter(1.0, x0=2, R_std=1., Q_var=5.1) #test case where x is 1d array f.update([[3]]) f.test_matrix_dimensions(z=3.) f.test_matrix_dimensions(z=[3.]) # test case whre x is 2d array f.x = np.array([[0., 0.]]).T f.update([[3]]) f.test_matrix_dimensions(z=3.) f.test_matrix_dimensions(z=[3.]) try: f.test_matrix_dimensions(z=[[3.]]) assert False, "test_matrix_dimensions should have asserted on shape of z" except: pass f = const_vel_filter_2d(1.0, R_std=1., Q_var=5.1) # test for 1D value for x, then set to a 2D vector and try again for i in range(2): try: f.test_matrix_dimensions(z=3.) assert False, "test_matrix_dimensions should have asserted on shape of z" except: pass try: f.test_matrix_dimensions(z=[3.]) assert False, "test_matrix_dimensions should have asserted on shape of z" except: pass try: f.test_matrix_dimensions(z=[3., 3.]) assert False, "test_matrix_dimensions should have asserted on shape of z" except: pass f.test_matrix_dimensions(z=[[3.], [3.]]) f.x = np.array([[1, 2, 3, 4.]]).T def test_default_dims(): kf = KalmanFilter(dim_x=3, dim_z=1) kf.predict() kf.update(np.array([[1.]]).T) def test_functions(): x, P = predict(x=10., P=3., u=1., Q=2.**2) x, P = update(x=x, P=P, z=12., R=3.5**2) x, P = predict(x=np.array([10.]), P=np.array([[3.]]), Q=2.**2) x, P = update(x=x, P=P, z=12., H=np.array([[1.]]), R=np.array([[3.5**2]])) x = np.array([1., 0]) P = np.diag([1., 1]) Q = np.diag([0., 0]) H = np.array([[1., 0]]) x, P = predict(x=x, P=P, Q=Q) assert x.shape == (2,) assert P.shape == (2, 2) x, P = update(x, P, z=[1], R=np.array([[1.]]), H=H) assert x[0] == 1 and x[1] == 0 # test velocity predictions x, P = predict(x=x, P=P, Q=Q) assert x[0] == 1 and x[1] == 0 x[1] = 1. F = np.array([[1., 1], [0, 1]]) x, P = predict(x=x, F=F, P=P, Q=Q) assert x[0] == 2 and x[1] == 1 x, P = predict(x=x, F=F, P=P, Q=Q) assert x[0] == 3 and x[1] == 1 def test_z_checks(): kf = KalmanFilter(dim_x=3, dim_z=1) kf.update(3.) kf.update([3]) kf.update((3)) kf.update([[3]]) kf.update(np.array([[3]])) try: kf.update([[3, 3]]) assert False, "accepted bad z shape" except ValueError: pass kf = KalmanFilter(dim_x=3, dim_z=2) kf.update([3, 4]) kf.update([[3, 4]]) kf.update(np.array([[3, 4]])) kf.update(np.array([[3, 4]]).T) if __name__ == "__main__": DO_PLOT = True test_functions() test_default_dims() test_z_checks() test_z_dim() test_batch_filter() test_procedural_batch_filter() test_univariate() test_noisy_1d() test_noisy_11d() filterpy-1.4.5/filterpy/kalman/tests/test_mmae.py000066400000000000000000000116431335747705400221760ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy.random as random import numpy as np import matplotlib.pyplot as plt from filterpy.kalman import KalmanFilter, MMAEFilterBank from numpy import array from filterpy.common import Q_discrete_white_noise, Saver import matplotlib.pyplot as plt from numpy.random import randn from math import sin, cos, radians DO_PLOT = False class NoisySensor(object): def __init__(self, noise_factor=1): self.noise_factor = noise_factor def sense(self, pos): return (pos[0] + randn()*self.noise_factor, pos[1] + randn()*self.noise_factor) def angle_between(x, y): return min(y-x, y-x+360, y-x-360, key=abs) class ManeuveringTarget(object): def __init__(self, x0, y0, v0, heading): self.x = x0 self.y = y0 self.vel = v0 self.hdg = heading self.cmd_vel = v0 self.cmd_hdg = heading self.vel_step = 0 self.hdg_step = 0 self.vel_delta = 0 self.hdg_delta = 0 def update(self): vx = self.vel * cos(radians(90-self.hdg)) vy = self.vel * sin(radians(90-self.hdg)) self.x += vx self.y += vy if self.hdg_step > 0: self.hdg_step -= 1 self.hdg += self.hdg_delta if self.vel_step > 0: self.vel_step -= 1 self.vel += self.vel_delta return (self.x, self.y) def set_commanded_heading(self, hdg_degrees, steps): self.cmd_hdg = hdg_degrees self.hdg_delta = angle_between(self.cmd_hdg, self.hdg) / steps if abs(self.hdg_delta) > 0: self.hdg_step = steps else: self.hdg_step = 0 def set_commanded_speed(self, speed, steps): self.cmd_vel = speed self.vel_delta = (self.cmd_vel - self.vel) / steps if abs(self.vel_delta) > 0: self.vel_step = steps else: self.vel_step = 0 def make_cv_filter(dt, noise_factor): cvfilter = KalmanFilter(dim_x = 2, dim_z=1) cvfilter.x = array([0., 0.]) cvfilter.P *= 3 cvfilter.R *= noise_factor**2 cvfilter.F = array([[1, dt], [0, 1]], dtype=float) cvfilter.H = array([[1, 0]], dtype=float) cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02) return cvfilter def make_ca_filter(dt, noise_factor): cafilter = KalmanFilter(dim_x=3, dim_z=1) cafilter.x = array([0., 0., 0.]) cafilter.P *= 3 cafilter.R *= noise_factor**2 cafilter.Q = Q_discrete_white_noise(dim=3, dt=dt, var=0.02) cafilter.F = array([[1, dt, 0.5*dt*dt], [0, 1, dt], [0, 0, 1]], dtype=float) cafilter.H = array([[1, 0, 0]], dtype=float) return cafilter def generate_data(steady_count, noise_factor): t = ManeuveringTarget(x0=0, y0=0, v0=0.3, heading=0) xs = [] ys = [] for i in range(30): x, y = t.update() xs.append(x) ys.append(y) t.set_commanded_heading(310, 25) t.set_commanded_speed(1, 15) for i in range(steady_count): x, y = t.update() xs.append(x) ys.append(y) ns = NoisySensor(noise_factor=noise_factor) pos = array(list(zip(xs, ys))) zs = array([ns.sense(p) for p in pos]) return pos, zs def test_MMAE2(): dt = 0.1 pos, zs = generate_data(120, noise_factor=0.6) z_xs = zs[:, 0] dt = 0.1 ca = make_ca_filter(dt, noise_factor=0.6) cv = make_ca_filter(dt, noise_factor=0.6) cv.F[:, 2] = 0 # remove acceleration term cv.P[2, 2] = 0 cv.Q[2, 2] = 0 filters = [cv, ca] bank = MMAEFilterBank(filters, (0.5, 0.5), dim_x=3, H=ca.H) xs, probs = [], [] cvxs, caxs = [], [] s = Saver(bank) for i, z in enumerate(z_xs): bank.predict() bank.update(z) xs.append(bank.x[0]) cvxs.append(cv.x[0]) caxs.append(ca.x[0]) print(i, cv.likelihood, ca.likelihood, bank.p) s.save() probs.append(bank.p[0] / bank.p[1]) s.to_array() if DO_PLOT: plt.subplot(121) plt.plot(xs) plt.plot(pos[:, 0]) plt.subplot(122) plt.plot(probs) plt.title('probability ratio p(cv)/p(ca)') plt.figure() plt.plot(cvxs, label='CV') plt.plot(caxs, label='CA') plt.plot(pos[:, 0]) plt.legend() plt.figure() plt.plot(xs) plt.plot(pos[:, 0]) return bank if __name__ == '__main__': DO_PLOT = True test_MMAE2()filterpy-1.4.5/filterpy/kalman/tests/test_rts.py000066400000000000000000000031031335747705400220570ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy as np from numpy import random import matplotlib.pyplot as plt from filterpy.kalman import KalmanFilter DO_PLOT = False def test_rts(): fk = KalmanFilter(dim_x=2, dim_z=1) fk.x = np.array([-1., 1.]) # initial state (location and velocity) fk.F = np.array([[1.,1.], [0.,1.]]) # state transition matrix fk.H = np.array([[1.,0.]]) # Measurement function fk.P = .01 # covariance matrix fk.R = 5 # state uncertainty fk.Q = 0.001 # process uncertainty zs = [t + random.randn()*4 for t in range(40)] mu, cov, _, _ = fk.batch_filter (zs) mus = [x[0] for x in mu] M, P, _, _ = fk.rts_smoother(mu, cov) if DO_PLOT: p1, = plt.plot(zs,'cyan', alpha=0.5) p2, = plt.plot(M[:,0],c='b') p3, = plt.plot(mus,c='r') p4, = plt.plot([0, len(zs)], [0, len(zs)], 'g') # perfect result plt.legend([p1, p2, p3, p4], ["measurement", "RKS", "KF output", "ideal"], loc=4) plt.show() if __name__ == '__main__': DO_PLOT = True test_rts() filterpy-1.4.5/filterpy/kalman/tests/test_sensor_fusion.py000066400000000000000000000057711335747705400241600ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy.random as random from numpy.random import randn import numpy as np import matplotlib.pyplot as plt from filterpy.kalman import KalmanFilter from numpy import array, asarray DO_PLOT = False SEED = 1124 def single_measurement_test(): dt = 0.1 sigma = 2. kf2 = KalmanFilter(dim_x=2, dim_z=1) kf2.F = array ([[1., dt], [0., 1.]]) kf2.H = array ([[1., 0.]]) kf2.x = array ([[0.], [1.]]) kf2.Q = array ([[dt**3/3, dt**2/2], [dt**2/2, dt]]) * 0.02 kf2.P *= 100 kf2.R[0,0] = sigma**2 random.seed(SEED) xs = [] zs = [] nom = [] for i in range(1, 100): m0 = i + randn()*sigma z = array([[m0]]) kf2.predict() kf2.update(z) xs.append(kf2.x.T[0]) zs.append(z.T[0]) nom.append(i) xs = asarray(xs) zs = asarray(zs) nom = asarray(nom) res = nom-xs[:,0] std_dev = np.std(res) print('std: {:.3f}'.format (std_dev)) global DO_PLOT if DO_PLOT: plt.subplot(211) plt.plot(xs[:,0]) #plt.plot(zs[:,0]) plt.subplot(212) plt.plot(res) plt.show() return std_dev def sensor_fusion_test(wheel_sigma=2., gps_sigma=4.): dt = 0.1 kf2 = KalmanFilter(dim_x=2, dim_z=2) kf2.F = array ([[1., dt], [0., 1.]]) kf2.H = array ([[1., 0.], [1., 0.]]) kf2.x = array ([[0.], [0.]]) kf2.Q = array ([[dt**3/3, dt**2/2], [dt**2/2, dt]]) * 0.02 kf2.P *= 100 kf2.R[0,0] = wheel_sigma**2 kf2.R[1,1] = gps_sigma**2 random.seed(SEED) xs = [] zs = [] nom = [] for i in range(1, 100): m0 = i + randn()*wheel_sigma m1 = i + randn()*gps_sigma if gps_sigma >1e40: m1 = -1e40 z = array([[m0], [m1]]) kf2.predict() kf2.update(z) xs.append(kf2.x.T[0]) zs.append(z.T[0]) nom.append(i) xs = asarray(xs) zs = asarray(zs) nom = asarray(nom) res = nom-xs[:,0] std_dev = np.std(res) print('fusion std: {:.3f}'.format (np.std(res))) if DO_PLOT: plt.subplot(211) plt.plot(xs[:,0]) #plt.plot(zs[:,0]) #plt.plot(zs[:,1]) plt.subplot(212) plt.axhline(0) plt.plot(res) plt.show() print(kf2.Q) print(kf2.K) return std_dev def test_fusion(): std1 = sensor_fusion_test() std2 = single_measurement_test() assert (std1 < std2) if __name__ == "__main__": DO_PLOT=True sensor_fusion_test(2,4e100) single_measurement_test() test_fusion() filterpy-1.4.5/filterpy/kalman/tests/test_sqrtkf.py000066400000000000000000000056541335747705400225760ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy.random as random import numpy as np import matplotlib.pyplot as plt from filterpy.common import Saver from filterpy.kalman import SquareRootKalmanFilter, KalmanFilter DO_PLOT = False def test_noisy_1d(): f = KalmanFilter (dim_x=2, dim_z=1) f.x = np.array([[2.], [0.]]) # initial state (location and velocity) f.F = np.array([[1.,1.], [0.,1.]]) # state transition matrix f.H = np.array([[1.,0.]]) # Measurement function f.P *= 1000. # covariance matrix f.R *= 5 # state uncertainty f.Q *= 0.0001 # process uncertainty fsq = SquareRootKalmanFilter (dim_x=2, dim_z=1) fsq.x = np.array([[2.], [0.]]) # initial state (location and velocity) fsq.F = np.array([[1.,1.], [0.,1.]]) # state transition matrix fsq.H = np.array([[1.,0.]]) # Measurement function fsq.P = np.eye(2) * 1000. # covariance matrix fsq.R *= 5 # state uncertainty fsq.Q *= 0.0001 # process uncertainty # does __repr__ work? str(fsq) measurements = [] results = [] zs = [] s = Saver(fsq) for t in range (100): # create measurement = t plus white noise z = t + random.randn()*20 zs.append(z) # perform kalman filtering f.update(z) f.predict() fsq.update(z) fsq.predict() assert abs(f.x[0,0] - fsq.x[0,0]) < 1.e-12 assert abs(f.x[1,0] - fsq.x[1,0]) < 1.e-12 # save data results.append (f.x[0,0]) measurements.append(z) s.save() s.to_array() for i in range(f.P.shape[0]): assert abs(f.P[i,i] - fsq.P[i,i]) < 0.01 # now do a batch run with the stored z values so we can test that # it is working the same as the recursive implementation. # give slightly different P so result is slightly different f.x = np.array([[2.,0]]).T f.P = np.eye(2)*100. m,c,_,_ = f.batch_filter(zs,update_first=False) # plot data if DO_PLOT: p1, = plt.plot(measurements,'r', alpha=0.5) p2, = plt.plot (results,'b') p4, = plt.plot(m[:,0], 'm') p3, = plt.plot ([0,100],[0,100], 'g') # perfect result plt.legend([p1,p2, p3, p4], ["noisy measurement", "KF output", "ideal", "batch"], loc=4) plt.show() if __name__ == "__main__": DO_PLOT = True test_noisy_1d()filterpy-1.4.5/filterpy/kalman/tests/test_ukf.py000066400000000000000000000574121335747705400220500ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=C0103 # pylint: disable=C0111 # ignore snakecase warning, missing docstring """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) from math import cos, sin import matplotlib.pyplot as plt import numpy.random as random from numpy.random import randn from numpy import asarray import numpy as np from pytest import approx from scipy.spatial.distance import mahalanobis as scipy_mahalanobis from filterpy.kalman import UnscentedKalmanFilter from filterpy.kalman import (unscented_transform, MerweScaledSigmaPoints, JulierSigmaPoints, SimplexSigmaPoints, KalmanFilter) from filterpy.common import Q_discrete_white_noise, Saver import filterpy.stats as stats DO_PLOT = False def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000) sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3) sp3 = SimplexSigmaPoints(n=2) # test __repr__ doesn't crash str(sp0) str(sp1) str(sp2) str(sp3) w0 = sp0.Wm w1 = sp1.Wm w2 = sp2.Wm w3 = sp3.Wm Xi0 = sp0.sigma_points(x, P) Xi1 = sp1.sigma_points(x, P) Xi2 = sp2.sigma_points(x, P) Xi3 = sp3.sigma_points(x, P) assert max(Xi1[:, 0]) > max(Xi0[:, 0]) assert max(Xi1[:, 1]) > max(Xi0[:, 1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i, 0]-x[0, 0])*w0[i] + x[0, 0], (Xi0[i, 1]-x[0, 1])*w0[i] + x[0, 1], color='blue', label='Julier low $\kappa$') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0, 0], (Xi1[i, 1]-x[0, 1]) * w1[i] + x[0, 1], color='green', label='Julier high $\kappa$') for i in range(Xi2.shape[0]): plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0], (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1], color='red') for i in range(Xi3.shape[0]): plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0], (Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1], color='black', label='Simplex') stats.plot_covariance_ellipse([1, 2], P) def test_scaled_weights(): for n in range(1, 5): for alpha in np.linspace(0.99, 1.01, 100): for beta in range(2): for kappa in range(2): sp = MerweScaledSigmaPoints(n, alpha, 0, 3-n) assert abs(sum(sp.Wm) - 1) < 1.e-1 assert abs(sum(sp.Wc) - 1) < 1.e-1 def test_julier_sigma_points_1D(): """ tests passing 1D data into sigma_points""" kappa = 0. sp = JulierSigmaPoints(1, kappa) Wm, Wc = sp.Wm, sp.Wc assert np.allclose(Wm, Wc, 1e-12) assert len(Wm) == 3 mean = 5 cov = 9 Xi = sp.sigma_points(mean, cov) xm, ucov = unscented_transform(Xi, Wm, Wc, 0) # sum of weights*sigma points should be the original mean m = 0.0 for x, w in zip(Xi, Wm): m += x*w assert abs(m-mean) < 1.e-12 assert abs(xm[0] - mean) < 1.e-12 assert abs(ucov[0, 0] - cov) < 1.e-12 assert Xi.shape == (3, 1) def test_simplex_sigma_points_1D(): """ tests passing 1D data into sigma_points""" sp = SimplexSigmaPoints(1) Wm, Wc = sp.Wm, sp.Wc assert np.allclose(Wm, Wc, 1e-12) assert len(Wm) == 2 mean = 5 cov = 9 Xi = sp.sigma_points(mean, cov) xm, ucov = unscented_transform(Xi, Wm, Wc, 0) # sum of weights*sigma points should be the original mean m = 0.0 for x, w in zip(Xi, Wm): m += x*w assert abs(m-mean) < 1.e-12 assert abs(xm[0] - mean) < 1.e-12 assert abs(ucov[0, 0]-cov) < 1.e-12 assert Xi.shape == (2, 1) class RadarSim(object): def __init__(self, dt): self.x = 0 self.dt = dt def get_range(self): vel = 100 + 5*randn() alt = 1000 + 10*randn() self.x += vel*self.dt v = self.x * 0.05*randn() rng = (self.x**2 + alt**2)**.5 + v return rng def test_radar(): def fx(x, dt): A = np.eye(3) + dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) return A.dot(x) def hx(x): return [np.sqrt(x[0]**2 + x[2]**2)] dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=0.) kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp) assert np.allclose(kf.x, kf.x_prior) assert np.allclose(kf.P, kf.P_prior) # test __repr__ doesn't crash str(kf) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 100. radar = RadarSim(dt) t = np.arange(0, 20+dt, dt) n = len(t) xs = np.zeros((n, 3)) random.seed(200) rs = [] for i in range(len(t)): r = radar.get_range() kf.predict() kf.update(z=[r]) xs[i, :] = kf.x rs.append(r) # test mahalanobis a = np.zeros(kf.y.shape) maha = scipy_mahalanobis(a, kf.y, kf.SI) assert kf.mahalanobis == approx(maha) if DO_PLOT: print(xs[:, 0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:, 0]) plt.subplot(312) plt.plot(t, xs[:, 1]) plt.subplot(313) plt.plot(t, xs[:, 2]) def test_linear_2d_merwe(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P *= 1.1 # test __repr__ doesn't crash str(kf) zs = [[i+randn()*0.1, i+randn()*0.1] for i in range(20)] Ms, Ps = kf.batch_filter(zs) smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dts=dt) if DO_PLOT: plt.figure() zs = np.asarray(zs) plt.plot(zs[:, 0], marker='+') plt.plot(Ms[:, 0], c='b') plt.plot(smooth_x[:, 0], smooth_x[:, 2], c='r') print(smooth_x) def test_linear_2d_simplex(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = SimplexSigmaPoints(n=4) kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P *= 0.0001 zs = [] for i in range(20): z = np.array([i+randn()*0.1, i+randn()*0.1]) zs.append(z) Ms, Ps = kf.batch_filter(zs) smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dts=dt) if DO_PLOT: zs = np.asarray(zs) plt.plot(Ms[:, 0]) plt.plot(smooth_x[:, 0], smooth_x[:, 2]) print(smooth_x) def test_linear_1d(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1., dt], [0, 1]]) return np.dot(F, x) def hx(x): return np.array([x[0]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) z = np.array([2.]) kf.predict() kf.update(z) zs = [] for i in range(50): z = np.array([i + randn()*0.1]) zs.append(z) kf.predict() kf.update(z) print('K', kf.K.T) print('x', kf.x) def test_batch_missing_data(): """ batch filter should accept missing data with None in the measurements """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P *= 0.0001 zs = [] for i in range(20): z = np.array([i + randn()*0.1, i + randn()*0.1]) zs.append(z) zs[2] = None Rs = [1]*len(zs) Rs[2] = None Ms, Ps = kf.batch_filter(zs) def test_rts(): def fx(x, dt): A = np.eye(3) + dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) f = np.dot(A, x) return f def hx(x): return [np.sqrt(x[0]**2 + x[2]**2)] dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=1.) kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 100. radar = RadarSim(dt) t = np.arange(0, 20 + dt, dt) n = len(t) xs = np.zeros((n, 3)) random.seed(200) rs = [] for i in range(len(t)): r = radar.get_range() kf.predict() kf.update(z=[r]) xs[i, :] = kf.x rs.append(r) kf.x = np.array([0., 90., 1100.]) kf.P = np.eye(3) * 100 M, P = kf.batch_filter(rs) assert np.array_equal(M, xs), "Batch filter generated different output" Qs = [kf.Q] * len(t) M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs) if DO_PLOT: print(xs[:, 0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:, 0]) plt.plot(t, M2[:, 0], c='g') plt.subplot(312) plt.plot(t, xs[:, 1]) plt.plot(t, M2[:, 1], c='g') plt.subplot(313) plt.plot(t, xs[:, 2]) plt.plot(t, M2[:, 2], c='g') def test_fixed_lag(): def fx(x, dt): A = np.eye(3) + dt * np.array([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) f = np.dot(A, x) return f def hx(x): return [np.sqrt(x[0]**2 + x[2]**2)] dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=0) kf = UnscentedKalmanFilter(3, 1, dt, fx=fx, hx=hx, points=sp) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 1. radar = RadarSim(dt) t = np.arange(0, 20 + dt, dt) n = len(t) xs = np.zeros((n, 3)) random.seed(200) rs = [] M = [] P = [] N = 10 flxs = [] for i in range(len(t)): r = radar.get_range() kf.predict() kf.update(z=[r]) xs[i, :] = kf.x flxs.append(kf.x) rs.append(r) M.append(kf.x) P.append(kf.P) print(i) if i == 20 and len(M) >= N: try: M2, P2, K = kf.rts_smoother(Xs=np.asarray(M)[-N:], Ps=np.asarray(P)[-N:]) flxs[-N:] = M2 except: print('except', i) kf.x = np.array([0., 90., 1100.]) kf.P = np.eye(3) * 100 M, P = kf.batch_filter(rs) Qs = [kf.Q]*len(t) M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs) flxs = np.asarray(flxs) print(xs[:, 0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:, 0]) plt.plot(t, flxs[:, 0], c='r') plt.plot(t, M2[:, 0], c='g') plt.subplot(312) plt.plot(t, xs[:, 1]) plt.plot(t, flxs[:, 1], c='r') plt.plot(t, M2[:, 1], c='g') plt.subplot(313) plt.plot(t, xs[:, 2]) plt.plot(t, flxs[:, 2], c='r') plt.plot(t, M2[:, 2], c='g') def test_circle(): from filterpy.kalman import KalmanFilter from math import radians def hx(x): radius = x[0] angle = x[1] x = cos(radians(angle)) * radius y = sin(radians(angle)) * radius return np.array([x, y]) def fx(x, dt): return np.array([x[0], x[1] + x[2], x[2]]) std_noise = .1 sp = JulierSigmaPoints(n=3, kappa=0.) f = UnscentedKalmanFilter(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, points=sp) f.x = np.array([50., 90., 0]) f.P *= 100 f.R = np.eye(2)*(std_noise**2) f.Q = np.eye(3)*.001 f.Q[0, 0] = 0 f.Q[2, 2] = 0 kf = KalmanFilter(dim_x=6, dim_z=2) kf.x = np.array([50., 0., 0, 0, .0, 0.]) F = np.array([[1., 1., .5, 0., 0., 0.], [0., 1., 1., 0., 0., 0.], [0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 1., .5], [0., 0., 0., 0., 1., 1.], [0., 0., 0., 0., 0., 1.]]) kf.F = F kf.P *= 100 kf.H = np.array([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]]) kf.R = f.R kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001) kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001) results = [] zs = [] kfxs = [] for t in range(12000): a = t / 30 + 90 x = cos(radians(a)) * 50. + randn() * std_noise y = sin(radians(a)) * 50. + randn() * std_noise # create measurement = t plus white noise z = np.array([x, y]) zs.append(z) f.predict() f.update(z) kf.predict() kf.update(z) # save data results.append(hx(f.x)) kfxs.append(kf.x) results = np.asarray(results) zs = np.asarray(zs) kfxs = np.asarray(kfxs) print(results) if DO_PLOT: plt.plot(zs[:, 0], zs[:, 1], c='r', label='z') plt.plot(results[:, 0], results[:, 1], c='k', label='UKF') plt.plot(kfxs[:, 0], kfxs[:, 3], c='g', label='KF') plt.legend(loc='best') plt.axis('equal') def kf_circle(): from filterpy.kalman import KalmanFilter from math import radians import math def hx(x): radius = x[0] angle = x[1] x = cos(radians(angle)) * radius y = sin(radians(angle)) * radius return np.array([x, y]) def fx(x, dt): return np.array([x[0], x[1] + x[2], x[2]]) def hx_inv(x, y): angle = math.atan2(y, x) radius = math.sqrt(x*x + y*y) return np.array([radius, angle]) std_noise = .1 kf = KalmanFilter(dim_x=3, dim_z=2) kf.x = np.array([50., 0., 0.]) F = np.array([[1., 0, 0.], [0., 1., 1.], [0., 0., 1.]]) kf.F = F kf.P *= 100 kf.H = np.array([[1, 0, 0], [0, 1, 0]]) kf.R = np.eye(2)*(std_noise**2) #kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001) zs = [] kfxs = [] for t in range(2000): a = t / 30 + 90 x = cos(radians(a)) * 50. + randn() * std_noise y = sin(radians(a)) * 50. + randn() * std_noise z = hx_inv(x, y) zs.append(z) kf.predict() kf.update(z) # save data kfxs.append(kf.x) zs = np.asarray(zs) kfxs = np.asarray(kfxs) if DO_PLOT: plt.plot(zs[:, 0], zs[:, 1], c='r', label='z') plt.plot(kfxs[:, 0], kfxs[:, 1], c='g', label='KF') plt.legend(loc='best') plt.axis('equal') def two_radar(): # code is not complete - I was using to test RTS smoother. very similar # to two_radary.py in book. import numpy as np import matplotlib.pyplot as plt from numpy import array from numpy.linalg import norm from numpy.random import randn from math import atan2 from filterpy.common import Q_discrete_white_noise class RadarStation(object): def __init__(self, pos, range_std, bearing_std): self.pos = asarray(pos) self.range_std = range_std self.bearing_std = bearing_std def reading_of(self, ac_pos): """ Returns range and bearing to aircraft as tuple. bearing is in radians. """ diff = np.subtract(self.pos, ac_pos) rng = norm(diff) brg = atan2(diff[1], diff[0]) return rng, brg def noisy_reading(self, ac_pos): rng, brg = self.reading_of(ac_pos) rng += randn() * self.range_std brg += randn() * self.bearing_std return rng, brg class ACSim(object): def __init__(self, pos, vel, vel_std): self.pos = asarray(pos, dtype=float) self.vel = asarray(vel, dtype=float) self.vel_std = vel_std def update(self): vel = self.vel + (randn() * self.vel_std) self.pos += vel return self.pos dt = 1. def hx(x): r1, b1 = hx.R1.reading_of((x[0], x[2])) r2, b2 = hx.R2.reading_of((x[0], x[2])) return array([r1, b1, r2, b2]) def fx(x, dt): x_est = x.copy() x_est[0] += x[1]*dt x_est[2] += x[3]*dt return x_est vx, vy = 0.1, 0.1 f = UnscentedKalmanFilter(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0) aircraft = ACSim((100, 100), (vx*dt, vy*dt), 0.00000002) range_std = 0.001 # 1 meter bearing_std = 1./1000 # 1mrad R1 = RadarStation((0, 0), range_std, bearing_std) R2 = RadarStation((200, 0), range_std, bearing_std) hx.R1 = R1 hx.R2 = R2 f.x = array([100, vx, 100, vy]) f.R = np.diag([range_std**2, bearing_std**2, range_std**2, bearing_std**2]) q = Q_discrete_white_noise(2, var=0.0002, dt=dt) f.Q[0:2, 0:2] = q f.Q[2:4, 2:4] = q f.P = np.diag([.1, 0.01, .1, 0.01]) track = [] zs = [] for i in range(int(300/dt)): pos = aircraft.update() r1, b1 = R1.noisy_reading(pos) r2, b2 = R2.noisy_reading(pos) z = np.array([r1, b1, r2, b2]) zs.append(z) track.append(pos.copy()) zs = asarray(zs) xs, Ps, Pxz, pM, pP = f.batch_filter(zs) ms, _, _ = f.rts_smoother(xs, Ps) track = asarray(track) time = np.arange(0, len(xs) * dt, dt) plt.figure() plt.subplot(411) plt.plot(time, track[:, 0]) plt.plot(time, xs[:, 0]) plt.legend(loc=4) plt.xlabel('time (sec)') plt.ylabel('x position (m)') plt.tight_layout() plt.subplot(412) plt.plot(time, track[:, 1]) plt.plot(time, xs[:, 2]) plt.legend(loc=4) plt.xlabel('time (sec)') plt.ylabel('y position (m)') plt.tight_layout() plt.subplot(413) plt.plot(time, xs[:, 1]) plt.plot(time, ms[:, 1]) plt.legend(loc=4) plt.ylim([0, 0.2]) plt.xlabel('time (sec)') plt.ylabel('x velocity (m/s)') plt.tight_layout() plt.subplot(414) plt.plot(time, xs[:, 3]) plt.plot(time, ms[:, 3]) plt.ylabel('y velocity (m/s)') plt.legend(loc=4) plt.xlabel('time (sec)') plt.tight_layout() plt.show() def test_linear_rts(): """ for a linear model the Kalman filter and UKF should produce nearly identical results. Test code mostly due to user gboehl as reported in GitHub issue #97, though I converted it from an AR(1) process to constant velocity kinematic model. """ dt = 1.0 F = np.array([[1., dt], [.0, 1]]) H = np.array([[1., .0]]) def t_func(x, dt): F = np.array([[1., dt], [.0, 1]]) return np.dot(F, x) def o_func(x): return np.dot(H, x) sig_t = .1 # peocess sig_o = .00000001 # measurement N = 50 X_true, X_obs = [], [] for i in range(N): X_true.append([i + 1, 1.]) X_obs.append(i + 1 + np.random.normal(scale=sig_o)) X_true = np.array(X_true) X_obs = np.array(X_obs) oc = np.ones((1, 1)) * sig_o**2 tc = np.zeros((2, 2)) tc[1, 1] = sig_t**2 tc = Q_discrete_white_noise(dim=2, dt=dt, var=sig_t**2) points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=1) ukf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=dt, hx=o_func, fx=t_func, points=points) ukf.x = np.array([0., 1.]) ukf.R = np.copy(oc) ukf.Q = np.copy(tc) s = Saver(ukf) s.save() s.to_array() kf = KalmanFilter(dim_x=2, dim_z=1) kf.x = np.array([[0., 1]]).T kf.R = np.copy(oc) kf.Q = np.copy(tc) kf.H = np.copy(H) kf.F = np.copy(F) mu_ukf, cov_ukf = ukf.batch_filter(X_obs) x_ukf, _, _ = ukf.rts_smoother(mu_ukf, cov_ukf) mu_kf, cov_kf, _, _ = kf.batch_filter(X_obs) x_kf, _, _, _ = kf.rts_smoother(mu_kf, cov_kf) # check results of filtering are correct kfx = mu_kf[:, 0, 0] ukfx = mu_ukf[:, 0] kfxx = mu_kf[:, 1, 0] ukfxx = mu_ukf[:, 1] dx = kfx - ukfx dxx = kfxx - ukfxx # error in position should be smaller then error in velocity, hence # atol is different for the two tests. assert np.allclose(dx, 0, atol=1e-7) assert np.allclose(dxx, 0, atol=1e-6) # now ensure the RTS smoothers gave nearly identical results kfx = x_kf[:, 0, 0] ukfx = x_ukf[:, 0] kfxx = x_kf[:, 1, 0] ukfxx = x_ukf[:, 1] dx = kfx - ukfx dxx = kfxx - ukfxx assert np.allclose(dx, 0, atol=1e-7) assert np.allclose(dxx, 0, atol=1e-6) return ukf def _test_log_likelihood(): from filterpy.common import Saver def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) z_std = 0.1 kf.R = np.diag([z_std**2, z_std**2]) # 1 standard kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=1.1**2, block_size=2) kf.x = np.array([-1., 1., -1., 1]) kf.P *= 1. zs = [[i+randn()*z_std, i+randn()*z_std] for i in range(40)] s = Saver(kf) for z in zs: kf.predict() kf.update(z) print(kf.x, kf.log_likelihood, kf.P.diagonal()) s.save() # test mahalanobis a = np.zeros(kf.y.shape) maha = scipy_mahalanobis(a, kf.y, kf.SI) assert kf.mahalanobis == approx(maha) s.to_array() plt.plot(s.x[:, 0], s.x[:, 2]) if __name__ == "__main__": plt.close('all') test_scaled_weights() _test_log_likelihood() test_linear_rts() DO_PLOT = True test_sigma_plot() test_linear_1d() test_batch_missing_data() # #est_linear_2d() test_julier_sigma_points_1D() test_simplex_sigma_points_1D() test_fixed_lag() # DO_PLOT = True test_rts() kf_circle() test_circle() '''test_1D_sigma_points() plot_sigma_test () x = np.array([[1,2]]) P = np.array([[2, 1.2], [1.2, 2]])\ kappa = .1 xi,w = sigma_points (x,P,kappa) xm, cov = unscented_transform(xi, w)''' test_radar() test_sigma_plot() test_scaled_weights() #print('xi=\n',Xi) """ xm, cov = unscented_transform(Xi, W) print(xm) print(cov)""" # sigma_points ([5,2],9*np.eye(2), 2) #plt.legend() #plt.show() filterpy-1.4.5/filterpy/kalman/tests/ukf2.py000066400000000000000000001345621335747705400210750ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. 6""" from __future__ import (absolute_import, division, print_function, unicode_literals) import matplotlib.pyplot as plt import numpy.random as random from numpy.random import randn from numpy import asarray import numpy as np from filterpy.kalman import UnscentedKalmanFilter as UKF from filterpy.kalman import (unscented_transform, MerweScaledSigmaPoints, JulierSigmaPoints, SimplexSigmaPoints) from filterpy.common import Q_discrete_white_noise import filterpy.stats as stats from math import cos, sin class UnscentedKalmanFilter2(object): # pylint: disable=too-many-instance-attributes # pylint: disable=C0103 r""" Implements the Scaled Unscented Kalman filter (UKF) as defined by Simon Julier in [1], using the formulation provided by Wan and Merle in [2]. This filter scales the sigma points to avoid strong nonlinearities. You will have to set the following attributes after constructing this object for the filter to perform properly. Attributes ---------- x : numpy.array(dim_x) state estimate vector P : numpy.array(dim_x, dim_x) covariance estimate matrix R : numpy.array(dim_z, dim_z) measurement noise matrix Q : numpy.array(dim_x, dim_x) process noise matrix You may read the following attributes. Readable Attributes ------------------- K : numpy.array Kalman gain y : numpy.array innovation residual x : numpy.array(dim_x) predicted/updated state (result of predict()/update()) P : numpy.array(dim_x, dim_x) predicted/updated covariance matrix (result of predict()/update()) likelihood : scalar Likelihood of last measurement update. log_likelihood : scalar Log likelihood of last measurement update. References ---------- .. [1] Julier, Simon J. "The scaled unscented transformation," American Control Converence, 2002, pp 4555-4559, vol 6. Online copy: https://www.cs.unc.edu/~welch/kalman/media/pdf/ACC02-IEEE1357.PDF .. [2] E. A. Wan and R. Van der Merwe, “The unscented Kalman filter for nonlinear estimation,” in Proc. Symp. Adaptive Syst. Signal Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000. Online Copy: https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf """ def __init__(self, dim_x, dim_z, dt, hx, fx, points, sqrt_fn=None, x_mean_fn=None, z_mean_fn=None, residual_x=None, residual_z=None): r""" Create a Kalman filter. You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter. Parameters ---------- dim_x : int Number of state variables for the filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4. dim_z : int Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2. dt : float Time between steps in seconds. hx : function(x) Measurement function. Converts state vector x into a measurement vector of shape (dim_z). fx : function(x,dt) function that returns the state x transformed by the state transistion function. dt is the time step in seconds. points : class Class which computes the sigma points and weights for a UKF algorithm. You can vary the UKF implementation by changing this class. For example, MerweScaledSigmaPoints implements the alpha, beta, kappa parameterization of Van der Merwe, and JulierSigmaPoints implements Julier's original kappa parameterization. See either of those for the required signature of this class if you want to implement your own. sqrt_fn : callable(ndarray), default = scipy.linalg.cholesky Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe's dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you. If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing. x_mean_fn : callable (sigma_points, weights), optional Function that computes the mean of the provided sigma points and weights. Use this if your state variable contains nonlinear values such as angles which cannot be summed. .. code-block:: Python def state_mean(sigmas, Wm): x = np.zeros(3) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] x[1] += s[1] * Wm[i] sum_sin += sin(s[2])*Wm[i] sum_cos += cos(s[2])*Wm[i] x[2] = atan2(sum_sin, sum_cos) return x z_mean_fn : callable (sigma_points, weights), optional Same as x_mean_fn, except it is called for sigma points which form the measurements after being passed through hx(). residual_x : callable (x, y), optional residual_z : callable (x, y), optional Function that computes the residual (difference) between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. One is for the state variable, the other is for the measurement state. .. code-block:: Python def residual(a, b): y = a[0] - b[0] if y > np.pi: y -= 2*np.pi if y < -np.pi: y = 2*np.pi return y References ---------- .. [3] S. Julier, J. Uhlmann, and H. Durrant-Whyte. "A new method for the nonlinear transformation of means and covariances in filters and estimators," IEEE Transactions on Automatic Control, 45(3), pp. 477-482 (March 2000). .. [4] E. A. Wan and R. Van der Merwe, “The Unscented Kalman filter for Nonlinear Estimation,” in Proc. Symp. Adaptive Syst. Signal Process., Commun. Contr., Lake Louise, AB, Canada, Oct. 2000. https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf .. [5] Wan, Merle "The Unscented Kalman Filter," chapter in *Kalman Filtering and Neural Networks*, John Wiley & Sons, Inc., 2001. .. [6] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models" (Doctoral dissertation) """ self.Q = eye(dim_x) self.R = eye(dim_z) self.x = zeros(dim_x) self.P = eye(dim_x) self._dim_x = dim_x self._dim_z = dim_z self.points_fn = points self._dt = dt self._num_sigmas = points.num_sigmas() self.hx = hx self.fx = fx self.x_mean = x_mean_fn self.z_mean = z_mean_fn self.log_likelihood = 0.0 if sqrt_fn is None: self.msqrt = cholesky else: self.msqrt = sqrt_fn # weights for the means and covariances. self.Wm, self.Wc = self.points_fn.weights() self.Wm = self.Wm.reshape((-1, 1)) self.Wc = self.Wc.reshape((-1, 1)) if residual_x is None: self.residual_x = np.subtract else: self.residual_x = residual_x if residual_z is None: self.residual_z = np.subtract else: self.residual_z = residual_z # sigma points transformed through f(x) and h(x) # variables for efficiency so we don't recreate every update self.sigmas_f = zeros((self._dim_x, self._num_sigmas)) self.sigmas_h = zeros((self._dim_z, self._num_sigmas)) def predict(self, dt=None, UT=None, fx_args=()): r""" Performs the predict step of the UKF. On return, self.x and self.P contain the predicted state (x) and covariance (P). ' Important: this MUST be called before update() is called for the first time. Parameters ---------- dt : double, optional If specified, the time step to be used for this prediction. self._dt is used if this is not provided. UT : function(sigmas, Wm, Wc, noise_cov), optional Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. fx_args : tuple, optional, default (,) optional arguments to be passed into fx() after the required state variable. """ if dt is None: dt = self._dt if not isinstance(fx_args, tuple): fx_args = (fx_args,) if UT is None: UT = unscented_transform2 # calculate sigma points for given mean and covariance sigmas = self.points_fn.sigma_points(self.x, self.P) for i in range(self._num_sigmas): self.sigmas_f[:, i] = self.fx(sigmas[:, i], dt, *fx_args) self.x, self.P = UT(self.sigmas_f, self.Wm, self.Wc, self.Q, self.x_mean, self.residual_x) def update(self, z, R=None, UT=None, hx_args=()): """ Update the UKF with the given measurements. On return, self.x and self.P contain the new mean and covariance of the filter. Parameters ---------- z : numpy.array of shape (dim_z) measurement vector R : numpy.array((dim_z, dim_z)), optional Measurement noise. If provided, overrides self.R for this function call. UT : function(sigmas, Wm, Wc, noise_cov), optional Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. hx_args : tuple, optional, default (,) arguments to be passed into Hx function after the required state variable. """ if z is None: return if not isinstance(hx_args, tuple): hx_args = (hx_args,) if UT is None: UT = unscented_transform2 if R is None: R = self.R elif isscalar(R): R = eye(self._dim_z) * R for i in range(self._num_sigmas): self.sigmas_h[:, i] = self.hx(self.sigmas_f[:, i], *hx_args) # mean and covariance of prediction passed through unscented transform zp, Pz = UT(self.sigmas_h, self.Wm, self.Wc, R, self.z_mean, self.residual_z) # compute cross variance of the state and the measurements Pxz = zeros((self._dim_x, self._dim_z)) for i in range(self._num_sigmas): dx = self.residual_x(self.sigmas_f[:, i:i+1], self.x) dz = self.residual_z(self.sigmas_h[:, i:i+1], zp) Pxz += self.Wc[i] * outer(dx, dz) self.K = dot(Pxz, inv(Pz)) # Kalman gain self.y = self.residual_z(z, zp) # residual self.x = self.x + dot(self.K, self.y) self.P = self.P - dot3(self.K, Pz, self.K.T) self.log_likelihood = multivariate_normal.logpdf( x=self.y, mean=np.zeros(len(self.y)), cov=Pz, allow_singular=True) @property def likelihood(self): return math.exp(self.log_likelihood) def batch_filter(self, zs, Rs=None, UT=None): """ Performs the UKF filter over the list of measurement in `zs`. Parameters ---------- zs : list-like list of measurements at each time step `self._dt` Missing measurements must be represented by 'None'. Rs : list-like, optional optional list of values to use for the measurement error covariance; a value of None in any position will cause the filter to use `self.R` for that time step. UT : function(sigmas, Wm, Wc, noise_cov), optional Optional function to compute the unscented transform for the sigma points passed through hx. Typically the default function will work - you can use x_mean_fn and z_mean_fn to alter the behavior of the unscented transform. Returns ------- means: ndarray((n,dim_x,1)) array of the state for each time step after the update. Each entry is an np.array. In other words `means[k,:]` is the state at step `k`. covariance: ndarray((n,dim_x,dim_x)) array of the covariances for each time step after the update. In other words `covariance[k,:,:]` is the covariance at step `k`. """ try: z = zs[0] except: assert not isscalar(zs), 'zs must be list-like' if self._dim_z == 1: assert isscalar(z) or (z.ndim==1 and len(z) == 1), \ 'zs must be a list of scalars or 1D, 1 element arrays' else: assert len(z) == self._dim_z, 'each element in zs must be a' \ '1D array of length {}'.format(self._dim_z) z_n = np.size(zs, 0) if Rs is None: Rs = [None] * z_n # mean estimates from Kalman Filter means = zeros((z_n, self._dim_x, 1)) # state covariances from Kalman Filter covariances = zeros((z_n, self._dim_x, self._dim_x)) for i, (z, r) in enumerate(zip(zs, Rs)): self.predict(UT=UT) self.update(z, r, UT=UT) means[i,:] = self.x covariances[i,:,:] = self.P return (means, covariances) def rts_smoother(self, Xs, Ps, Qs=None, dt=None): """ Runs the Rauch-Tung-Striebal Kalman smoother on a set of means and covariances computed by the UKF. The usual input would come from the output of `batch_filter()`. Parameters ---------- Xs : numpy.array array of the means (state variable x) of the output of a Kalman filter. Ps : numpy.array array of the covariances of the output of a kalman filter. Qs: list-like collection of numpy.array, optional Process noise of the Kalman filter at each time step. Optional, if not provided the filter's self.Q will be used dt : optional, float or array-like of float If provided, specifies the time step of each step of the filter. If float, then the same time step is used for all steps. If an array, then each element k contains the time at step k. Units are seconds. Returns ------- x : numpy.ndarray smoothed means P : numpy.ndarray smoothed state covariances K : numpy.ndarray smoother gain at each step Examples -------- .. code-block:: Python zs = [t + random.randn()*4 for t in range (40)] (mu, cov, _, _) = kalman.batch_filter(zs) (x, P, K) = rts_smoother(mu, cov, fk.F, fk.Q) """ assert len(Xs) == len(Ps) n, dim_x, _ = Xs.shape if dt is None: dt = [self._dt] * n elif isscalar(dt): dt = [dt] * n if Qs is None: Qs = [self.Q] * n # smoother gain Ks = zeros((n,dim_x,dim_x)) num_sigmas = self._num_sigmas xs, ps = Xs.copy(), Ps.copy() sigmas_f = zeros((dim_x, num_sigmas)) for k in range(n-2,-1,-1): # create sigma points from state estimate, pass through state func sigmas = self.points_fn.sigma_points(xs[k], ps[k]) for i in range(num_sigmas): sigmas_f[:, i] = self.fx(sigmas[:, i], dt[k]) # compute backwards prior state and covariance xb = dot(sigmas_f, self.Wm) Pb = 0 x = Xs[k] for i in range(num_sigmas): y = self.residual_x(sigmas_f[:, i:i+1], x) Pb += self.Wc[i] * outer(y, y) Pb += Qs[k] # compute cross variance Pxb = 0 for i in range(num_sigmas): z = self.residual_x(sigmas[:,i:i+1], Xs[k]) y = self.residual_x(sigmas_f[:,i:i+1], xb) Pxb += self.Wc[i] * outer(z, y) # compute gain K = dot(Pxb, inv(Pb)) # update the smoothed estimates xs[k] += dot (K, self.residual_x(xs[k+1], xb)) ps[k] += dot3(K, ps[k+1] - Pb, K.T) Ks[k] = K return (xs, ps, Ks) class MerweScaledSigmaPoints2(object): def __init__(self, n, alpha, beta, kappa, sqrt_method=None, subtract=None): """ Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1]. It parametizes the sigma points using alpha, beta, kappa terms, and is the version seen in most publications. Unless you know better, this should be your default choice. Parameters ---------- n : int Dimensionality of the state. 2n+1 weights will be generated. alpha : float Determins the spread of the sigma points around the mean. Usually a small positive value (1e-3) according to [3]. beta : float Incorporates prior knowledge of the distribution of the mean. For Gaussian x beta=2 is optimal, according to [3]. kappa : float, default=0.0 Secondary scaling parameter usually set to 0 according to [4], or to 3-n according to [5]. sqrt_method : function(ndarray), default=scipy.linalg.cholesky Defines how we compute the square root of a matrix, which has no unique answer. Cholesky is the default choice due to its speed. Typically your alternative choice will be scipy.linalg.sqrtm. Different choices affect how the sigma points are arranged relative to the eigenvectors of the covariance matrix. Usually this will not matter to you; if so the default cholesky() yields maximal performance. As of van der Merwe's dissertation of 2004 [6] this was not a well reseached area so I have no advice to give you. If your method returns a triangular matrix it must be upper triangular. Do not use numpy.linalg.cholesky - for historical reasons it returns a lower triangular matrix. The SciPy version does the right thing. subtract : callable (x, y), optional Function that computes the difference between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. References ---------- .. [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic State-Space Models" (Doctoral dissertation) """ self.n = n self.alpha = alpha self.beta = beta self.kappa = kappa if sqrt_method is None: self.sqrt = cholesky else: self.sqrt = sqrt_method if subtract is None: self.subtract= np.subtract else: self.subtract = subtract def num_sigmas(self): """ Number of sigma points for each variable in the state x""" return 2*self.n + 1 def sigma_points(self, x, P): """ Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P) of the filter. Returns tuple of the sigma points and weights. Works with both scalar and array inputs: sigma_points (5, 9, 2) # mean 5, covariance 9 sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I Parameters ---------- X An array-like object of the means of length n Can be a scalar if 1D. examples: 1, [1,2], np.array([1,2]) P : scalar, or np.array Covariance of the filter. If scalar, is treated as eye(n)*P. Returns ------- sigmas : np.array, of size (n, 2n+1) Two dimensional array of sigma points. Each column contains all of the sigmas for one dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n} """ assert x.ndim == 2 and x.shape[1], "x must be a column vector" n = self.n if np.isscalar(P): P = np.eye(n)*P else: P = np.asarray(P) lambda_ = self.alpha**2 * (n + self.kappa) - n U = self.sqrt((lambda_ + n)*P) sigmas = np.zeros((n, 2*n+1)) x0 = x[:, 0] sigmas[:,0] = x0 for k in range(n): sigmas[:, k+1] = self.subtract(x0, -U[k]) sigmas[:, n+k+1] = self.subtract(x0, U[k]) return sigmas def weights(self): """ Computes the weights for the scaled unscented Kalman filter. Returns ------- Wm : ndarray[2n+1] weights for mean Wc : ndarray[2n+1] weights for the covariances """ n = self.n lambda_ = self.alpha**2 * (n +self.kappa) - n c = .5 / (n + lambda_) Wc = np.full(2*n + 1, c) Wm = np.full(2*n + 1, c) Wc[0] = lambda_ / (n + lambda_) + (1 - self.alpha**2 + self.beta) Wm[0] = lambda_ / (n + lambda_) return Wm, Wc def unscented_transform2(sigmas, Wm, Wc, noise_cov=None, mean_fn=np.dot, residual_fn=None): """ Computes unscented transform of a set of sigma points and weights. returns the mean and covariance in a tuple. Parameters ---------- sigmas: ndarray [#sigmas per dimension, dimension] 2D array of sigma points. Wm : ndarray [# sigmas per dimension] Weights for the mean. Must sum to 1. Wc : ndarray [# sigmas per dimension] Weights for the covariance. Must sum to 1. noise_cov : ndarray, optional noise matrix added to the final computed covariance matrix. mean_fn : callable (sigma_points, weights), optional Function that computes the mean of the provided sigma points and weights. Use this if your state variable contains nonlinear values such as angles which cannot be summed. .. code-block:: Python def state_mean(sigmas, Wm): x = np.zeros(3) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] x[1] += s[1] * Wm[i] sum_sin += sin(s[2])*Wm[i] sum_cos += cos(s[2])*Wm[i] x[2] = atan2(sum_sin, sum_cos) return x residual_fn : callable (x, y), optional Function that computes the residual (difference) between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. .. code-block:: Python def residual(a, b): y = a[0] - b[0] if y > np.pi: y -= 2*np.pi if y < -np.pi: y = 2*np.pi return y Returns ------- x : ndarray [dimension] Mean of the sigma points after passing through the transform. P : ndarray covariance of the sigma points after passing throgh the transform. """ n, kmax = sigmas.shape x = np.dot(sigmas, Wm).reshape((-1, 1)) # dot = \Sigma^n_1 (W[k]*Xi[k]) # new covariance is the sum of the outer product of the residuals # times the weights # this is the fast way to do this - see 'else' for the slow way if residual_fn is None: y = sigmas - x[np.newaxis,:] P = y.T.dot(np.diag(Wc)).dot(y) else: P = np.zeros((n, n)) for k in range(kmax): y = residual_fn(sigmas[:, k], x[:, 0]) P += Wc[k] * np.outer(y, y) if noise_cov is not None: P += noise_cov return (x, P) DO_PLOT = False def test_sigma_plot(): """ Test to make sure sigma's correctly mirror the shape and orientation of the covariance array.""" x = np.array([[1, 2]]) P = np.array([[2, 1.2], [1.2, 2]]) kappa = .1 # if kappa is larger, than points shoudld be closer together sp0 = JulierSigmaPoints(n=2, kappa=kappa) sp1 = JulierSigmaPoints(n=2, kappa=kappa*1000) sp2 = MerweScaledSigmaPoints(n=2, kappa=0, beta=2, alpha=1e-3) sp3 = SimplexSigmaPoints(n=2) w0, _ = sp0.weights() w1, _ = sp1.weights() w2, _ = sp2.weights() w3, _ = sp3.weights() Xi0 = sp0.sigma_points(x, P) Xi1 = sp1.sigma_points(x, P) Xi2 = sp2.sigma_points(x, P) Xi3 = sp3.sigma_points(x, P) assert max(Xi1[:,0]) > max(Xi0[:,0]) assert max(Xi1[:,1]) > max(Xi0[:,1]) if DO_PLOT: plt.figure() for i in range(Xi0.shape[0]): plt.scatter((Xi0[i,0]-x[0, 0])*w0[i] + x[0, 0], (Xi0[i,1]-x[0, 1])*w0[i] + x[0, 1], color='blue', label='Julier low $\kappa$') for i in range(Xi1.shape[0]): plt.scatter((Xi1[i, 0]-x[0, 0]) * w1[i] + x[0,0], (Xi1[i, 1]-x[0, 1]) * w1[i] + x[0,1], color='green', label='Julier high $\kappa$') # for i in range(Xi2.shape[0]): # plt.scatter((Xi2[i, 0] - x[0, 0]) * w2[i] + x[0, 0], # (Xi2[i, 1] - x[0, 1]) * w2[i] + x[0, 1], # color='red') for i in range(Xi3.shape[0]): plt.scatter((Xi3[i, 0] - x[0, 0]) * w3[i] + x[0, 0], (Xi3[i, 1] - x[0, 1]) * w3[i] + x[0, 1], color='black', label='Simplex') stats.plot_covariance_ellipse([1, 2], P) def test_simplex_weights(): for n in range(1,15): for k in np.linspace(0,5,0.1): Wm = UKF.weights(n, k) assert abs(sum(Wm) - 1) < 1.e-12 def test_julier_weights(): for n in range(1,15): for k in np.linspace(0,5,0.1): Wm = UKF.weights(n, k) assert abs(sum(Wm) - 1) < 1.e-12 def test_scaled_weights(): for n in range(1,5): for alpha in np.linspace(0.99, 1.01, 100): for beta in range(0,2): for kappa in range(0,2): sp = MerweScaledSigmaPoints(n, alpha, 0, 3-n) Wm, Wc = sp.weights() assert abs(sum(Wm) - 1) < 1.e-1 assert abs(sum(Wc) - 1) < 1.e-1 def test_julier_sigma_points_1D(): """ tests passing 1D data into sigma_points""" kappa = 0. sp = JulierSigmaPoints(1, kappa) #ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa) Wm, Wc = sp.weights() assert np.allclose(Wm, Wc, 1e-12) assert len(Wm) == 3 mean = 5 cov = 9 Xi = sp.sigma_points(mean, cov) xm, ucov = unscented_transform(Xi, Wm, Wc, 0) # sum of weights*sigma points should be the original mean m = 0.0 for x, w in zip(Xi, Wm): m += x*w assert abs(m-mean) < 1.e-12 assert abs(xm[0] - mean) < 1.e-12 assert abs(ucov[0,0]-cov) < 1.e-12 assert Xi.shape == (3,1) def test_simplex_sigma_points_1D(): """ tests passing 1D data into sigma_points""" sp = SimplexSigmaPoints(1) #ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa) Wm, Wc = sp.weights() assert np.allclose(Wm, Wc, 1e-12) assert len(Wm) == 2 mean = 5 cov = 9 Xi = sp.sigma_points(mean, cov) xm, ucov = unscented_transform(Xi, Wm, Wc, 0) # sum of weights*sigma points should be the original mean m = 0.0 for x, w in zip(Xi, Wm): m += x*w assert abs(m-mean) < 1.e-12 assert abs(xm[0] - mean) < 1.e-12 assert abs(ucov[0,0]-cov) < 1.e-12 assert Xi.shape == (2,1) class RadarSim(object): def __init__(self, dt): self.x = 0 self.dt = dt def get_range(self): vel = 100 + 5*randn() alt = 1000 + 10*randn() self.x += vel*self.dt v = self.x * 0.05*randn() rng = (self.x**2 + alt**2)**.5 + v return rng def test_radar(): def fx(x, dt): A = np.eye(3) + dt * np.array ([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) return A.dot(x) def hx(x): return np.sqrt(x[0]**2 + x[2]**2) dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=0.) # sp = SimplexSigmaPoints(n=3) kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 100. radar = RadarSim(dt) t = np.arange(0,20+dt, dt) n = len(t) xs = np.zeros((n,3)) random.seed(200) rs = [] #xs = [] for i in range(len(t)): r = radar.get_range() #r = GetRadar(dt) kf.predict() kf.update(z=[r]) xs[i,:] = kf.x rs.append(r) if DO_PLOT: print(xs[:,0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:,0]) plt.subplot(312) plt.plot(t, xs[:,1]) plt.subplot(313) plt.plot(t, xs[:,2]) def test_linear_2d_merwe(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P*=0.0001 #kf.R *=0 #kf.Q zs = [] for i in range(20): z = np.array([i+randn()*0.1, i+randn()*0.1]) zs.append(z) Ms, Ps = kf.batch_filter(zs) smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt) if DO_PLOT: plt.figure() zs = np.asarray(zs) plt.plot(zs[:,0], marker='+') plt.plot(Ms[:,0], c='b') plt.plot(smooth_x[:,0], smooth_x[:,2], c='r') print(smooth_x) from filterpy.kalman import UnscentedKalmanFilter2 as UKF2 def test_linear_2d_merwe_column(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints2(4, .1, 2., -1) kf = UKF2(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([[-1., 1., -1., 1]]).T kf.P*=0.0001 #kf.R *=0 #kf.Q zs = [] for i in range(20): z = np.array([[i+randn()*0.1], [i+randn()*0.1]]) zs.append(z) Ms, Ps = kf.batch_filter(zs) smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt) if DO_PLOT: plt.figure() zs = np.asarray(zs) plt.plot(zs[:,0], marker='+', c='b') plt.plot(Ms[:,0], c='b') plt.plot(smooth_x[:,0], smooth_x[:,2], c='r') print(smooth_x) def test_linear_2d_simplex(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = SimplexSigmaPoints(n=4) kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P*=0.0001 #kf.R *=0 #kf.Q zs = [] for i in range(20): z = np.array([i+randn()*0.1, i+randn()*0.1]) zs.append(z) Ms, Ps = kf.batch_filter(zs) smooth_x, _, _ = kf.rts_smoother(Ms, Ps, dt=dt) if DO_PLOT: zs = np.asarray(zs) #plt.plot(zs[:,0]) plt.plot(Ms[:,0]) plt.plot(smooth_x[:,0], smooth_x[:,2]) print(smooth_x) def test_linear_1d(): """ should work like a linear KF if problem is linear """ def fx(x, dt): F = np.array([[1., dt], [0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0]]) dt = 0.1 points = MerweScaledSigmaPoints(2, .1, 2., -1) kf = UKF(dim_x=2, dim_z=1, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([1, 2]) kf.P = np.array([[1, 1.1], [1.1, 3]]) kf.R *= 0.05 kf.Q = np.array([[0., 0], [0., .001]]) z = np.array([2.]) kf.predict() kf.update(z) zs = [] for i in range(50): z = np.array([i+randn()*0.1]) zs.append(z) kf.predict() kf.update(z) print('K', kf.K.T) print('x', kf.x) def test_batch_missing_data(): """ batch filter should accept missing data with None in the measurements """ def fx(x, dt): F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]], dtype=float) return np.dot(F, x) def hx(x): return np.array([x[0], x[2]]) dt = 0.1 points = MerweScaledSigmaPoints(4, .1, 2., -1) kf = UKF(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points) kf.x = np.array([-1., 1., -1., 1]) kf.P*=0.0001 zs = [] for i in range(20): z = np.array([i+randn()*0.1, i+randn()*0.1]) zs.append(z) zs[2] = None Rs = [1]*len(zs) Rs[2] = None Ms, Ps = kf.batch_filter(zs) def test_rts(): def fx(x, dt): A = np.eye(3) + dt * np.array ([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) f = np.dot(A, x) return f def hx(x): return np.sqrt (x[0]**2 + x[2]**2) dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=1.) kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 100. radar = RadarSim(dt) t = np.arange(0,20+dt, dt) n = len(t) xs = np.zeros((n,3)) random.seed(200) rs = [] #xs = [] for i in range(len(t)): r = radar.get_range() #r = GetRadar(dt) kf.predict() kf.update(z=[r]) xs[i,:] = kf.x rs.append(r) kf.x = np.array([0., 90., 1100.]) kf.P = np.eye(3)*100 M, P = kf.batch_filter(rs) assert np.array_equal(M, xs), "Batch filter generated different output" Qs = [kf.Q]*len(t) M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs) if DO_PLOT: print(xs[:,0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:,0]) plt.plot(t, M2[:,0], c='g') plt.subplot(312) plt.plot(t, xs[:,1]) plt.plot(t, M2[:,1], c='g') plt.subplot(313) plt.plot(t, xs[:,2]) plt.plot(t, M2[:,2], c='g') def test_fixed_lag(): def fx(x, dt): A = np.eye(3) + dt * np.array ([[0, 1, 0], [0, 0, 0], [0, 0, 0]]) f = np.dot(A, x) return f def hx(x): return np.sqrt (x[0]**2 + x[2]**2) dt = 0.05 sp = JulierSigmaPoints(n=3, kappa=0) kf = UKF(3, 1, dt, fx=fx, hx=hx, points=sp) kf.Q *= 0.01 kf.R = 10 kf.x = np.array([0., 90., 1100.]) kf.P *= 1. radar = RadarSim(dt) t = np.arange(0,20+dt, dt) n = len(t) xs = np.zeros((n,3)) random.seed(200) rs = [] #xs = [] M = [] P = [] N =10 flxs = [] for i in range(len(t)): r = radar.get_range() #r = GetRadar(dt) kf.predict() kf.update(z=[r]) xs[i,:] = kf.x flxs.append(kf.x) rs.append(r) M.append(kf.x) P.append(kf.P) print(i) #print(i, np.asarray(flxs)[:,0]) if i == 20 and len(M) >= N: try: M2, P2, K = kf.rts_smoother(Xs=np.asarray(M)[-N:], Ps=np.asarray(P)[-N:]) flxs[-N:] = M2 #flxs[-N:] = [20]*N except: print('except', i) #P[-N:] = P2 kf.x = np.array([0., 90., 1100.]) kf.P = np.eye(3)*100 M, P = kf.batch_filter(rs) Qs = [kf.Q]*len(t) M2, P2, K = kf.rts_smoother(Xs=M, Ps=P, Qs=Qs) flxs = np.asarray(flxs) print(xs[:,0].shape) plt.figure() plt.subplot(311) plt.plot(t, xs[:,0]) plt.plot(t, flxs[:,0], c='r') plt.plot(t, M2[:,0], c='g') plt.subplot(312) plt.plot(t, xs[:,1]) plt.plot(t, flxs[:,1], c='r') plt.plot(t, M2[:,1], c='g') plt.subplot(313) plt.plot(t, xs[:,2]) plt.plot(t, flxs[:,2], c='r') plt.plot(t, M2[:,2], c='g') def test_circle(): from filterpy.kalman import KalmanFilter from math import radians def hx(x): radius = x[0] angle = x[1] x = cos(radians(angle)) * radius y = sin(radians(angle)) * radius return np.array([x, y]) def fx(x, dt): return np.array([x[0], x[1]+x[2], x[2]]) std_noise = .1 sp = JulierSigmaPoints(n=3, kappa=0.) f = UKF(dim_x=3, dim_z=2, dt=.01, hx=hx, fx=fx, points=sp) f.x = np.array([50., 90., 0]) f.P *= 100 f.R = np.eye(2)*(std_noise**2) f.Q = np.eye(3)*.001 f.Q[0,0]=0 f.Q[2,2]=0 kf = KalmanFilter(dim_x=6, dim_z=2) kf.x = np.array([50., 0., 0, 0, .0, 0.]) F = np.array([[1., 1., .5, 0., 0., 0.], [0., 1., 1., 0., 0., 0.], [0., 0., 1., 0., 0., 0.], [0., 0., 0., 1., 1., .5], [0., 0., 0., 0., 1., 1.], [0., 0., 0., 0., 0., 1.]]) kf.F = F kf.P*= 100 kf.H = np.array([[1,0,0,0,0,0], [0,0,0,1,0,0]]) kf.R = f.R kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001) kf.Q[3:6, 3:6] = Q_discrete_white_noise(3, 1., .00001) measurements = [] results = [] zs = [] kfxs = [] for t in range (0,12000): a = t / 30 + 90 x = cos(radians(a)) * 50.+ randn() * std_noise y = sin(radians(a)) * 50. + randn() * std_noise # create measurement = t plus white noise z = np.array([x,y]) zs.append(z) f.predict() f.update(z) kf.predict() kf.update(z) # save data results.append (hx(f.x)) kfxs.append(kf.x) #print(f.x) results = np.asarray(results) zs = np.asarray(zs) kfxs = np.asarray(kfxs) print(results) if DO_PLOT: plt.plot(zs[:,0], zs[:,1], c='r', label='z') plt.plot(results[:,0], results[:,1], c='k', label='UKF') plt.plot(kfxs[:,0], kfxs[:,3], c='g', label='KF') plt.legend(loc='best') plt.axis('equal') def kf_circle(): from filterpy.kalman import KalmanFilter from math import radians import math def hx(x): radius = x[0] angle = x[1] x = cos(radians(angle)) * radius y = sin(radians(angle)) * radius return np.array([x, y]) def fx(x, dt): return np.array([x[0], x[1]+x[2], x[2]]) def hx_inv(x, y): angle = math.atan2(y,x) radius = math.sqrt(x*x + y*y) return np.array([radius, angle]) std_noise = .1 kf = KalmanFilter(dim_x=3, dim_z=2) kf.x = np.array([50., 0., 0.]) F = np.array([[1., 0, 0.], [0., 1., 1.,], [0., 0., 1.,]]) kf.F = F kf.P*= 100 kf.H = np.array([[1,0,0], [0,1,0]]) kf.R = np.eye(2)*(std_noise**2) #kf.Q[0:3, 0:3] = Q_discrete_white_noise(3, 1., .00001) zs = [] kfxs = [] for t in range (0,2000): a = t / 30 + 90 x = cos(radians(a)) * 50.+ randn() * std_noise y = sin(radians(a)) * 50. + randn() * std_noise z = hx_inv(x,y) zs.append(z) kf.predict() kf.update(z) # save data kfxs.append(kf.x) zs = np.asarray(zs) kfxs = np.asarray(kfxs) if DO_PLOT: plt.plot(zs[:,0], zs[:,1], c='r', label='z') plt.plot(kfxs[:,0], kfxs[:,1], c='g', label='KF') plt.legend(loc='best') plt.axis('equal') def two_radar(): # code is not complete - I was using to test RTS smoother. very similar # to two_radary.py in book. import numpy as np import matplotlib.pyplot as plt from numpy import array from numpy.linalg import norm from numpy.random import randn from math import atan2, radians from filterpy.common import Q_discrete_white_noise class RadarStation(object): def __init__(self, pos, range_std, bearing_std): self.pos = asarray(pos) self.range_std = range_std self.bearing_std = bearing_std def reading_of(self, ac_pos): """ Returns range and bearing to aircraft as tuple. bearing is in radians. """ diff = np.subtract(self.pos, ac_pos) rng = norm(diff) brg = atan2(diff[1], diff[0]) return rng, brg def noisy_reading(self, ac_pos): rng, brg = self.reading_of(ac_pos) rng += randn() * self.range_std brg += randn() * self.bearing_std return rng, brg class ACSim(object): def __init__(self, pos, vel, vel_std): self.pos = asarray(pos, dtype=float) self.vel = asarray(vel, dtype=float) self.vel_std = vel_std def update(self): vel = self.vel + (randn() * self.vel_std) self.pos += vel return self.pos dt = 1. def hx(x): r1, b1 = hx.R1.reading_of((x[0], x[2])) r2, b2 = hx.R2.reading_of((x[0], x[2])) return array([r1, b1, r2, b2]) pass def fx(x, dt): x_est = x.copy() x_est[0] += x[1]*dt x_est[2] += x[3]*dt return x_est vx, vy = 0.1, 0.1 f = UnscentedKalmanFilter(dim_x=4, dim_z=4, dt=dt, hx=hx, fx=fx, kappa=0) aircraft = ACSim ((100,100), (vx*dt,vy*dt), 0.00000002) range_std = 0.001 # 1 meter bearing_std = 1/1000 # 1mrad R1 = RadarStation ((0,0), range_std, bearing_std) R2 = RadarStation ((200,0), range_std, bearing_std) hx.R1 = R1 hx.R2 = R2 f.x = array([100, vx, 100, vy]) f.R = np.diag([range_std**2, bearing_std**2, range_std**2, bearing_std**2]) q = Q_discrete_white_noise(2, var=0.0002, dt=dt) #q = np.array([[0,0],[0,0.0002]]) f.Q[0:2, 0:2] = q f.Q[2:4, 2:4] = q f.P = np.diag([.1, 0.01, .1, 0.01]) track = [] zs = [] for i in range(int(300/dt)): pos = aircraft.update() r1, b1 = R1.noisy_reading(pos) r2, b2 = R2.noisy_reading(pos) z = np.array([r1, b1, r2, b2]) zs.append(z) track.append(pos.copy()) zs = asarray(zs) xs, Ps, Pxz, pM, pP = f.batch_filter(zs) ms, _, _ = f.rts_smoother(xs, Ps) track = asarray(track) time = np.arange(0,len(xs)*dt, dt) plt.figure() plt.subplot(411) plt.plot(time, track[:,0]) plt.plot(time, xs[:,0]) plt.legend(loc=4) plt.xlabel('time (sec)') plt.ylabel('x position (m)') plt.tight_layout() plt.subplot(412) plt.plot(time, track[:,1]) plt.plot(time, xs[:,2]) plt.legend(loc=4) plt.xlabel('time (sec)') plt.ylabel('y position (m)') plt.tight_layout() plt.subplot(413) plt.plot(time, xs[:,1]) plt.plot(time, ms[:,1]) plt.legend(loc=4) plt.ylim([0, 0.2]) plt.xlabel('time (sec)') plt.ylabel('x velocity (m/s)') plt.tight_layout() plt.subplot(414) plt.plot(time, xs[:,3]) plt.plot(time, ms[:,3]) plt.ylabel('y velocity (m/s)') plt.legend(loc=4) plt.xlabel('time (sec)') plt.tight_layout() plt.show() if __name__ == "__main__": DO_PLOT = True test_linear_2d_merwe() test_linear_2d_merwe_column() #test_sigma_plot() # test_linear_1d() # test_batch_missing_data() # # test_linear_2d() # test_julier_sigma_points_1D() #test_simplex_sigma_points_1D() # test_fixed_lag() # DO_PLOT = True # test_rts() # kf_circle() # test_circle() '''test_1D_sigma_points() #plot_sigma_test () x = np.array([[1,2]]) P = np.array([[2, 1.2], [1.2, 2]])\ kappa = .1 xi,w = sigma_points (x,P,kappa) xm, cov = unscented_transform(xi, w)''' #test_radar() # test_sigma_plot() # test_julier_weights() # test_scaled_weights() # test_simplex_weights() #print('xi=\n',Xi) """ xm, cov = unscented_transform(Xi, W) print(xm) print(cov)""" # sigma_points ([5,2],9*np.eye(2), 2) #plt.legend() #plt.show() filterpy-1.4.5/filterpy/kalman/unscented_transform.py000066400000000000000000000067341335747705400231460ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-arguments """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ import numpy as np def unscented_transform(sigmas, Wm, Wc, noise_cov=None, mean_fn=None, residual_fn=None): r""" Computes unscented transform of a set of sigma points and weights. returns the mean and covariance in a tuple. This works in conjunction with the UnscentedKalmanFilter class. Parameters ---------- sigmas: ndarray, of size (n, 2n+1) 2D array of sigma points. Wm : ndarray [# sigmas per dimension] Weights for the mean. Wc : ndarray [# sigmas per dimension] Weights for the covariance. noise_cov : ndarray, optional noise matrix added to the final computed covariance matrix. mean_fn : callable (sigma_points, weights), optional Function that computes the mean of the provided sigma points and weights. Use this if your state variable contains nonlinear values such as angles which cannot be summed. .. code-block:: Python def state_mean(sigmas, Wm): x = np.zeros(3) sum_sin, sum_cos = 0., 0. for i in range(len(sigmas)): s = sigmas[i] x[0] += s[0] * Wm[i] x[1] += s[1] * Wm[i] sum_sin += sin(s[2])*Wm[i] sum_cos += cos(s[2])*Wm[i] x[2] = atan2(sum_sin, sum_cos) return x residual_fn : callable (x, y), optional Function that computes the residual (difference) between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. .. code-block:: Python def residual(a, b): y = a[0] - b[0] y = y % (2 * np.pi) if y > np.pi: y -= 2*np.pi return y Returns ------- x : ndarray [dimension] Mean of the sigma points after passing through the transform. P : ndarray covariance of the sigma points after passing throgh the transform. Examples -------- See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python """ kmax, n = sigmas.shape try: if mean_fn is None: # new mean is just the sum of the sigmas * weight x = np.dot(Wm, sigmas) # dot = \Sigma^n_1 (W[k]*Xi[k]) else: x = mean_fn(sigmas, Wm) except: print(sigmas) raise # new covariance is the sum of the outer product of the residuals # times the weights # this is the fast way to do this - see 'else' for the slow way if residual_fn is np.subtract or residual_fn is None: y = sigmas - x[np.newaxis, :] P = np.dot(y.T, np.dot(np.diag(Wc), y)) else: P = np.zeros((n, n)) for k in range(kmax): y = residual_fn(sigmas[k], x) P += Wc[k] * np.outer(y, y) if noise_cov is not None: P += noise_cov return (x, P) filterpy-1.4.5/filterpy/leastsq/000077500000000000000000000000001335747705400167105ustar00rootroot00000000000000filterpy-1.4.5/filterpy/leastsq/__init__.py000066400000000000000000000010151335747705400210160ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) __all__ = ["least_squares"] from .least_squares import LeastSquaresFilter filterpy-1.4.5/filterpy/leastsq/least_squares.py000066400000000000000000000136001335747705400221350ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=C0103, R0913, R0902, C0326, R0914 # disable snake_case warning, too many arguments, too many attributes, # one space before assignment, too many local variables """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import absolute_import, division from math import sqrt import numpy as np from filterpy.kalman import pretty_str class LeastSquaresFilter(object): """Implements a Least Squares recursive filter. Formulation is per Zarchan [1]_. Filter may be of order 0 to 2. Order 0 assumes the value being tracked is a constant, order 1 assumes that it moves in a line, and order 2 assumes that it is tracking a second order polynomial. Parameters ---------- dt : float time step per update order : int order of filter 0..2 noise_sigma : float sigma (std dev) in x. This allows us to calculate the error of the filter, it does not influence the filter output. Attributes ---------- n : int step in the recursion. 0 prior to first call, 1 after the first call, etc. K : np.array Gains for the filter. K[0] for all orders, K[1] for orders 0 and 1, and K[2] for order 2 x: np.array (order + 1, 1) estimate(s) of the output. It is a vector containing the estimate x and the derivatives of x: [x x' x''].T. It contains as many derivatives as the order allows. That is, a zero order filter has no derivatives, a first order has one derivative, and a second order has two. y : float residual (difference between measurement projection of previous estimate to current time). Examples -------- .. code-block:: Python from filterpy.leastsq import LeastSquaresFilter lsq = LeastSquaresFilter(dt=0.1, order=1, noise_sigma=2.3) while True: z = sensor_reading() # get a measurement x = lsq.update(z) # get the filtered estimate. print('error: {}, velocity error: {}'.format( lsq.error, lsq.derror)) References ---------- .. [1] Zarchan and Musoff. "Fundamentals of Kalman Filtering: A Practical Approach." Third Edition. AIAA, 2009. """ def __init__(self, dt, order, noise_sigma=0.): if order < 0 or order > 2: raise ValueError('order must be between 0 and 2') self.dt = dt self.sigma = noise_sigma self._order = order self.reset() def reset(self): """ reset filter back to state at time of construction""" self.n = 0 # nth step in the recursion self.x = np.zeros(self._order + 1) self.K = np.zeros(self._order + 1) self.y = 0 # residual def update(self, z): """ Update filter with new measurement `z` Returns ------- x : np.array estimate for this time step (same as self.x) """ self.n += 1 # rename for readability n = self.n dt = self.dt x = self.x K = self.K y = self.y if self._order == 0: K[0] = 1. / n y = z - x x[0] += K[0] * y elif self._order == 1: K[0] = 2. * (2*n - 1) / (n*(n + 1)) K[1] = 6. / (n*(n + 1)*dt) y = z - x[0] - (dt * x[1]) x[0] += (K[0] * y) + (dt * x[1]) x[1] += (K[1] * y) else: den = n * (n+1) * (n+2) K[0] = 3. * (3*n**2 - 3*n + 2) / den K[1] = 18. * (2*n-1) / (den*dt) K[2] = 60. / (den*dt**2) y = z - x[0] - (dt * x[1]) - (0.5 * dt**2 * x[2]) x[0] += (K[0] * y) + (x[1] * dt) + (.5 * dt**2 * x[2]) x[1] += (K[1] * y) + (x[2] * dt) x[2] += (K[2] * y) return self.x def errors(self): """ Computes and returns the error and standard deviation of the filter at this time step. Returns ------- error : np.array size 1xorder+1 std : np.array size 1xorder+1 """ n = self.n dt = self.dt order = self._order sigma = self.sigma error = np.zeros(order + 1) std = np.zeros(order + 1) if n == 0: return (error, std) if order == 0: error[0] = sigma/sqrt(n) std[0] = sigma/sqrt(n) elif order == 1: if n > 1: error[0] = sigma * sqrt(2*(2*n-1) / (n*(n+1))) error[1] = sigma * sqrt(12. / (n*(n*n-1)*dt*dt)) std[0] = sigma * sqrt((2*(2*n-1)) / (n*(n+1))) std[1] = (sigma/dt) * sqrt(12. / (n*(n*n-1))) elif order == 2: dt2 = dt * dt if n >= 3: error[0] = sigma * sqrt(3*(3*n*n-3*n+2) / (n*(n+1)*(n+2))) error[1] = sigma * sqrt(12*(16*n*n-30*n+11) / (n*(n*n-1)*(n*n-4)*dt2)) error[2] = sigma * sqrt(720/(n*(n*n-1)*(n*n-4)*dt2*dt2)) std[0] = sigma * sqrt((3*(3*n*n - 3*n + 2)) / (n*(n+1)*(n+2))) std[1] = (sigma/dt) * sqrt((12*(16*n*n - 30*n + 11)) / (n*(n*n - 1)*(n*n - 4))) std[2] = (sigma/dt2) * sqrt(720 / (n*(n*n-1)*(n*n-4))) return error, std def __repr__(self): return '\n'.join([ 'LeastSquaresFilter object', pretty_str('dt', self.dt), pretty_str('sigma', self.sigma), pretty_str('_order', self._order), pretty_str('x', self.x), pretty_str('K', self.K) ]) filterpy-1.4.5/filterpy/leastsq/tests/000077500000000000000000000000001335747705400200525ustar00rootroot00000000000000filterpy-1.4.5/filterpy/leastsq/tests/test_lsq.py000066400000000000000000000213171335747705400222660ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) from math import sqrt import matplotlib.pyplot as plt import numpy as np from numpy import dot import numpy.random as random from scipy.linalg import inv from filterpy.gh import GHFilter from filterpy.leastsq import LeastSquaresFilter def near_equal(x, y, e=1.e-14): return abs(x-y) < e class LSQ(object): def __init__(self, dim_x): self.dim_x = dim_x self.I = np.eye(dim_x) self.H = 0 self.x = np.zeros((dim_x, 1)) self.k = 0 def update(self,Z): self.x += 1 self.k += 1 print('k=', self.k, 1/self.k, 1/(self.k+1)) S = dot(self.H, self.P).dot(self.H.T) + self.R K1 = dot(self.P, self.H.T).dot(inv(S)) print('K1=', K1[0, 0]) I_KH = self.I - dot(K1, self.H) y = Z - dot(self.H, self.x) print('y=', y) self.x = self.x + dot(K1, y) self.P = dot(I_KH, self.P) print(self.P) class LeastSquaresFilterOriginal(object): """Implements a Least Squares recursive filter. Formulation is per Zarchan [1]. Filter may be of order 0 to 2. Order 0 assumes the value being tracked is a constant, order 1 assumes that it moves in a line, and order 2 assumes that it is tracking a second order polynomial. It is implemented to be directly callable like a function. See examples. Examples -------- lsq = LeastSquaresFilter(dt=0.1, order=1, noise_variance=2.3) while True: z = sensor_reading() # get a measurement x = lsq(z) # get the filtered estimate. print('error: {}, velocity error: {}'.format(lsq.error, lsq.derror)) Attributes ---------- n : int step in the recursion. 0 prior to first call, 1 after the first call, etc. K1,K2,K3 : float Gains for the filter. K1 for all orders, K2 for orders 0 and 1, and K3 for order 2 x, dx, ddx: type(z) estimate(s) of the output. 'd' denotes derivative, so 'dx' is the first derivative of x, 'ddx' is the second derivative. References ---------- [1] Zarchan and Musoff. "Fundamentals of Kalman Filtering: A Practical Approach." Third Edition. AIAA, 2009. """ def __init__(self, dt, order, noise_variance=0.): """ Least Squares filter of order 0 to 2. Parameters ---------- dt : float time step per update order : int order of filter 0..2 noise_variance : float variance in x. This allows us to calculate the error of the filter, it does not influence the filter output. """ assert order >= 0 assert order <= 2 self.reset() self.dt = dt self.dt2 = dt**2 self.sigma = noise_variance self._order = order def reset(self): """ reset filter back to state at time of construction""" self.n = 0 #nth step in the recursion self.x = 0. self.error = 0. self.derror = 0. self.dderror = 0. self.dx = 0. self.ddx = 0. self.K1 = 0 self.K2 = 0 self.K3 = 0 def __call__(self, z): self.n += 1 n = self.n dt = self.dt dt2 = self.dt2 if self._order == 0: self.K1 = 1. / n residual = z - self.x self.x = self.x + residual * self.K1 self.error = self.sigma/sqrt(n) elif self._order == 1: self.K1 = 2*(2*n-1) / (n*(n+1)) self.K2 = 6 / (n*(n+1)*dt) residual = z - self.x - self.dx*dt self.x = self.x + self.dx*dt + self.K1*residual self.dx = self.dx + self.K2*residual if n > 1: self.error = self.sigma*sqrt(2.*(2*n-1)/(n*(n+1))) self.derror = self.sigma*sqrt(12./(n*(n*n-1)*dt*dt)) else: den = n*(n+1)*(n+2) self.K1 = 3*(3*n**2 - 3*n + 2) / den self.K2 = 18*(2*n-1) / (den*dt) self.K3 = 60./ (den*dt2) residual = z - self.x - self.dx*dt - .5*self.ddx*dt2 self.x += self.dx*dt + .5*self.ddx*dt2 +self. K1 * residual self.dx += self.ddx*dt + self.K2*residual self.ddx += self.K3*residual if n >= 3: self.error = self.sigma*sqrt(3*(3*n*n-3*n+2)/(n*(n+1)*(n+2))) self.derror = self.sigma*sqrt(12*(16*n*n-30*n+11) / (n*(n*n-1)*(n*n-4)*dt2)) self.dderror = self.sigma*sqrt(720/(n*(n*n-1)*(n*n-4)*dt2*dt2)) return self.x def standard_deviation(self): if self.n == 0: return 0. if self._order == 0: return 1./sqrt(self) elif self._order == 1: pass def __repr__(self): return 'LeastSquareFilter x={}, dx={}, ddx={}'.format( self.x, self.dx, self.ddx) def test_lsq(): """ implements alternative version of first order Least Squares filter using g-h filter formulation and uses it to check the output of the LeastSquaresFilter class.""" global lsq, lsq2, xs, lsq_xs gh = GHFilter(x=0, dx=0, dt=1, g=.5, h=0.02) lsq = LeastSquaresFilterOriginal(dt=1, order=1) lsq2 = LeastSquaresFilter(dt=1, order=1) zs = [x+random.randn()*10 for x in range(0, 10000)] # test __repr__ at least doesn't crash try: str(lsq2) except: assert False, "LeastSquaresFilter.__repr__ exception" xs = [] lsq_xs = [] for i, z in enumerate(zs): g = 2*(2*i + 1) / ((i+2)*(i+1)) h = 6 / ((i+2)*(i+1)) x, dx = gh.update(z, g, h) lx = lsq(z) lsq_xs.append(lx) x2 = lsq2.update(z) assert near_equal(x2[0], lx, 1.e-10), '{}, {}, {}'.format( i, x2[0], lx) xs.append(x) plt.plot(xs) plt.plot(lsq_xs) for x, y in zip(xs, lsq_xs): r = x-y assert r < 1.e-8 def test_first_order(): ''' data and example from Zarchan, page 105-6''' lsf = LeastSquaresFilter(dt=1, order=1) xs = [1.2, .2, 2.9, 2.1] ys = [] for x in xs: ys.append(lsf.update(x)[0]) plt.plot(xs, c='b') plt.plot(ys, c='g') plt.plot([0, len(xs)-1], [ys[0], ys[-1]]) def test_second_order(): ''' data and example from Zarchan, page 114''' lsf = LeastSquaresFilter(1, order=2) lsf0 = LeastSquaresFilterOriginal(1, order=2) xs = [1.2, .2, 2.9, 2.1] ys = [] for x in xs: y = lsf.update(x)[0] y0 = lsf0(x) assert near_equal(y, y0) ys.append(y) plt.scatter(range(len(xs)), xs, c='r', marker='+') plt.plot(ys, c='g') plt.plot([0, len(xs)-1], [ys[0], ys[-1]], c='b') def test_fig_3_8(): """ figure 3.8 in Zarchan, p. 108""" lsf = LeastSquaresFilter(0.1, order=1) lsf0 = LeastSquaresFilterOriginal(0.1, order=1) xs = [x + 3 + random.randn() for x in np.arange(0, 10, 0.1)] ys = [] for x in xs: y0 = lsf0(x) y = lsf.update(x)[0] assert near_equal(y, y0) ys.append(y) plt.plot(xs) plt.plot(ys) def test_listing_3_4(): """ listing 3.4 in Zarchan, p. 117""" lsf = LeastSquaresFilter(0.1, order=2) xs = [5*x*x - x + 2 + 30*random.randn() for x in np.arange(0, 10, 0.1)] ys = [] for x in xs: ys.append(lsf.update(x)[0]) plt.plot(xs) plt.plot(ys) def lsq2_plot(): fl = LSQ(2) fl.H = np.array([[1., 1.], [0., 1.]]) fl.R = np.eye(2) fl.P = np.array([[2., .5], [.5, 2.]]) for x in range(10): fl.update(np.array([[x], [x]], dtype=float)) plt.scatter(x, fl.x[0, 0]) def test_big_data(): N = 1000000 xs = np.array([i+random.randn() for i in range(N)]) for order in [1, 2]: lsq = LeastSquaresFilter(dt=1, order=order) ys = np.array([lsq.update(x)[0] for x in xs]) delta = xs - ys assert delta.max() < 6, delta.max() assert delta.min() > -6, delta.min() # zero order is special case, it can't adapt quickly to changing data xs = np.array([random.randn() for i in range(N)]) lsq = LeastSquaresFilter(dt=1, order=0) ys = np.array([lsq.update(x)[0] for x in xs]) delta = xs - ys assert delta.max() < 6, delta.max() assert delta.min() > -6, delta.min() if __name__ == "__main__": test_big_data() filterpy-1.4.5/filterpy/memory/000077500000000000000000000000001335747705400165445ustar00rootroot00000000000000filterpy-1.4.5/filterpy/memory/__init__.py000066400000000000000000000010361335747705400206550ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: disable=wildcard-import from __future__ import (absolute_import, division, print_function, unicode_literals) __all__ = ["fading_memory"] from .fading_memory import * filterpy-1.4.5/filterpy/memory/fading_memory.py000066400000000000000000000134731335747705400217460ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=C0103, R0913, R0902, C0326, R0914, R0903 # disable snake_case warning, too many arguments, too many attributes, # one space before assignment, too many local variables, too few public # methods """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import numpy as np from numpy import dot from filterpy.common import pretty_str class FadingMemoryFilter(object): """ Creates a fading memory filter of order 0, 1, or 2. The KalmanFilter class also implements a more general fading memory filter and should be preferred in most cases. This is probably faster for low order systems. This algorithm is based on the fading filter algorithm developed in Zarcan's "Fundamentals of Kalman Filtering" [1]. Parameters ---------- x0 : 1D np.array or scalar Initial value for the filter state. Each value can be a scalar or a np.array. You can use a scalar for x0. If order > 0, then 0.0 is assumed for the higher order terms. x[0] is the value being tracked x[1] is the first derivative (for order 1 and 2 filters) x[2] is the second derivative (for order 2 filters) dt : scalar timestep order : int order of the filter. Defines the order of the system 0 - assumes system of form x = a_0 + a_1*t 1 - assumes system of form x = a_0 +a_1*t + a_2*t^2 2 - assumes system of form x = a_0 +a_1*t + a_2*t^2 + a_3*t^3 beta : float filter gain parameter. Attributes ---------- x : np.array State of the filter. x[0] is the value being tracked x[1] is the derivative of x[0] (order 1 and 2 only) x[2] is the 2nd derivative of x[0] (order 2 only) This is always an np.array, even for order 0 where you can initialize x0 with a scalar. P : np.array The diagonal of the covariance matrix. Assumes that variance is one; multiply by sigma^2 to get the actual variances. This is a constant and will not vary as the filter runs. e : np.array The truncation error of the filter. Each term must be multiplied by the a_1, a_2, or a_3 of the polynomial for the system. For example, if the filter is order 2, then multiply all terms of self.e by a_3 to get the actual error. Multipy by a_2 for order 1, and a_1 for order 0. References ---------- Paul Zarchan and Howard Musoff. "Fundamentals of Kalman Filtering: A Practical Approach" American Institute of Aeronautics and Astronautics, Inc. Fourth Edition. p. 521-536. (2015) """ def __init__(self, x0, dt, order, beta): if order < 0 or order > 2: raise ValueError('order must be between 0 and 2') if np.isscalar(x0): self.x = np.zeros(order+1) self.x[0] = x0 else: self.x = np.copy(x0) self.dt = dt self.order = order self.beta = beta if order == 0: self.P = np.array([(1-beta)/(1+beta)], dtype=float) self.e = np.array([dt * beta / (1-beta)], dtype=float) elif order == 1: p11 = (1-beta) * (1+4*beta+5*beta**2) / (1+beta)**3 p22 = 2*(1-beta)**3 / (1+beta)**3 self.P = np.array([p11, p22], dtype=float) e = 2*dt*2 * (beta / (1-beta))**2 de = dt*((1+3*beta)/(1-beta)) self.e = np.array([e, de], dtype=float) else: p11 = (1-beta)*((1+6*beta + 16*beta**2 + 24*beta**3 + 19*beta**4) / (1+beta)**5) p22 = (1-beta)**3 * ((13+50*beta + 49*beta**2) / (2*(1+beta)**5 * dt**2)) p33 = 6*(1-beta)**5 / ((1+beta)**5 * dt**4) self.P = np.array([p11, p22, p33], dtype=float) e = 6*dt**3*(beta/(1-beta))**3 de = dt**2 * (2 + 5*beta + 11*beta**2) / (1-beta)**2 dde = 6*dt*(1+2*beta) / (1-beta) self.e = np.array([e, de, dde], dtype=float) def __repr__(self): return '\n'.join([ 'FadingMemoryFilter object', pretty_str('dt', self.dt), pretty_str('order', self.order), pretty_str('beta', self.beta), pretty_str('x', self.x), pretty_str('P', self.P), pretty_str('e', self.e), ]) def update(self, z): """ update the filter with measurement z. z must be the same type (or treatable as the same type) as self.x[0]. """ if self.order == 0: G = 1 - self.beta self.x = self.x + dot(G, (z - self.x)) elif self.order == 1: G = 1 - self.beta**2 H = (1-self.beta)**2 x = self.x[0] dx = self.x[1] dxdt = dot(dx, self.dt) residual = z - (x+dxdt) self.x[0] = x + dxdt + G*residual self.x[1] = dx + (H / self.dt)*residual else: # order == 2 G = 1-self.beta**3 H = 1.5*(1+self.beta)*(1-self.beta)**2 K = 0.5*(1-self.beta)**3 x = self.x[0] dx = self.x[1] ddx = self.x[2] dxdt = dot(dx, self.dt) T2 = self.dt**2. residual = z - (x + dxdt + 0.5*ddx*T2) self.x[0] = x + dxdt + 0.5*ddx*T2 + G*residual self.x[1] = dx + ddx*self.dt + (H/self.dt)*residual self.x[2] = ddx + (2*K/(self.dt**2))*residual filterpy-1.4.5/filterpy/memory/tests/000077500000000000000000000000001335747705400177065ustar00rootroot00000000000000filterpy-1.4.5/filterpy/memory/tests/test_fading_memory.py000066400000000000000000000035501335747705400241420ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import (absolute_import, division, print_function, unicode_literals) import matplotlib.pyplot as plt from numpy.random import randn import numpy as np from filterpy.memory import FadingMemoryFilter from filterpy.gh import GHKFilter def dotest_2d_data(): """ tests having multidimensional data for x""" fm = FadingMemoryFilter(x0=np.array([[0.,2.],[0.,0.]]), dt=1, order=1, beta=.6) xs = [x for x in range(0,50)] for x in xs: data = [x+randn()*3, x+2+randn()*3] fm.update(data) plt.scatter(fm.x[0,0], fm.x[0,1], c = 'r') plt.scatter(data[0], data[1], c='b') def dotest_1d(order, beta): fm = FadingMemoryFilter(x0=0, dt=1, order=order, beta=beta) xs = [x for x in range(0,50)] fxs = [] for x in xs: data = x+randn()*3 fm.update(data) plt.scatter(x, fm.x[0], c = 'r') fxs.append(fm.x[0]) plt.scatter(x,data,c='b') plt.plot(fxs, c='r') def test_ghk_formulation(): beta = .6 g = 1-beta**3 h = 1.5*(1+beta)*(1-beta)**2 k = 0.5*(1-beta)**3 f1 = GHKFilter(0,0,0,1, g, h, k) f2 = FadingMemoryFilter(x0=0, dt=1, order=2, beta=beta) def fx(x): return .02*x**2 + 2*x - 3 for i in range(1,100): z = fx(i) f1.update(z) f2.update(z) assert abs(f1.x-f2.x[0]) < 1.e-80 if __name__ == "__main__": test_ghk_formulation() '''dotest_1d(0, .7) dotest_1d(1, .7) dotest_1d(2, .7) plt.figure(2) dotest_2d_data()'''filterpy-1.4.5/filterpy/monte_carlo/000077500000000000000000000000001335747705400175365ustar00rootroot00000000000000filterpy-1.4.5/filterpy/monte_carlo/__init__.py000066400000000000000000000010301335747705400216410ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. filterpy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: disable=wildcard-import from __future__ import (absolute_import, division, print_function, unicode_literals) __all__ = ["resampling"] from .resampling import * filterpy-1.4.5/filterpy/monte_carlo/resampling.py000066400000000000000000000121441335747705400222530ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=C0103, R0913, R0902, C0326, R0914 # disable snake_case warning, too many arguments, too many attributes, # one space before assignment, too many local variables """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ import numpy as np from numpy.random import random def residual_resample(weights): """ Performs the residual resampling algorithm used by particle filters. Based on observation that we don't need to use random numbers to select most of the weights. Take int(N*w^i) samples of each particle i, and then resample any remaining using a standard resampling algorithm [1] Parameters ---------- weights : list-like of float list of weights as floats Returns ------- indexes : ndarray of ints array of indexes into the weights defining the resample. i.e. the index of the zeroth resample is indexes[0], etc. References ---------- .. [1] J. S. Liu and R. Chen. Sequential Monte Carlo methods for dynamic systems. Journal of the American Statistical Association, 93(443):1032–1044, 1998. """ N = len(weights) indexes = np.zeros(N, 'i') # take int(N*w) copies of each weight, which ensures particles with the # same weight are drawn uniformly num_copies = (np.floor(N*np.asarray(weights))).astype(int) k = 0 for i in range(N): for _ in range(num_copies[i]): # make n copies indexes[k] = i k += 1 # use multinormal resample on the residual to fill up the rest. This # maximizes the variance of the samples residual = weights - num_copies # get fractional part residual /= sum(residual) # normalize cumulative_sum = np.cumsum(residual) cumulative_sum[-1] = 1. # avoid round-off errors: ensures sum is exactly one indexes[k:N] = np.searchsorted(cumulative_sum, random(N-k)) return indexes def stratified_resample(weights): """ Performs the stratified resampling algorithm used by particle filters. This algorithms aims to make selections relatively uniformly across the particles. It divides the cumulative sum of the weights into N equal divisions, and then selects one particle randomly from each division. This guarantees that each sample is between 0 and 2/N apart. Parameters ---------- weights : list-like of float list of weights as floats Returns ------- indexes : ndarray of ints array of indexes into the weights defining the resample. i.e. the index of the zeroth resample is indexes[0], etc. """ N = len(weights) # make N subdivisions, and chose a random position within each one positions = (random(N) + range(N)) / N indexes = np.zeros(N, 'i') cumulative_sum = np.cumsum(weights) i, j = 0, 0 while i < N: if positions[i] < cumulative_sum[j]: indexes[i] = j i += 1 else: j += 1 return indexes def systematic_resample(weights): """ Performs the systemic resampling algorithm used by particle filters. This algorithm separates the sample space into N divisions. A single random offset is used to to choose where to sample from for all divisions. This guarantees that every sample is exactly 1/N apart. Parameters ---------- weights : list-like of float list of weights as floats Returns ------- indexes : ndarray of ints array of indexes into the weights defining the resample. i.e. the index of the zeroth resample is indexes[0], etc. """ N = len(weights) # make N subdivisions, and choose positions with a consistent random offset positions = (random() + np.arange(N)) / N indexes = np.zeros(N, 'i') cumulative_sum = np.cumsum(weights) i, j = 0, 0 while i < N: if positions[i] < cumulative_sum[j]: indexes[i] = j i += 1 else: j += 1 return indexes def multinomial_resample(weights): """ This is the naive form of roulette sampling where we compute the cumulative sum of the weights and then use binary search to select the resampled point based on a uniformly distributed random number. Run time is O(n log n). You do not want to use this algorithm in practice; for some reason it is popular in blogs and online courses so I included it for reference. Parameters ---------- weights : list-like of float list of weights as floats Returns ------- indexes : ndarray of ints array of indexes into the weights defining the resample. i.e. the index of the zeroth resample is indexes[0], etc. """ cumulative_sum = np.cumsum(weights) cumulative_sum[-1] = 1. # avoid round-off errors: ensures sum is exactly one return np.searchsorted(cumulative_sum, random(len(weights))) filterpy-1.4.5/filterpy/stats/000077500000000000000000000000001335747705400163725ustar00rootroot00000000000000filterpy-1.4.5/filterpy/stats/__init__.py000066400000000000000000000006601335747705400205050ustar00rootroot00000000000000"""Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ #pylint: disable=wildcard-import from __future__ import absolute_import __all__ = ["stats"] from .stats import * filterpy-1.4.5/filterpy/stats/stats.py000066400000000000000000000765431335747705400201210ustar00rootroot00000000000000# -*- coding: utf-8 -*- # pylint: disable=invalid-name, too-many-arguments, bad-whitespace # pylint: disable=too-many-lines, too-many-locals, len-as-condition """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import absolute_import, division, unicode_literals import math from math import cos, sin import random import warnings import numpy as np from numpy.linalg import inv import scipy.linalg as linalg import scipy.sparse as sp import scipy.sparse.linalg as spln from scipy.stats import norm, multivariate_normal # Older versions of scipy do not support the allow_singular keyword. I could # check the version number explicily, but perhaps this is clearer _support_singular = True try: multivariate_normal.logpdf(1, 1, 1, allow_singular=True) except TypeError: warnings.warn( 'You are using a version of SciPy that does not support the '\ 'allow_singular parameter in scipy.stats.multivariate_normal.logpdf(). '\ 'Future versions of FilterPy will require a version of SciPy that '\ 'implements this keyword', DeprecationWarning) _support_singular = False def _validate_vector(u, dtype=None): # this is taken from scipy.spatial.distance. Internal function, so # redefining here. u = np.asarray(u, dtype=dtype).squeeze() # Ensure values such as u=1 and u=[1] still return 1-D arrays. u = np.atleast_1d(u) if u.ndim > 1: raise ValueError("Input vector should be 1-D.") return u def mahalanobis(x, mean, cov): """ Computes the Mahalanobis distance between the state vector x from the Gaussian `mean` with covariance `cov`. This can be thought as the number of standard deviations x is from the mean, i.e. a return value of 3 means x is 3 std from mean. Parameters ---------- x : (N,) array_like, or float Input state vector mean : (N,) array_like, or float mean of multivariate Gaussian cov : (N, N) array_like or float covariance of the multivariate Gaussian Returns ------- mahalanobis : double The Mahalanobis distance between vectors `x` and `mean` Examples -------- >>> mahalanobis(x=3., mean=3.5, cov=4.**2) # univariate case 0.125 >>> mahalanobis(x=3., mean=6, cov=1) # univariate, 3 std away 3.0 >>> mahalanobis([1., 2], [1.1, 3.5], [[1., .1],[.1, 13]]) 0.42533327058913922 """ x = _validate_vector(x) mean = _validate_vector(mean) if x.shape != mean.shape: raise ValueError("length of input vectors must be the same") y = x - mean S = np.atleast_2d(cov) dist = float(np.dot(np.dot(y.T, inv(S)), y)) return math.sqrt(dist) def log_likelihood(z, x, P, H, R): """ Returns log-likelihood of the measurement z given the Gaussian posterior (x, P) using measurement function H and measurement covariance error R """ S = np.dot(H, np.dot(P, H.T)) + R return logpdf(z, np.dot(H, x), S) def likelihood(z, x, P, H, R): """ Returns likelihood of the measurement z given the Gaussian posterior (x, P) using measurement function H and measurement covariance error R """ return np.exp(log_likelihood(z, x, P, H, R)) def logpdf(x, mean=None, cov=1, allow_singular=True): """ Computes the log of the probability density function of the normal N(mean, cov) for the data x. The normal may be univariate or multivariate. Wrapper for older versions of scipy.multivariate_normal.logpdf which don't support support the allow_singular keyword prior to verion 0.15.0. If it is not supported, and cov is singular or not PSD you may get an exception. `x` and `mean` may be column vectors, row vectors, or lists. """ if mean is not None: flat_mean = np.asarray(mean).flatten() else: flat_mean = None flat_x = np.asarray(x).flatten() if _support_singular: return multivariate_normal.logpdf(flat_x, flat_mean, cov, allow_singular) return multivariate_normal.logpdf(flat_x, flat_mean, cov) def gaussian(x, mean, var, normed=True): """ returns normal distribution (pdf) for x given a Gaussian with the specified mean and variance. All must be scalars. gaussian (1,2,3) is equivalent to scipy.stats.norm(2,math.sqrt(3)).pdf(1) It is quite a bit faster albeit much less flexible than the latter. Parameters ---------- x : scalar or array-like The value for which we compute the probability mean : scalar Mean of the Gaussian var : scalar Variance of the Gaussian norm : bool, default True Normalize the output if the input is an array of values. Returns ------- probability : float probability of x for the Gaussian (mean, var). E.g. 0.101 denotes 10.1%. Examples -------- >>> gaussian(8, 1, 2) 1.3498566943461957e-06 >>> gaussian([8, 7, 9], 1, 2) array([1.34985669e-06, 3.48132630e-05, 3.17455867e-08]) """ g = ((2*math.pi*var)**-.5) * np.exp((-0.5*(np.asarray(x)-mean)**2.) / var) if normed and len(np.shape(g)) > 0: g = g / sum(g) return g def mul(mean1, var1, mean2, var2): """ Multiply Gaussian (mean1, var1) with (mean2, var2) and return the results as a tuple (mean, var). Strictly speaking the product of two Gaussian PDFs is a Gaussian function, not Gaussian PDF. It is, however, proportional to a Gaussian PDF, so it is safe to treat the output as a PDF for any filter using Bayes equation, which normalizes the result anyway. Parameters ---------- mean1 : scalar mean of first Gaussian var1 : scalar variance of first Gaussian mean2 : scalar mean of second Gaussian var2 : scalar variance of second Gaussian Returns ------- mean : scalar mean of product var : scalar variance of product Examples -------- >>> mul(1, 2, 3, 4) (1.6666666666666667, 1.3333333333333333) References ---------- Bromily. "Products and Convolutions of Gaussian Probability Functions", Tina Memo No. 2003-003. http://www.tina-vision.net/docs/memos/2003-003.pdf """ mean = (var1*mean2 + var2*mean1) / (var1 + var2) var = 1 / (1/var1 + 1/var2) return (mean, var) def mul_pdf(mean1, var1, mean2, var2): """ Multiply Gaussian (mean1, var1) with (mean2, var2) and return the results as a tuple (mean, var, scale_factor). Strictly speaking the product of two Gaussian PDFs is a Gaussian function, not Gaussian PDF. It is, however, proportional to a Gaussian PDF. `scale_factor` provides this proportionality constant Parameters ---------- mean1 : scalar mean of first Gaussian var1 : scalar variance of first Gaussian mean2 : scalar mean of second Gaussian var2 : scalar variance of second Gaussian Returns ------- mean : scalar mean of product var : scalar variance of product scale_factor : scalar proportionality constant Examples -------- >>> mul(1, 2, 3, 4) (1.6666666666666667, 1.3333333333333333) References ---------- Bromily. "Products and Convolutions of Gaussian Probability Functions", Tina Memo No. 2003-003. http://www.tina-vision.net/docs/memos/2003-003.pdf """ mean = (var1*mean2 + var2*mean1) / (var1 + var2) var = 1. / (1./var1 + 1./var2) S = math.exp(-(mean1 - mean2)**2 / (2*(var1 + var2))) / \ math.sqrt(2 * math.pi * (var1 + var2)) return mean, var, S def add(mean1, var1, mean2, var2): """ Add the Gaussians (mean1, var1) with (mean2, var2) and return the results as a tuple (mean,var). var1 and var2 are variances - sigma squared in the usual parlance. """ return (mean1+mean2, var1+var2) def multivariate_gaussian(x, mu, cov): """ This is designed to replace scipy.stats.multivariate_normal which is not available before version 0.14. You may either pass in a multivariate set of data: .. code-block:: Python multivariate_gaussian (array([1,1]), array([3,4]), eye(2)*1.4) multivariate_gaussian (array([1,1,1]), array([3,4,5]), 1.4) or unidimensional data: .. code-block:: Python multivariate_gaussian(1, 3, 1.4) In the multivariate case if cov is a scalar it is interpreted as eye(n)*cov The function gaussian() implements the 1D (univariate)case, and is much faster than this function. equivalent calls: .. code-block:: Python multivariate_gaussian(1, 2, 3) scipy.stats.multivariate_normal(2,3).pdf(1) Parameters ---------- x : float, or np.array-like Value to compute the probability for. May be a scalar if univariate, or any type that can be converted to an np.array (list, tuple, etc). np.array is best for speed. mu : float, or np.array-like mean for the Gaussian . May be a scalar if univariate, or any type that can be converted to an np.array (list, tuple, etc).np.array is best for speed. cov : float, or np.array-like Covariance for the Gaussian . May be a scalar if univariate, or any type that can be converted to an np.array (list, tuple, etc).np.array is best for speed. Returns ------- probability : float probability for x for the Gaussian (mu,cov) """ warnings.warn( ("This was implemented before SciPy version 0.14, which implemented " "scipy.stats.multivariate_normal. This function will be removed in " "a future release of FilterPy"), DeprecationWarning) # force all to numpy.array type, and flatten in case they are vectors x = np.array(x, copy=False, ndmin=1).flatten() mu = np.array(mu, copy=False, ndmin=1).flatten() nx = len(mu) cov = _to_cov(cov, nx) norm_coeff = nx*math.log(2*math.pi) + np.linalg.slogdet(cov)[1] err = x - mu if sp.issparse(cov): numerator = spln.spsolve(cov, err).T.dot(err) else: numerator = np.linalg.solve(cov, err).T.dot(err) return math.exp(-0.5*(norm_coeff + numerator)) def multivariate_multiply(m1, c1, m2, c2): """ Multiplies the two multivariate Gaussians together and returns the results as the tuple (mean, covariance). Examples -------- .. code-block:: Python m, c = multivariate_multiply([7.0, 2], [[1.0, 2.0], [2.0, 1.0]], [3.2, 0], [[8.0, 1.1], [1.1,8.0]]) Parameters ---------- m1 : array-like Mean of first Gaussian. Must be convertable to an 1D array via numpy.asarray(), For example 6, [6], [6, 5], np.array([3, 4, 5, 6]) are all valid. c1 : matrix-like Covariance of first Gaussian. Must be convertable to an 2D array via numpy.asarray(). m2 : array-like Mean of second Gaussian. Must be convertable to an 1D array via numpy.asarray(), For example 6, [6], [6, 5], np.array([3, 4, 5, 6]) are all valid. c2 : matrix-like Covariance of second Gaussian. Must be convertable to an 2D array via numpy.asarray(). Returns ------- m : ndarray mean of the result c : ndarray covariance of the result """ C1 = np.asarray(c1) C2 = np.asarray(c2) M1 = np.asarray(m1) M2 = np.asarray(m2) sum_inv = np.linalg.inv(C1+C2) C3 = np.dot(C1, sum_inv).dot(C2) M3 = (np.dot(C2, sum_inv).dot(M1) + np.dot(C1, sum_inv).dot(M2)) return M3, C3 def plot_discrete_cdf(xs, ys, ax=None, xlabel=None, ylabel=None, label=None): """ Plots a normal distribution CDF with the given mean and variance. x-axis contains the mean, the y-axis shows the cumulative probability. Parameters ---------- xs : list-like of scalars x values corresponding to the values in `y`s. Can be `None`, in which case range(len(ys)) will be used. ys : list-like of scalars list of probabilities to be plotted which should sum to 1. ax : matplotlib axes object, optional If provided, the axes to draw on, otherwise plt.gca() is used. xlim, ylim: (float,float), optional specify the limits for the x or y axis as tuple (low,high). If not specified, limits will be automatically chosen to be 'nice' xlabel : str,optional label for the x-axis ylabel : str, optional label for the y-axis label : str, optional label for the legend Returns ------- axis of plot """ import matplotlib.pyplot as plt if ax is None: ax = plt.gca() if xs is None: xs = range(len(ys)) ys = np.cumsum(ys) ax.plot(xs, ys, label=label) ax.set_xlabel(xlabel) ax.set_ylabel(ylabel) return ax def plot_gaussian_cdf(mean=0., variance=1., ax=None, xlim=None, ylim=(0., 1.), xlabel=None, ylabel=None, label=None): """ Plots a normal distribution CDF with the given mean and variance. x-axis contains the mean, the y-axis shows the cumulative probability. Parameters ---------- mean : scalar, default 0. mean for the normal distribution. variance : scalar, default 0. variance for the normal distribution. ax : matplotlib axes object, optional If provided, the axes to draw on, otherwise plt.gca() is used. xlim, ylim: (float,float), optional specify the limits for the x or y axis as tuple (low,high). If not specified, limits will be automatically chosen to be 'nice' xlabel : str,optional label for the x-axis ylabel : str, optional label for the y-axis label : str, optional label for the legend Returns ------- axis of plot """ import matplotlib.pyplot as plt if ax is None: ax = plt.gca() sigma = math.sqrt(variance) n = norm(mean, sigma) if xlim is None: xlim = [n.ppf(0.001), n.ppf(0.999)] xs = np.arange(xlim[0], xlim[1], (xlim[1] - xlim[0]) / 1000.) cdf = n.cdf(xs) ax.plot(xs, cdf, label=label) ax.set_xlim(xlim) ax.set_ylim(ylim) ax.set_xlabel(xlabel) ax.set_ylabel(ylabel) return ax def plot_gaussian_pdf(mean=0., variance=1., std=None, ax=None, mean_line=False, xlim=None, ylim=None, xlabel=None, ylabel=None, label=None): """ Plots a normal distribution PDF with the given mean and variance. x-axis contains the mean, the y-axis shows the probability density. Parameters ---------- mean : scalar, default 0. mean for the normal distribution. variance : scalar, default 1., optional variance for the normal distribution. std: scalar, default=None, optional standard deviation of the normal distribution. Use instead of `variance` if desired ax : matplotlib axes object, optional If provided, the axes to draw on, otherwise plt.gca() is used. mean_line : boolean draws a line at x=mean xlim, ylim: (float,float), optional specify the limits for the x or y axis as tuple (low,high). If not specified, limits will be automatically chosen to be 'nice' xlabel : str,optional label for the x-axis ylabel : str, optional label for the y-axis label : str, optional label for the legend Returns ------- axis of plot """ import matplotlib.pyplot as plt if ax is None: ax = plt.gca() if variance is not None and std is not None: raise ValueError('Specify only one of variance and std') if variance is None and std is None: raise ValueError('Specify variance or std') if variance is not None: std = math.sqrt(variance) n = norm(mean, std) if xlim is None: xlim = [n.ppf(0.001), n.ppf(0.999)] xs = np.arange(xlim[0], xlim[1], (xlim[1] - xlim[0]) / 1000.) ax.plot(xs, n.pdf(xs), label=label) ax.set_xlim(xlim) if ylim is not None: ax.set_ylim(ylim) if mean_line: plt.axvline(mean) if xlabel is not None: ax.set_xlabel(xlabel) if ylabel is not None: ax.set_ylabel(ylabel) return ax def plot_gaussian(mean=0., variance=1., ax=None, mean_line=False, xlim=None, ylim=None, xlabel=None, ylabel=None, label=None): """ DEPRECATED. Use plot_gaussian_pdf() instead. This is poorly named, as there are multiple ways to plot a Gaussian. """ warnings.warn('This function is deprecated. It is poorly named. '\ 'A Gaussian can be plotted as a PDF or CDF. This '\ 'plots a PDF. Use plot_gaussian_pdf() instead,', DeprecationWarning) return plot_gaussian_pdf(mean, variance, ax, mean_line, xlim, ylim, xlabel, ylabel, label) def covariance_ellipse(P, deviations=1): """ Returns a tuple defining the ellipse representing the 2 dimensional covariance matrix P. Parameters ---------- P : nd.array shape (2,2) covariance matrix deviations : int (optional, default = 1) # of standard deviations. Default is 1. Returns (angle_radians, width_radius, height_radius) """ U, s, _ = linalg.svd(P) orientation = math.atan2(U[1, 0], U[0, 0]) width = deviations * math.sqrt(s[0]) height = deviations * math.sqrt(s[1]) if height > width: raise ValueError('width must be greater than height') return (orientation, width, height) def _eigsorted(cov, asc=True): """ Computes eigenvalues and eigenvectors of a covariance matrix and returns them sorted by eigenvalue. Parameters ---------- cov : ndarray covariance matrix asc : bool, default=True determines whether we are sorted smallest to largest (asc=True), or largest to smallest (asc=False) Returns ------- eigval : 1D ndarray eigenvalues of covariance ordered largest to smallest eigvec : 2D ndarray eigenvectors of covariance matrix ordered to match `eigval` ordering. I.e eigvec[:, 0] is the rotation vector for eigval[0] """ eigval, eigvec = np.linalg.eigh(cov) order = eigval.argsort() if not asc: # sort largest to smallest order = order[::-1] return eigval[order], eigvec[:, order] def plot_3d_covariance(mean, cov, std=1., ax=None, title=None, color=None, alpha=1., label_xyz=True, N=60, shade=True, limit_xyz=True, **kwargs): """ Plots a covariance matrix `cov` as a 3D ellipsoid centered around the `mean`. Parameters ---------- mean : 3-vector mean in x, y, z. Can be any type convertable to a row vector. cov : ndarray 3x3 covariance matrix std : double, default=1 standard deviation of ellipsoid ax : matplotlib.axes._subplots.Axes3DSubplot, optional Axis to draw on. If not provided, a new 3d axis will be generated for the current figure title : str, optional If provided, specifies the title for the plot color : any value convertible to a color if specified, color of the ellipsoid. alpha : float, default 1. Alpha value of the ellipsoid. <1 makes is semi-transparent. label_xyz: bool, default True Gives labels 'X', 'Y', and 'Z' to the axis. N : int, default=60 Number of segments to compute ellipsoid in u,v space. Large numbers can take a very long time to plot. Default looks nice. shade : bool, default=True Use shading to draw the ellipse limit_xyz : bool, default=True Limit the axis range to fit the ellipse **kwargs : optional keyword arguments to supply to the call to plot_surface() """ from mpl_toolkits.mplot3d import Axes3D import matplotlib.pyplot as plt # force mean to be a 1d vector no matter its shape when passed in mean = np.atleast_2d(mean) if mean.shape[1] == 1: mean = mean.T if not(mean.shape[0] == 1 and mean.shape[1] == 3): raise ValueError('mean must be convertible to a 1x3 row vector') mean = mean[0] # force covariance to be 3x3 np.array cov = np.asarray(cov) if cov.shape[0] != 3 or cov.shape[1] != 3: raise ValueError("covariance must be 3x3") # The idea is simple - find the 3 axis of the covariance matrix # by finding the eigenvalues and vectors. The eigenvalues are the # radii (squared, since covariance has squared terms), and the # eigenvectors give the rotation. So we make an ellipse with the # given radii and then rotate it to the proper orientation. eigval, eigvec = _eigsorted(cov, asc=True) radii = std * np.sqrt(np.real(eigval)) if eigval[0] < 0: raise ValueError("covariance matrix must be positive definite") # calculate cartesian coordinates for the ellipsoid surface u = np.linspace(0.0, 2.0 * np.pi, N) v = np.linspace(0.0, np.pi, N) x = np.outer(np.cos(u), np.sin(v)) * radii[0] y = np.outer(np.sin(u), np.sin(v)) * radii[1] z = np.outer(np.ones_like(u), np.cos(v)) * radii[2] # rotate data with eigenvector and center on mu a = np.kron(eigvec[:, 0], x) b = np.kron(eigvec[:, 1], y) c = np.kron(eigvec[:, 2], z) data = a + b + c N = data.shape[0] x = data[:, 0:N] + mean[0] y = data[:, N:N*2] + mean[1] z = data[:, N*2:] + mean[2] fig = plt.gcf() if ax is None: ax = fig.add_subplot(111, projection='3d') ax.plot_surface(x, y, z, rstride=3, cstride=3, linewidth=0.1, alpha=alpha, shade=shade, color=color, **kwargs) # now make it pretty! if label_xyz: ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') if limit_xyz: r = radii.max() ax.set_xlim(-r + mean[0], r + mean[0]) ax.set_ylim(-r + mean[1], r + mean[1]) ax.set_zlim(-r + mean[2], r + mean[2]) if title is not None: plt.title(title) #pylint: disable=pointless-statement Axes3D #kill pylint warning about unused import return ax def plot_covariance_ellipse( mean, cov=None, variance=1.0, std=None, ellipse=None, title=None, axis_equal=True, show_semiaxis=False, facecolor=None, edgecolor=None, fc='none', ec='#004080', alpha=1.0, xlim=None, ylim=None, ls='solid'): """ Deprecated function to plot a covariance ellipse. Use plot_covariance instead. See Also -------- plot_covariance """ warnings.warn("deprecated, use plot_covariance instead", DeprecationWarning) plot_covariance(mean=mean, cov=cov, variance=variance, std=std, ellipse=ellipse, title=title, axis_equal=axis_equal, show_semiaxis=show_semiaxis, facecolor=facecolor, edgecolor=edgecolor, fc=fc, ec=ec, alpha=alpha, xlim=xlim, ylim=ylim, ls=ls) def _std_tuple_of(var=None, std=None, interval=None): """ Convienence function for plotting. Given one of var, standard deviation, or interval, return the std. Any of the three can be an iterable list. Examples -------- >>>_std_tuple_of(var=[1, 3, 9]) (1, 2, 3) """ if std is not None: if np.isscalar(std): std = (std,) return std if interval is not None: if np.isscalar(interval): interval = (interval,) return norm.interval(interval)[1] if var is None: raise ValueError("no inputs were provided") if np.isscalar(var): var = (var,) return np.sqrt(var) def plot_covariance( mean, cov=None, variance=1.0, std=None, interval=None, ellipse=None, title=None, axis_equal=True, show_semiaxis=False, show_center=True, facecolor=None, edgecolor=None, fc='none', ec='#004080', alpha=1.0, xlim=None, ylim=None, ls='solid'): """ Plots the covariance ellipse for the 2D normal defined by (mean, cov) `variance` is the normal sigma^2 that we want to plot. If list-like, ellipses for all ellipses will be ploted. E.g. [1,2] will plot the sigma^2 = 1 and sigma^2 = 2 ellipses. Alternatively, use std for the standard deviation, in which case `variance` will be ignored. ellipse is a (angle,width,height) tuple containing the angle in radians, and width and height radii. You may provide either cov or ellipse, but not both. Parameters ---------- mean : row vector like (2x1) The mean of the normal cov : ndarray-like 2x2 covariance matrix variance : float, default 1, or iterable float, optional Variance of the plotted ellipse. May specify std or interval instead. If iterable, such as (1, 2**2, 3**2), then ellipses will be drawn for all in the list. std : float, or iterable float, optional Standard deviation of the plotted ellipse. If specified, variance is ignored, and interval must be `None`. If iterable, such as (1, 2, 3), then ellipses will be drawn for all in the list. interval : float range [0,1), or iterable float, optional Confidence interval for the plotted ellipse. For example, .68 (for 68%) gives roughly 1 standand deviation. If specified, variance is ignored and `std` must be `None` If iterable, such as (.68, .95), then ellipses will be drawn for all in the list. ellipse: (float, float, float) Instead of a covariance, plots an ellipse described by (angle, width, height), where angle is in radians, and the width and height are the minor and major sub-axis radii. `cov` must be `None`. title: str, optional title for the plot axis_equal: bool, default=True Use the same scale for the x-axis and y-axis to ensure the aspect ratio is correct. show_semiaxis: bool, default=False Draw the semiaxis of the ellipse show_center: bool, default=True Mark the center of the ellipse with a cross facecolor, fc: color, default=None If specified, fills the ellipse with the specified color. `fc` is an allowed abbreviation edgecolor, ec: color, default=None If specified, overrides the default color sequence for the edge color of the ellipse. `ec` is an allowed abbreviation alpha: float range [0,1], default=1. alpha value for the ellipse xlim: float or (float,float), default=None specifies the limits for the x-axis ylim: float or (float,float), default=None specifies the limits for the y-axis ls: str, default='solid': line style for the edge of the ellipse """ from matplotlib.patches import Ellipse import matplotlib.pyplot as plt if cov is not None and ellipse is not None: raise ValueError('You cannot specify both cov and ellipse') if cov is None and ellipse is None: raise ValueError('Specify one of cov or ellipse') if facecolor is None: facecolor = fc if edgecolor is None: edgecolor = ec if cov is not None: ellipse = covariance_ellipse(cov) if axis_equal: plt.axis('equal') if title is not None: plt.title(title) ax = plt.gca() angle = np.degrees(ellipse[0]) width = ellipse[1] * 2. height = ellipse[2] * 2. std = _std_tuple_of(variance, std, interval) for sd in std: e = Ellipse(xy=mean, width=sd*width, height=sd*height, angle=angle, facecolor=facecolor, edgecolor=edgecolor, alpha=alpha, lw=2, ls=ls) ax.add_patch(e) x, y = mean if show_center: plt.scatter(x, y, marker='+', color=edgecolor) if xlim is not None: ax.set_xlim(xlim) if ylim is not None: ax.set_ylim(ylim) if show_semiaxis: a = ellipse[0] h, w = height/4, width/4 plt.plot([x, x+ h*cos(a+np.pi/2)], [y, y + h*sin(a+np.pi/2)]) plt.plot([x, x+ w*cos(a)], [y, y + w*sin(a)]) def norm_cdf(x_range, mu, var=1, std=None): """ Computes the probability that a Gaussian distribution lies within a range of values. Parameters ---------- x_range : (float, float) tuple of range to compute probability for mu : float mean of the Gaussian var : float, optional variance of the Gaussian. Ignored if `std` is provided std : float, optional standard deviation of the Gaussian. This overrides the `var` parameter Returns ------- probability : float probability that Gaussian is within x_range. E.g. .1 means 10%. """ if std is None: std = math.sqrt(var) return abs(norm.cdf(x_range[0], loc=mu, scale=std) - norm.cdf(x_range[1], loc=mu, scale=std)) def _to_cov(x, n): """ If x is a scalar, returns a covariance matrix generated from it as the identity matrix multiplied by x. The dimension will be nxn. If x is already a 2D numpy array then it is returned unchanged. Raises ValueError if not positive definite """ if np.isscalar(x): if x < 0: raise ValueError('covariance must be > 0') return np.eye(n) * x x = np.atleast_2d(x) try: # quickly find out if we are positive definite np.linalg.cholesky(x) except: raise ValueError('covariance must be positive definit') return x def rand_student_t(df, mu=0, std=1): """ return random number distributed by student's t distribution with `df` degrees of freedom with the specified mean and standard deviation. """ x = random.gauss(0, std) y = 2.0*random.gammavariate(0.5 * df, 2.0) return x / (math.sqrt(y / df)) + mu def NESS(xs, est_xs, ps): """ Computes the normalized estimated error squared test on a sequence of estimates. The estimates are optimal if the mean error is zero and the covariance matches the Kalman filter's covariance. If this holds, then the mean of the NESS should be equal to or less than the dimension of x. Examples -------- .. code-block: Python xs = ground_truth() est_xs, ps, _, _ = kf.batch_filter(zs) NESS(xs, est_xs, ps) Parameters ---------- xs : list-like sequence of true values for the state x est_xs : list-like sequence of estimates from an estimator (such as Kalman filter) ps : list-like sequence of covariance matrices from the estimator Returns ------- ness : list of floats list of NESS computed for each estimate """ est_err = xs - est_xs ness = [] for x, p in zip(est_err, ps): ness.append(np.dot(x.T, linalg.inv(p)).dot(x)) return ness filterpy-1.4.5/filterpy/stats/tests/000077500000000000000000000000001335747705400175345ustar00rootroot00000000000000filterpy-1.4.5/filterpy/stats/tests/test_stats.py000066400000000000000000000140401335747705400223020ustar00rootroot00000000000000# -*- coding: utf-8 -*- """Copyright 2015 Roger R Labbe Jr. FilterPy library. http://github.com/rlabbe/filterpy Documentation at: https://filterpy.readthedocs.org Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python This is licensed under an MIT license. See the readme.MD file for more information. """ from __future__ import division from math import exp import numpy as np from numpy.linalg import inv import scipy from scipy.spatial.distance import mahalanobis as scipy_mahalanobis from filterpy.stats import norm_cdf, multivariate_gaussian, logpdf, mahalanobis ITERS = 10000 def test_mahalanobis(): global a, b, S # int test a, b, S = 3, 1, 2 assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12 # int list assert abs(mahalanobis([a], [b], [S]) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12 assert abs(mahalanobis([a], b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12 # float a, b, S = 3.123, 3.235235, .01234 assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12 assert abs(mahalanobis([a], [b], [S]) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12 assert abs(mahalanobis([a], b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12 #float array assert abs(mahalanobis(np.array([a]), b, S) - scipy_mahalanobis(a, b, 1/S)) < 1.e-12 #1d array a = np.array([1., 2.]) b = np.array([1.4, 1.2]) S = np.array([[1., 2.], [2., 4.001]]) assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12 #2d array a = np.array([[1., 2.]]) b = np.array([[1.4, 1.2]]) S = np.array([[1., 2.], [2., 4.001]]) assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12 assert abs(mahalanobis(a.T, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12 assert abs(mahalanobis(a, b.T, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12 assert abs(mahalanobis(a.T, b.T, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12 try: # mismatched shapes mahalanobis([1], b, S) assert "didn't catch vectors of different lengths" except ValueError: pass except: assert "raised exception other than ValueError" # okay, now check for numerical accuracy for _ in range(ITERS): N = np.random.randint(1, 20) a = np.random.randn(N) b = np.random.randn(N) S = np.random.randn(N, N) S = np.dot(S, S.T) #ensure positive semi-definite assert abs(mahalanobis(a, b, S) - scipy_mahalanobis(a, b, inv(S))) < 1.e-12 def test_multivariate_gaussian(): # test that we treat lists and arrays the same mean= (0, 0) cov=[[1, .5], [.5, 1]] a = [[multivariate_gaussian((i, j), mean, cov) for i in (-1, 0, 1)] for j in (-1, 0, 1)] b = [[multivariate_gaussian((i, j), mean, np.asarray(cov)) for i in (-1, 0, 1)] for j in (-1, 0, 1)] assert np.allclose(a, b) a = [[multivariate_gaussian((i, j), np.asarray(mean), cov) for i in (-1, 0, 1)] for j in (-1, 0, 1)] assert np.allclose(a, b) try: multivariate_gaussian(1, 1, -1) except: pass else: assert False, "negative variances are meaningless" # test that we get the same results as scipy.stats.multivariate_normal xs = np.random.randn(1000) mean = np.random.randn(1000) var = np.random.random(1000) * 5 for x, m, v in zip(xs, mean, var): assert abs(multivariate_gaussian(x, m, v) - scipy.stats.multivariate_normal(m, v).pdf(x)) < 1.e-12 def _is_inside_ellipse(x, y, ex, ey, orientation, width, height): co = np.cos(orientation) so = np.sin(orientation) xx = x*co + y*so yy = y*co - x*so return (xx / width)**2 + (yy / height)**2 <= 1. def do_plot_test(): import matplotlib.pyplot as plt from numpy.random import multivariate_normal as mnormal from filterpy.stats import covariance_ellipse, plot_covariance p = np.array([[32, 15], [15., 40.]]) x, y = mnormal(mean=(0, 0), cov=p, size=5000).T sd = 2 a, w, h = covariance_ellipse(p, sd) print(np.degrees(a), w, h) count = 0 color = [] for i in range(len(x)): if _is_inside_ellipse(x[i], y[i], 0, 0, a, w, h): color.append('b') count += 1 else: color.append('r') plt.scatter(x, y, alpha=0.2, c=color) plt.axis('equal') plot_covariance(mean=(0., 0.), cov=p, std=[1,2,3], alpha=0.3, facecolor='none') print(count / len(x)) def test_norm_cdf(): # test using the 68-95-99.7 rule mu = 5 std = 3 var = std*std std_1 = (norm_cdf((mu-std, mu+std), mu, var)) assert abs(std_1 - .6827) < .0001 std_1 = (norm_cdf((mu+std, mu-std), mu, std=std)) assert abs(std_1 - .6827) < .0001 std_1half = (norm_cdf((mu+std, mu), mu, var)) assert abs(std_1half - .6827/2) < .0001 std_2 = (norm_cdf((mu-2*std, mu+2*std), mu, var)) assert abs(std_2 - .9545) < .0001 std_3 = (norm_cdf((mu-3*std, mu+3*std), mu, var)) assert abs(std_3 - .9973) < .0001 def test_logpdf(): assert 3.9 < exp(logpdf(1, 1, .01)) < 4. assert 3.9 < exp(logpdf([1], [1], .01)) < 4. assert 3.9 < exp(logpdf([[1]], [[1]], .01)) < 4. logpdf([1., 2], [1.1, 2], cov=np.array([[1., 2], [2, 5]]), allow_singular=False) logpdf([1., 2], [1.1, 2], cov=np.array([[1., 2], [2, 5]]), allow_singular=True) def covariance_3d_plot_test(): import matplotlib.pyplot as plt from filterpy.stats import plot_3d_covariance mu = [13456.3,2320,672.5] C = np.array([[1.0, .03, .2], [.03, 4.0, .0], [.2, .0, 16.1]]) sample = np.random.multivariate_normal(mu, C, size=1000) fig = plt.gcf() ax = fig.add_subplot(111, projection='3d') ax.scatter(xs=sample[:, 0], ys=sample[:, 1], zs=sample[:, 2], s=1) plot_3d_covariance(mu, C, alpha=.4, std=3, limit_xyz=True, ax=ax) if __name__ == "__main__": covariance_3d_plot_test() plt.figure() do_plot_test() filterpy-1.4.5/pypi-install.sh000066400000000000000000000001401335747705400163520ustar00rootroot00000000000000python setup.py sdist --formats=zip python setup.py register -r pypi twine upload dist/* -r pypifilterpy-1.4.5/pypi-test-install.sh000066400000000000000000000002141335747705400173310ustar00rootroot00000000000000python setup.py sdist --formats=gztar,zip python setup.py register -r test #python setup.py sdist upload -r test twine upload dist/* -r testfilterpy-1.4.5/requirements.readthedocs..txt000066400000000000000000000000241335747705400212200ustar00rootroot00000000000000numpy scipy numpydocfilterpy-1.4.5/requirements.txt000066400000000000000000000000431335747705400166570ustar00rootroot00000000000000pip install numpy pip install scipyfilterpy-1.4.5/setup.cfg000066400000000000000000000000501335747705400152120ustar00rootroot00000000000000[metadata] description-file = README.rstfilterpy-1.4.5/setup.py000066400000000000000000000076411335747705400151200ustar00rootroot00000000000000from setuptools import setup, find_packages # Always prefer setuptools over distutils from codecs import open # To use a consistent encoding from os import path import filterpy here = path.abspath(path.dirname(__file__)) # Get the long description from the relevant file with open(path.join(here, 'README.rst'), encoding='utf-8') as f: long_description = f.read() setup( name='filterpy', # Versions should comply with PEP440. For a discussion on single-sourcing # the version across setup.py and the project code, see # http://packaging.python.org/en/latest/tutorial.html#version version=filterpy.__version__, description='Kalman filtering and optimal estimation library', long_description=long_description, # The project's main homepage. url='https://github.com/rlabbe/filterpy', # Author details author='Roger Labbe', author_email='rlabbejr@gmail.com', # Choose your license license='MIT', # See https://pypi.python.org/pypi?%3Aaction=list_classifiers classifiers=[ # How mature is this project? Common values are # 3 - Alpha # 4 - Beta # 5 - Production/Stable 'Development Status :: 5 - Production/Stable', # Indicate who your project is intended for 'Intended Audience :: Developers', 'Intended Audience :: Education', 'Intended Audience :: Science/Research', 'Topic :: Scientific/Engineering', 'Topic :: Scientific/Engineering :: Mathematics', 'Topic :: Scientific/Engineering :: Physics', 'Topic :: Utilities', # Pick your license as you wish (should match "license" above) 'License :: OSI Approved :: MIT License', # Specify the Python versions you support here. In particular, ensure # that you indicate whether you support Python 2, Python 3 or both. 'Programming Language :: Python :: 2', 'Programming Language :: Python :: 2.7', 'Programming Language :: Python :: 3', 'Programming Language :: Python :: 3.4', 'Programming Language :: Python :: 3.5', 'Programming Language :: Python :: 3.6', 'Operating System :: Microsoft :: Windows', 'Operating System :: POSIX', 'Operating System :: Unix', 'Operating System :: MacOS' ], # What does your project relate to? keywords='Kalman filters filtering optimal estimation tracking', # You can just specify the packages manually here if your project is # simple. Or you can use find_packages(). packages=find_packages(exclude=['contrib']), # List run-time dependencies here. These will be installed by pip when your # project is installed. For an analysis of "install_requires" vs pip's # requirements files see: # https://packaging.python.org/en/latest/technical.html#install-requires-vs-requirements-files install_requires=['numpy', 'scipy', 'matplotlib'], # If there are data files included in your packages that need to be # installed, specify them here. If using Python 2.6 or less, then these # have to be included in MANIFEST.in as well. package_data={ 'filterpy': ['README.rst', 'filterpy/changelog.txt', 'LICENSE', 'filterpy/kalman/tests/*.py'], }, # Although 'package_data' is the preferred approach, in some case you may # need to place data files outside of your packages. # see http://docs.python.org/3.4/distutils/setupscript.html#installing-additional-files # In this case, 'data_file' will be installed into '/my_data' #data_files=[('my_data', ['data/data_file'])], # To provide executable scripts, use entry points in preference to the # "scripts" keyword. Entry points provide cross-platform support and allow # pip to create the appropriate form of executable for the target platform. #entry_points={ # 'console_scripts': [ # 'sample=sample:main', # ], #}, )