text: # -*- 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.SI = linalg.inv(self.S)
          self.K = PHT.dot(self.SI)
  
          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)
              ])
  