Source code for filterpy.hinfinity.hinfinity_filter

# -*- 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)
import numpy as np
import scipy.linalg as linalg
from numpy import dot, zeros, eye
from filterpy.common import setter, setter_scalar, dot3


[docs]class HInfinityFilter(object):
def __init__(self, dim_x, dim_z, dim_u, gamma): """ Create an 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. """ 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._G = 0 # control transistion matrx self._F = 0 # state transition matrix self._H = 0 # Measurement function self._P = eye(dim_x) # uncertainty covariance self._V_inv = zeros((dim_z, dim_z)) self._W = zeros((dim_x, dim_x)) self._Q = eye(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 # kalman gain self.residual = 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 kalman filter. If Z is None, nothing is changed. **Parameters** Z : np.array 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) + dot3(HTVI, H, P)) #common subexpression P*L PL = dot(P,L) K = dot3(F, PL, HTVI) self.residual = Z - dot(H, x) # x = x + Ky # predict new x with residual scaled by the kalman gain self._x = self._x + dot(K, self.residual) self._P = dot3(F, PL, F.T) + W # force P to be symmetric self._P = (self._P + self._P.T) / 2 '''def update_safe(self, Z): """ same as update(), except we perform a check to ensure that the eigenvalues are < 1. An exception is thrown if not. """ update(Z) evalue = linalg.eig(self.P)' ''' def predict(self, u=0): """ Predict next position. **Parameters** u : np.array Optional control vector. If non-zero, it is multiplied by G to create the control input into the system. """ # x = Fx + Gu self._x = dot(self._F, self._x) + dot(self._G, u) 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. 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`. """ 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() else: for i,(z,r) in enumerate(zip(Zs,Rs)): self.predict() self.update(z,r) means[i,:] = self.x covariances[i,:,:] = self.P 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 : np.array optional control input **Returns** x : numpy.ndarray State vecto of the prediction. """ x = dot(self._F, self._x) + dot(self._G, u) return x 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 x(self): """ state vector property""" return self._x @x.setter def x(self, value): self._x = setter(value, self.dim_x, 1) @property def G(self): return self._G @G.setter def G(self, value): self._G = setter(self.dim_x, 1) @property def P(self): """ covariance matrix property""" return self._P @P.setter def P(self, value): self._P = setter_scalar(value, self.dim_x) @property def F(self): return self._F @F.setter def F(self, value): self._F = setter(value, self.dim_x, self.dim_x) @property def G(self): return self._G @G.setter def G(self, value): self._G = setter(value, self.dim_x, self.dim_u) @property def H(self): return self._H @H.setter def H(self, value): self._H = setter(value, self.dim_z, self.dim_x) @property def V(self): return self._V @V.setter def V(self, value): self._V = setter_scalar(value, self.dim_z) self._V_inv = linalg.inv(self.V) @property def W(self): return self._W @W.setter def W(self, value): self._W = setter_scalar(value, self.dim_x) @property def Q(self): return self._Q @Q.setter def Q(self, value): self._Q = setter_scalar(value, self.dim_x)