Source code for filterpy.hinfinity.hinfinity_filter

# -*- 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.

Documentation at:

Supporting book at:

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

[docs]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. """
[docs] 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)
[docs] 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)
[docs] 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)
[docs] 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, 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: 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: return (means, covariances)
[docs] 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)
[docs] 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)
[docs] 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), ])