py-neuromodulation 0.0.5__py3-none-any.whl → 0.0.6__py3-none-any.whl

py_neuromodulation/filter/kalman_filter_external.py

@@ -0,0 +1,1890 @@
+"""
+This file contains unmodified code from the FilterPy library, specifically the KalmanFilter class.
+Original sources:
+- https://github.com/rlabbe/filterpy/blob/master/filterpy/kalman/kalman_filter.py
+- https://github.com/rlabbe/filterpy/blob/master/filterpy/common/discretization.py
+- https://github.com/rlabbe/filterpy/blob/master/filterpy/stats/stats.py
+
+Certain modifications were made to the original code:
+- Updated some code to be compatible with newer Python versions and package versions
+- Made some simplifications to adjust the code for our purposes
+- Refactored some code to be more in line with our codebase style
+
+Original software under MIT license:
+
+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.
+
+------------------------------------------------------------------------------------
+
+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 copy import deepcopy
+import sys
+import numpy as np
+from scipy.stats import multivariate_normal
+
+
+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 = np.zeros((dim_x, 1))  # state
+        self.P = np.eye(dim_x)  # uncertainty covariance
+        self.Q = np.eye(dim_x)  # process uncertainty
+        self.B = None  # control transition matrix
+        self.F = np.eye(dim_x)  # state transition matrix
+        self.H = np.zeros((dim_z, dim_x))  # Measurement function
+        self.R = np.eye(dim_z)  # state uncertainty
+        self._alpha_sq = 1.0  # 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 = np.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 = np.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 np.isscalar(Q):
+            Q = np.eye(self.dim_x) * Q
+
+        # x = Fx + Bu
+        if B is not None and u is not None:
+            self.x = np.dot(F, self.x) + np.dot(B, u)
+        else:
+            self.x = np.dot(F, self.x)
+
+        # P = FPF' + Q
+        self.P = self._alpha_sq * np.dot(np.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 = np.zeros((self.dim_z, 1))
+            return
+
+        z = reshape_z(z, self.dim_z, self.x.ndim)
+
+        if R is None:
+            R = self.R
+        elif np.isscalar(R):
+            R = np.eye(self.dim_z) * R
+
+        if H is None:
+            H = self.H
+
+        # y = z - Hx
+        # error (residual) between measurement and prediction
+        self.y = z - np.dot(H, self.x)
+
+        # common subexpression for speed
+        PHT = np.dot(self.P, H.T)
+
+        # S = HPH' + R
+        # project system uncertainty into measurement space
+        self.S = np.dot(H, PHT) + R
+        self.SI = self.inv(self.S)
+        # K = PH'inv(S)
+        # map system uncertainty into kalman gain
+        self.K = np.dot(PHT, self.SI)
+
+        # x = x + Ky
+        # predict new x with residual scaled by the kalman gain
+        self.x = self.x + np.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 - np.dot(self.K, H)
+        self.P = np.dot(np.dot(I_KH, self.P), I_KH.T) + np.dot(
+            np.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 = np.dot(self.F, self.x) + np.dot(B, u)
+        else:
+            self.x = np.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 = np.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 - np.dot(self.H, self.x)
+
+        # x = x + Ky
+        # predict new x with residual scaled by the kalman gain
+        self.x = self.x + np.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 = np.zeros((self.dim_z, 1))
+            return
+
+        z = reshape_z(z, self.dim_z, self.x.ndim)
+
+        if R is None:
+            R = self.R
+        elif np.isscalar(R):
+            R = np.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 np.shape(z) == (1, 1):
+            z = z[0]
+
+        if np.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 - np.dot(H, self.x)
+
+        # common subexpression for speed
+        PHT = np.dot(self.P, H.T)
+
+        # project system uncertainty into measurement space
+        self.S = np.dot(H, PHT) + np.dot(H, self.M) + np.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 = np.dot(PHT + self.M, self.SI)
+
+        # x = x + Ky
+        # predict new x with residual scaled by the kalman gain
+        self.x = self.x + np.dot(self.K, self.y)
+        self.P = self.P - np.dot(self.K, np.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 = np.zeros((n, self.dim_x))
+            means_p = np.zeros((n, self.dim_x))
+        else:
+            means = np.zeros((n, self.dim_x, 1))
+            means_p = np.zeros((n, self.dim_x, 1))
+
+        # state covariances from Kalman Filter
+        covariances = np.zeros((n, self.dim_x, self.dim_x))
+        covariances_p = np.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 = np.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] = np.dot(np.dot(Fs[k + 1], P[k]), Fs[k + 1].T) + Qs[k + 1]
+
+            # pylint: disable=bad-whitespace
+            K[k] = np.dot(np.dot(P[k], Fs[k + 1].T), inv(Pp[k]))
+            x[k] += np.dot(K[k], x[k + 1] - np.dot(Fs[k + 1], x[k]))
+            P[k] += np.dot(np.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 = np.dot(self.F, self.x) + np.dot(self.B, u)
+        P = self._alpha_sq * np.dot(np.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 - np.dot(H, x)
+
+        # common subexpression for speed
+        PHT = np.dot(P, H.T)
+
+        # project system uncertainty into measurement space
+        S = np.dot(H, PHT) + R
+
+        # map system uncertainty into kalman gain
+        K = np.dot(PHT, self.inv(S))
+
+        # predict new x with residual scaled by the kalman gain
+        x = x + np.dot(K, y)
+
+        # P = (I-KH)P(I-KH)' + KRK'
+        I_KH = self._I - np.dot(K, H)
+        P = np.dot(np.dot(I_KH, P), I_KH.T) + np.dot(np.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 - np.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 np.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 = np.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 = np.sqrt(
+                float(np.dot(np.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**0.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 np.log(sys.float_info.min)
+        return logpdf(z, np.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], np.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 = np.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 = np.shape(z)
+        else:
+            z_shape = (self.dim_z, 1)
+
+        # H@x must have shape of z
+        Hx = np.dot(H, x)
+
+        if z_shape == ():  # scalar or np.array(scalar)
+            assert Hx.ndim == 1 or np.shape(Hx) == (
+                1,
+                1,
+            ), "shape of z should be {}, not {} for the given H".format(
+                np.shape(Hx), z_shape
+            )
+
+        elif np.shape(Hx) == (1,):
+            assert z_shape[0] == 1, "Shape of z must be {} for the given H".format(
+                np.shape(Hx)
+            )
+
+        else:
+            assert z_shape == np.shape(Hx) or (
+                len(z_shape) == 1 and np.shape(Hx) == (z_shape[0], 1)
+            ), "shape of z should be {}, not {} for the given H".format(
+                np.shape(Hx), z_shape
+            )
+
+        if np.ndim(Hx) > 1 and np.shape(Hx) != (1, 1):
+            assert (
+                np.shape(Hx) == z_shape
+            ), "shape of z should be {} for the given H, but it is {}".format(
+                np.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(np.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 = np.dot(np.dot(H, P), H.T) + R
+
+    # map system uncertainty into kalman gain
+    try:
+        K = np.dot(np.dot(P, H.T), np.linalg.inv(S))
+    except Exception:
+        # can't invert a 1D array, annoyingly
+        K = np.dot(np.dot(P, H.T), 1.0 / S)
+
+    # predict new x with residual scaled by the kalman gain
+    x = x + np.dot(K, y)
+
+    # P = (I-KH)P(I-KH)' + KRK'
+    KH = np.dot(K, H)
+
+    try:
+        I_KH = np.eye(KH.shape[0]) - KH
+    except Exception:
+        I_KH = np.array([1 - KH])
+    P = np.dot(np.dot(I_KH, P), I_KH.T) + np.dot(np.dot(K, R), K.T)
+
+    if return_all:
+        # compute log likelihood
+        log_likelihood = logpdf(z, np.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(np.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 + np.dot(K, y)
+
+
+def predict(x, P, F=1, Q=0, u=0, B=1, alpha=1.0):
+    """
+    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 = np.dot(F, x) + np.dot(B, u)
+    P = (alpha * alpha) * np.dot(np.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 = np.dot(F, x) + np.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 = np.zeros((n, dim_x))
+        means_p = np.zeros((n, dim_x))
+    else:
+        means = np.zeros((n, dim_x, 1))
+        means_p = np.zeros((n, dim_x, 1))
+
+    # state covariances from Kalman Filter
+    covariances = np.zeros((n, dim_x, dim_x))
+    covariances_p = np.zeros((n, dim_x, dim_x))
+
+    if us is None:
+        us = [0.0] * n
+        Bs = [0.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 = np.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] = np.dot(np.dot(Fs[k], P[k]), Fs[k].T) + Qs[k]
+
+        # pylint: disable=bad-whitespace
+        K[k] = np.dot(np.dot(P[k], Fs[k].T), np.linalg.inv(pP[k]))
+        x[k] += np.dot(K[k], x[k + 1] - np.dot(Fs[k], x[k]))
+        P[k] += np.dot(np.dot(K[k], P[k + 1] - pP[k]), K[k].T)
+
+    return (x, P, K, pP)
+
+
+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 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 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()
+
+    return multivariate_normal.logpdf(flat_x, flat_mean, cov, allow_singular)