def bayes(self, yt, cond = np.empty(0)): r"""Perform exact bayes rule. :param yt: observation at time t :type yt: 1D :class:`numpy.ndarray` :param cond: control (intervention) vector at time t. May be unspecified if the filter is control-less. :type cond: 1D :class:`numpy.ndarray` :return: always returns True (see :meth:`~Filter.posterior` to get posterior density) """ if not isinstance(yt, np.ndarray): raise TypeError("yt must be and instance of numpy.ndarray ({0} given)".format(type(yt))) if yt.ndim != 1 or yt.shape[0] != self.j: raise ValueError("yt must have shape {0}. ({1} given)".format((self.j,), (yt.shape[0],))) if not isinstance(cond, np.ndarray): raise TypeError("cond must be and instance of numpy.ndarray ({0} given)".format(type(cond))) if cond.ndim != 1 or cond.shape[0] != self.k: raise ValueError("cond must have shape {0}. ({1} given)".format((self.k,), (cond.shape[0],))) # predict self.P.mu = np.dot(self.A, self.P.mu) + np.dot(self.B, cond) # prior state mean estimate self.P.R = np.dot(np.dot(self.A, self.P.R), self.A.T) + self.Q # prior state covariance estimate # data update self.S.mu = np.dot(self.C, self.P.mu) + np.dot(self.D, cond) # prior observation mean estimate self.S.R = np.dot(np.dot(self.C, self.P.R), self.C.T) + self.R # prior observation covariance estimate # kalman gain K = np.dot(np.dot(self.P.R, self.C.T), linalg.inv(self.S.R)) # update according to observation self.P.mu += np.dot(K, (yt - self.S.mu)) # posterior state mean estimate self.P.R -= np.dot(np.dot(K, self.C), self.P.R) # posterior state covariance estimate return True
def __init__(self, n, init_pdf, p_bt_btp, kalman_args, kalman_class = KalmanFilter): r"""Initialise marginalized particle filter. :param int n: number of particles :param init_pdf: probability density which initial particles are sampled from. (both :math:`a_t` and :math:`b_t` parts) :type init_pdf: :class:`~pybayes.pdfs.Pdf` :param p_bt_btp: :math:`p(b_t|b_{t-1})` cpdf of the (b part of the) state in *t* given state in *t-1* :type p_bt_btp: :class:`~pybayes.pdfs.CPdf` :param dict kalman_args: arguments for the Kalman filter, passed as dictionary; *state_pdf* key should not be speficied as it is supplied by the marginalized particle filter :param class kalman_class: class of the filter used for the :math:`a_t` part of the system; defaults to :class:`KalmanFilter` """ if not isinstance(n, int) or n < 1: raise TypeError("n must be a positive integer") if not isinstance(init_pdf, Pdf) or not isinstance(p_bt_btp, CPdf): raise TypeError("init_pdf must be a Pdf and p_bt_btp must be a CPdf") if not issubclass(kalman_class, KalmanFilter): raise TypeError("kalman_class must be a subclass (not an instance) of KalmanFilter") b_shape = p_bt_btp.shape() if p_bt_btp.cond_shape() != b_shape: raise ValueError("p_bt_btp's shape ({0}) and cond shape ({1}) must both be {2}".format( p_bt_btp.shape(), p_bt_btp.cond_shape(), b_shape)) self.p_bt_btp = p_bt_btp a_shape = init_pdf.shape() - b_shape # this will be removed when hardcoding Q,R into kalman filter will be removed kalman_args['Q'] = np.array([[-123.]]) kalman_args['R'] = np.array([[-494658.]]) # generate both initial parts of particles init_particles = init_pdf.samples(n) # create all Kalman filters first self.kalmans = np.empty(n, dtype=KalmanFilter) # array of references to Kalman filters gausses = np.empty(n, dtype=GaussPdf) # array of Kalman filter state pdfs for i in range(n): gausses[i] = GaussPdf(init_particles[i,0:a_shape], np.array([[1.]])) kalman_args['state_pdf'] = gausses[i] self.kalmans[i] = kalman_class(**kalman_args) # construct apost pdf. Important: reference to ith GaussPdf is shared between ith Kalman # filter's state_pdf and ith memp't gauss self.memp = MarginalizedEmpPdf(gausses, init_particles[:,a_shape:])
def __init__(self, A, B = None, C = None, D = None, Q = None, R = None, state_pdf = None): r"""Initialise Kalman filter. :param A: process model matrix :math:`A_t` from :class:`class description <KalmanFilter>` :type A: 2D :class:`numpy.ndarray` :param B: process control model matrix :math:`B_t` from :class:`class description <KalmanFilter>`; may be None or unspecified for control-less systems :type B: 2D :class:`numpy.ndarray` :param C: observation model matrix :math:`C_t` from :class:`class description <KalmanFilter>`; must be full-ranked :type C: 2D :class:`numpy.ndarray` :param D: observation control model matrix :math:`D_t` from :class:`class description <KalmanFilter>`; may be None or unspecified for control-less systems :type D: 2D :class:`numpy.ndarray` :param Q: process noise covariance matrix :math:`Q_t` from :class:`class description <KalmanFilter>`; must be positive definite :type Q: 2D :class:`numpy.ndarray` :param R: observation noise covariance matrix :math:`R_t` from :class:`class description <KalmanFilter>`; must be positive definite :type R: 2D :class:`numpy.ndarray` :param state_pdf: initial state pdf; this object is referenced and used throughout whole life of KalmanFilter, so it is not safe to reuse state pdf for other purposes :type state_pdf: :class:`~pybayes.pdfs.GaussPdf` All matrices can be time-varying - you can modify or replace all above stated matrices providing that you don't change their shape and all constraints still hold. On the other hand, you **should not modify state_pdf** unless you really know what you are doing. >>> # initialise control-less Kalman filter: >>> kf = pb.KalmanFilter(A=np.array([[1.]]), C=np.array([[1.]]), Q=np.array([[0.7]]), R=np.array([[0.3]]), state_pdf=pb.GaussPdf(...)) """ # check type of pdf if not isinstance(state_pdf, GaussPdf): raise TypeError("state_pdf must be (a subclass of) GaussPdf") # check type of input arrays matrices = {"A":A, "B":B, "C":C, "D":D, "Q":Q, "R":R} for name in matrices: matrix = matrices[name] if name == 'B' and matrix is None: # we allow B to be unspecified matrices['B'] = matrix = B = np.empty((A.shape[0], 0)) if name == 'D' and matrix is None: # we allow D to be unspecified matrices['D'] = matrix = D = np.empty((C.shape[0], 0)) if not isinstance(matrix, np.ndarray): raise TypeError("{0} must be numpy.ndarray, {1} given".format(name, type(matrix))) if matrix.ndim != 2: raise ValueError("{0} must have 2 dimensions (forming a matrix) {1} given".format( name, matrix.ndim)) # remember vector shapes self.n = state_pdf.shape() # dimension of state vector self.k = B.shape[1] # dimension of control vector self.j = C.shape[0] # dimension of observation vector # dict of required matrice shapes (sizes) shapes = { "A":(self.n, self.n), "B":(self.n, self.k), "C":(self.j, self.n), "D":(self.j, self.k), "Q":(self.n, self.n), "R":(self.j, self.j) } # check input matrix sizes for name in matrices: matrix = matrices[name] if matrix.shape != shapes[name]: raise ValueError("Given shapes of state_pdf, B and C, matrix " + name + " must have shape " + str(shapes[name]) + ", " + str(matrix.shape) + " given") self.A, self.B, self.C, self.D, self.Q, self.R = A, B, C, D, Q, R self.P = state_pdf self.S = GaussPdf(np.array([0.]), np.array([[1.]])) # observation probability density function