def _discrete_extkalman_update(time, randvar, data, measmod, **kwargs):
mpred, cpred = randvar.mean, randvar.cov
if np.isscalar(mpred) and np.isscalar(cpred):
mpred, cpred = mpred * np.ones(1), cpred * np.eye(1)
jacob = measmod.jacobian(time, mpred, **kwargs)
meascov = measmod.diffusionmatrix(time, **kwargs)
meanest = measmod.dynamics(time, mpred, **kwargs)
return linear_discrete_update(meanest, cpred, data, meascov, jacob, mpred)
def _update_discrete_linear(time, randvar, data, measmod, **kwargs):
mpred, cpred = randvar.mean, randvar.cov
if np.isscalar(mpred) and np.isscalar(cpred):
mpred, cpred = mpred * np.ones(1), cpred * np.eye(1)
measmat = measmod.dynamicsmatrix(time, **kwargs)
meascov = measmod.diffusionmatrix(time, **kwargs)
meanest = measmat @ mpred
return linear_discrete_update(meanest, cpred, data, meascov, measmat,
mpred)
def _discrete_kalman_update(time, randvar, data, measurementmodel, **kwargs):
"""Discrete Kalman update."""
mpred, cpred = randvar.mean, randvar.cov
if np.isscalar(mpred) and np.isscalar(cpred):
mpred, cpred = mpred * np.ones(1), cpred * np.eye(1)
measmat = measurementmodel.dynamicsmatrix(time, **kwargs)
meascov = measurementmodel.diffusionmatrix(time, **kwargs)
meanest = measmat @ mpred
return linear_discrete_update(meanest, cpred, data, meascov, measmat,
mpred)