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
