H-Infinity Filter

HInfinityFilter

H-Infinity filter implementation for robust state estimation.

API Reference

HInfinityFilter

Bases: 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:

Name	Type	Description	Default
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	required
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.	required
dim_u	int	Number of control inputs for the Gu part of the prediction step.	required
gamma	float		required
Warning		I do not believe this code is correct. DO NOT USE THIS. In particular, note that predict does not update the covariance matrix.	required

Source code in bayesian_filters/hinfinity/hinfinity_filter.py

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: bool = False, saver: Optional[SaverProtocol] = 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'.

        update_first : bool, default=False, optional,
            controls whether the order of operations is update followed by
            predict, or predict followed by update.

        saver : bayesian_filters.common.Saver, optional
            bayesian_filters.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),
            ]
        )

V property writable

measurement noise matrix

batch_filter(Zs, update_first=False, saver=None)

Batch processes a sequences of measurements.

Parameters:

Name	Type	Description	Default
Zs	list - like	list of measurements at each time step self.dt Missing measurements must be represented by 'None'.	required
update_first	bool	controls whether the order of operations is update followed by predict, or predict followed by update.	False, optional,
saver	Saver	bayesian_filters.common.Saver object. If provided, saver.save() will be called after every epoch	None

Returns:

Name	Type	Description
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.

Source code in bayesian_filters/hinfinity/hinfinity_filter.py

def batch_filter(self, Zs, update_first: bool = False, saver: Optional[SaverProtocol] = 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'.

    update_first : bool, default=False, optional,
        controls whether the order of operations is update followed by
        predict, or predict followed by update.

    saver : bayesian_filters.common.Saver, optional
        bayesian_filters.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)

get_prediction(u=0)

Predicts the next state of the filter and returns it. Does not alter the state of the filter.

Parameters:

Name	Type	Description	Default
u	ndarray	optional control input	0

Returns:

Name	Type	Description
x	ndarray	State vector of the prediction.

Source code in bayesian_filters/hinfinity/hinfinity_filter.py

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)

measurement_of_state(x)

Helper function that converts a state into a measurement.

Parameters:

Name	Type	Description	Default
x	ndarray	H-Infinity state vector	required

Returns:

Name	Type	Description
z	ndarray	measurement corresponding to the given state

Source code in bayesian_filters/hinfinity/hinfinity_filter.py

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)

predict(u=0)

Predict next position.

Parameters:

Name	Type	Description	Default
u	ndarray	Optional control vector. If non-zero, it is multiplied by B to create the control input into the system.	0

Source code in bayesian_filters/hinfinity/hinfinity_filter.py

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)

residual_of(z)

returns the residual for the given measurement (z). Does not alter the state of the filter.

Source code in bayesian_filters/hinfinity/hinfinity_filter.py

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)

update(z)

Add a new measurement z to the H-Infinity filter. If z is None, nothing is changed.

Parameters:

Name	Type	Description	Default
z	ndarray	measurement for this update.	required

Source code in bayesian_filters/hinfinity/hinfinity_filter.py

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)