def __set_obsoffset(self, value): if value is not None: self.__obsoffset = npu.tondim2(value, ndim1tocolumn=True, copy=True) else: self.__obsoffset = None
def observe(self, obs, **kwargs): if 'obsnoisecov' in kwargs: obsnoisecov = kwargs['obsnoisecov'] else: obsnoisecov = self.obsnoisecov assert not obsnoisecov is None, 'The covariance matrix obsnoisecov is not set' # By default, our measurement matrix is the procdim-by-procdim-dimensional identity # matrix: we are observing the state directly. This only makes sense if # procdim == obsdim. if self.obsmap is None: if (not self.procdim is None) and (self.procdim == self.obsdim): warnings.warn( 'The measurement matrix obsmap is not set. Defaulting to procdim-by-procdim-dimensional identity' ) self.obsmap = np.eye(self.procdim) assert not self.obsmap is None, 'The measurement matrix obsmap is not set' if self.obsnoisemap is None: if self.obsdim is not None: warnings.warn( 'The matrix obsnoisemap is not set. Defaulting to obsdim-by-obsdim-dimensional identity' ) self.obsnoisemap = np.eye(self.obsdim) assert not self.obsnoisemap is None, 'The matrix obsnoisemap is not set' obs = npu.tondim2(obs, ndim1tocolumn=True, copy=False) # Here we shall refer to the steps given in [Haykin-2001]_. # Kalman gain matrix (step 3): self.innovcov = np.dot(np.dot( self.obsmap, self.statecov), self.obsmap.T) + np.dot( np.dot(self.obsnoisemap, obsnoisecov), self.obsnoisemap.T) self.gain = np.dot(np.dot(self.statecov, self.obsmap.T), np.linalg.pinv(self.innovcov)) self.predictedobs = np.dot(self.obsmap, self.state) if self.obsoffset is not None: self.predictedobs += self.obsoffset # State estimate update (step 4): self.innov = obs - self.predictedobs self.state = self.state + np.dot(self.gain, self.innov) self.statecov = np.dot( np.identity(self.procdim) - np.dot(self.gain, self.obsmap), self.statecov) # TODO Cater for the multidimensional observation case self.loglikelihood += MINUS_HALF_LN_2PI - .5 * ( np.log(self.innovcov) + self.innov * self.innov / self.innovcov) self._lastobs = obs return self.state
def generatenormalvariates(self, cov, count): cov = npp.checkshapeissquare(npu.tondim2(cov)) count = pre.checknonnegativeinteger(count) dim = np.shape(cov)[0] covroot = np.linalg.cholesky(cov) variates = np.reshape(self.__randomstate.normal(size=count * dim), (count, dim)) return np.dot(variates, covroot.T)
def __set_state(self, value): if value is not None: self.__state = npu.tondim2(value, ndim1tocolumn=True, copy=True) shape = np.shape(self.__state) assert (self.procdim is None) or ( self.procdim == shape[0]), 'The state must be procdim-dimensional' self.procdim = shape[0] else: self.__state = None
def __set_statecov(self, value): if value is not None: self.__statecov = npu.tondim2(value, copy=True) shape = np.shape(self.__statecov) assert shape[0] == shape[1], 'The state covariance must be square' assert (self.procdim is None) or ( self.procdim == shape[0] ), 'The state covariance must be procdim-by-procdim-dimensional' self.procdim = shape[0] else: self.__statecov = None
def cor2cov(cor, var=None, sd=None, copy=True): sd = np.sqrt(var) if var is not None else sd if isinstance(cor, (DiagonalArray, SubdiagonalArray)): cor = cor.tonumpyarray() cor = npu.tondim2(cor, copy=copy) dim = len(var) assert dim == np.shape(cor)[0] and dim == np.shape(cor)[1] np.fill_diagonal(cor, 1.) cor = (sd.T * (sd * cor).T).T npu.lowertosymmetric(cor, copy=False) return cor
def __set_procmap(self, value): if value is not None: self.__procmap = npu.tondim2(value, copy=True) shape = np.shape(self.__procmap) assert shape[0] == shape[ 1], 'The transition matrix procmap must be square' assert (self.procdim is None) or ( self.procdim == shape[0] ), 'The transition matrix procmap must be procdim-by-procdim-dimensional' self.procdim = shape[0] else: self.__procmap = None
def __set_obsnoisecov(self, value): if value is not None: self.__obsnoisecov = npu.tondim2(value, copy=True) shape = np.shape(self.__obsnoisecov) assert shape[0] == shape[ 1], 'The covariance matrix obsnoisecov must be square' assert (self.obsdim is None) or ( self.obsdim == shape[0] ), 'The covariance matrix obsnoisecov must be obsdim-by-obsdim-dimensional' self.obsdim = shape[0] else: self.__obsnoisecov = None
def __set_obsmap(self, value): if value is not None: self.__obsmap = npu.tondim2(value, copy=True) shape = np.shape(self.__obsmap) assert (self.obsdim is None) or ( self.obsdim == shape[0] ), 'The measurement matrix obsmap must have obsdim (%d) rows; it has %d rows' % ( self.obsdim, shape[0]) assert (self.procdim is None) or ( self.procdim == shape[1] ), 'The measurement matrix obsmap must have procdim (%d) columns; it has %d columns' % ( self.procdim, shape[1]) self.obsdim = shape[0] self.procdim = shape[1] else: self.__obsmap = None