def test_sampling_shapes_1d(locs, size):
"""Make the sampling tests for a 1d posterior."""
locations = np.linspace(0, 2 * np.pi, 100)
data = 0.5 * np.random.randn(100) + np.sin(locations)
prior = statespace.IBM(0, 1)
measmod = statespace.DiscreteLTIGaussian(state_trans_mat=np.eye(1),
shift_vec=np.zeros(1),
proc_noise_cov_mat=np.eye(1))
initrv = randvars.Normal(np.zeros(1), np.eye(1))
kalman = filtsmooth.Kalman(prior, measmod, initrv)
regression_problem = problems.RegressionProblem(observations=data,
locations=locations)
posterior, _ = kalman.filtsmooth(regression_problem)
size = utils.as_shape(size)
if locs is None:
base_measure_reals = np.random.randn(
*(size + posterior.locations.shape + (1, )))
samples = posterior.transform_base_measure_realizations(
base_measure_reals, t=posterior.locations)
else:
locs = np.union1d(locs, posterior.locations)
base_measure_reals = np.random.randn(*(size + (len(locs), )) + (1, ))
samples = posterior.transform_base_measure_realizations(
base_measure_reals, t=locs)
assert samples.shape == base_measure_reals.shape
def __init__(
self,
ivp: IVP,
prior: statespace.Integrator,
measurement_model: statespace.DiscreteGaussian,
with_smoothing: bool,
init_implementation: Callable[[
Callable,
np.ndarray,
float,
statespace.Integrator,
randvars.Normal,
Optional[Callable],
], randvars.Normal, ],
initrv: Optional[randvars.Normal] = None,
):
if initrv is None:
initrv = randvars.Normal(
np.zeros(prior.dimension),
np.eye(prior.dimension),
cov_cholesky=np.eye(prior.dimension),
)
self.gfilt = filtsmooth.Kalman(dynamics_model=prior,
measurement_model=measurement_model,
initrv=initrv)
if not isinstance(prior, statespace.Integrator):
raise ValueError(
"Please initialise a Gaussian filter with an Integrator (see `probnum.statespace`)"
)
self.sigma_squared_mle = 1.0
self.with_smoothing = with_smoothing
self.init_implementation = init_implementation
super().__init__(ivp=ivp, order=prior.ordint)
def test_kalman_smoother_high_order_ibm():
"""The highest feasible order (without damping, which we dont use) is 11.
If this test breaks, someone played with the stable square-root
implementations in discrete_transition: for instance,
solve_triangular() and cho_solve() must not be changed to inv()!
"""
regression_problem, statespace_components = filtsmooth_zoo.car_tracking(
model_ordint=11,
timespan=(0.0, 1e-3),
step=1e-5,
forward_implementation="sqrt",
backward_implementation="sqrt",
)
truth = regression_problem.solution
kalman = filtsmooth.Kalman(
statespace_components["dynamics_model"],
statespace_components["measurement_model"],
statespace_components["initrv"],
)
posterior, _ = kalman.filtsmooth(regression_problem)
filtms = posterior.filtering_posterior.states.mean
smooms = posterior.states.mean
filtms_rmse = np.mean(np.abs(filtms[:, :2] - truth[:, :2]))
smooms_rmse = np.mean(np.abs(smooms[:, :2] - truth[:, :2]))
obs_rmse = np.mean(np.abs(regression_problem.observations - truth[:, :2]))
assert smooms_rmse < filtms_rmse < obs_rmse
def setup(self, linearization_implementation):
measvar = 0.1024
initrv = randvars.Normal(np.ones(2), measvar * np.eye(2))
regression_problem, statespace_components = filtsmooth_zoo.pendulum(
measurement_variance=measvar,
timespan=(0.0, 4.0),
step=0.0075,
initrv=initrv,
)
linearization, implementation = linearization_implementation
_lin_method = {
"ekf":
functools.partial(
filtsmooth.DiscreteEKFComponent,
forward_implementation=implementation,
backward_implementation=implementation,
),
"ukf":
filtsmooth.DiscreteUKFComponent,
}[linearization]
linearized_dynmod = _lin_method(
statespace_components["dynamics_model"])
linearized_measmod = _lin_method(
statespace_components["measurement_model"])
self.kalman_filter = filtsmooth.Kalman(
dynamics_model=linearized_dynmod,
measurement_model=linearized_measmod,
initrv=statespace_components["initrv"],
)
self.filtering_posterior, _ = self.kalman_filter.filter(
regression_problem)
def kalman(problem):
"""Create a Kalman object."""
dynmod, measmod, initrv, info = problem
return pnfs.Kalman(
dynmod,
measmod,
initrv,
)
def setup(problem):
"""Filter and regression problem."""
regression_problem, statespace_components = problem
kalman = filtsmooth.Kalman(
statespace_components["dynamics_model"],
statespace_components["measurement_model"],
statespace_components["initrv"],
)
return kalman, regression_problem
def ivp2ukf(ivp, prior, evlvar):
"""Computes measurement model and initial distribution for EKF based on IVP and
prior.
Returns ExtendedKalmanFilter object.
evlvar : float, (this is "R")
"""
ukf_mod = pnfs.DiscreteUKFComponent.from_ode(ivp, prior, evlvar)
initrv = _initialdistribution(ivp, prior)
return pnfs.Kalman(prior, ukf_mod, initrv)
def ivp2ekf1(ivp, prior, evlvar):
"""Computes measurement model and initial distribution for EKF based on IVP and
prior.
Returns ExtendedKalmanFilter object.
evlvar : float, (this is "R")
"""
ekf_mod = pnfs.DiscreteEKFComponent.from_ode(
ivp,
prior,
evlvar,
ek0_or_ek1=1,
forward_implementation="sqrt",
backward_implementation="sqrt",
)
initrv = _initialdistribution(ivp, prior)
return pnfs.Kalman(prior, ekf_mod, initrv)
def setup(self, linearization_implementation, num_samples):
measvar = 0.1024
initrv = randvars.Normal(np.ones(2), measvar * np.eye(2))
regression_problem, statespace_components = filtsmooth_zoo.pendulum(
measurement_variance=measvar,
timespan=(0.0, 4.0),
step=0.0075,
initrv=initrv,
)
linearization, implementation = linearization_implementation
_lin_method = {
"ekf":
functools.partial(
filtsmooth.DiscreteEKFComponent,
forward_implementation=implementation,
backward_implementation=implementation,
),
"ukf":
filtsmooth.DiscreteUKFComponent,
}[linearization]
self.dense_locations = np.sort(
np.unique(
np.random.uniform(
low=regression_problem.locations[0],
high=1.2 * regression_problem.locations[-1],
size=int(1.2 * len(regression_problem.locations)),
)))
linearized_dynmod = _lin_method(
statespace_components["dynamics_model"])
linearized_measmod = _lin_method(
statespace_components["measurement_model"])
self.kalman_filter = filtsmooth.Kalman(
dynamics_model=linearized_dynmod,
measurement_model=linearized_measmod,
initrv=statespace_components["initrv"],
)
self.filtering_posterior, _ = self.kalman_filter.filter(
regression_problem)
self.smoothing_posterior = self.kalman_filter.smooth(
filter_posterior=self.filtering_posterior)
def initialize_odefilter_with_rk(f,
y0,
t0,
prior,
initrv,
df=None,
h0=1e-2,
method="DOP853"):
r"""Initialize an ODE filter by fitting the prior process to a few steps of an approximate ODE solution computed with Scipy's RK.
It goes as follows:
1. The ODE integration problem is set up on the interval ``[t0, t0 + (2*order+1)*h0]``
and solved with a call to ``scipy.integrate.solve_ivp``. The solver is uses adaptive steps with ``atol=rtol=1e-12``,
but is forced to pass through the
events ``(t0, t0+h0, t0 + 2*h0, ..., t0 + (2*order+1)*h0)``.
The result is a vector of time points and states, with at least ``(2*order+1)``.
Potentially, the adaptive steps selected many more steps, but because of the events, fewer steps cannot have happened.
2. A prescribed prior is fitted to the first ``(2*order+1)`` (t, y) pairs of the solution. ``order`` is the order of the prior.
3. The value of the resulting posterior at time ``t=t0`` is an estimate of the state and all its derivatives.
The resulting marginal standard deviations estimate the error. This random variable is returned.
Parameters
----------
f
ODE vector field.
y0
Initial value.
t0
Initial time point.
prior
Prior distribution used for the ODE solver. For instance an integrated Brownian motion prior (``IBM``).
initrv
Initial random variable.
df
Jacobian of the ODE vector field. Optional. If specified, more components of the result will be exact.
h0
Maximum step-size to use for computing the approximate ODE solution. The smaller, the more accurate, but also, the smaller, the less stable.
The best value here depends on the ODE problem, and probably the chosen method. Optional. Default is ``1e-2``.
method
Which solver to use. This is communicated as a string that is compatible with ``scipy.integrate.solve_ivp(..., method=method)``.
Optional. Default is `DOP853`.
Returns
-------
Normal
Estimated (improved) initial random variable. Compatible with the specified prior.
Examples
--------
>>> from dataclasses import astuple
>>> from probnum.randvars import Normal
>>> from probnum.statespace import IBM
>>> from probnum.problems.zoo.diffeq import vanderpol
Compute the initial values of the van-der-Pol problem as follows
>>> f, t0, tmax, y0, df, *_ = astuple(vanderpol())
>>> print(y0)
[2. 0.]
>>> prior = IBM(ordint=3, spatialdim=2)
>>> initrv = Normal(mean=np.zeros(prior.dimension), cov=np.eye(prior.dimension))
>>> improved_initrv = initialize_odefilter_with_rk(f, y0, t0, prior=prior, initrv=initrv, df=df)
>>> print(prior.proj2coord(0) @ improved_initrv.mean)
[2. 0.]
>>> print(np.round(improved_initrv.mean, 1))
[ 2. 0. -2. 58.2 0. -2. 60. -1745.7]
>>> print(np.round(np.log10(improved_initrv.std), 1))
[-13.8 -11.3 -9. -1.5 -13.8 -11.3 -9. -1.5]
"""
y0 = np.asarray(y0)
ode_dim = y0.shape[0] if y0.ndim > 0 else 1
order = prior.ordint
proj_to_y = prior.proj2coord(0)
zeros_shift = np.zeros(ode_dim)
zeros_cov = np.zeros((ode_dim, ode_dim))
measmod = statespace.DiscreteLTIGaussian(
proj_to_y,
zeros_shift,
zeros_cov,
proc_noise_cov_cholesky=zeros_cov,
forward_implementation="sqrt",
backward_implementation="sqrt",
)
# order + 1 would suffice in theory, 2*order + 1 is for good measure
# (the "+1" is a safety factor for order=1)
num_steps = 2 * order + 1
t_eval = np.arange(t0, t0 + (num_steps + 1) * h0, h0)
sol = sci.solve_ivp(
f,
(t0, t0 + (num_steps + 1) * h0),
y0=y0,
atol=1e-12,
rtol=1e-12,
t_eval=t_eval,
method=method,
)
ts = sol.t[:num_steps]
ys = sol.y[:, :num_steps].T
initmean = initrv.mean.copy()
initmean[0::(order + 1)] = y0
initmean[1::(order + 1)] = f(t0, y0)
initcov_diag = np.diag(initrv.cov).copy()
initcov_diag[0::(order + 1)] = SMALL_VALUE
initcov_diag[1::(order + 1)] = SMALL_VALUE
if df is not None:
if order > 1:
initmean[2::(order + 1)] = df(t0, y0) @ f(t0, y0)
initcov_diag[2::(order + 1)] = SMALL_VALUE
initcov = np.diag(initcov_diag)
initcov_cholesky = np.diag(np.sqrt(initcov_diag))
initrv = randvars.Normal(initmean, initcov, cov_cholesky=initcov_cholesky)
kalman = filtsmooth.Kalman(prior, measmod, initrv)
regression_problem = problems.RegressionProblem(observations=ys,
locations=ts)
out, _ = kalman.filtsmooth(regression_problem)
estimated_initrv = out.states[0]
return estimated_initrv
def kalman(problem):
"""Standard Kalman filter."""
dynmod, measmod, initrv, *_ = problem
return pnfs.Kalman(dynmod, measmod, initrv)
def test_filtsmooth_pendulum(self):
# pylint: disable=not-callable
# Set up test problem
regression_problem, statespace_components = filtsmooth_zoo.pendulum()
# Linearise problem
ekf_meas = self.linearising_component_pendulum(
statespace_components["measurement_model"])
ekf_dyna = self.linearising_component_pendulum(
statespace_components["dynamics_model"])
method = filtsmooth.Kalman(ekf_dyna, ekf_meas,
statespace_components["initrv"])
# Compute filter/smoother solution
posterior, _ = method.filtsmooth(regression_problem)
filtms = posterior.filtering_posterior.states.mean
smooms = posterior.states.mean
# Compute RMSEs
comp = regression_problem.solution[:, 0]
normaliser = np.sqrt(comp.size)
filtrmse = np.linalg.norm(filtms[:, 0] - comp) / normaliser
smoormse = np.linalg.norm(smooms[:, 0] - comp) / normaliser
obs_rmse = (
np.linalg.norm(regression_problem.observations[:, 0] - comp) /
normaliser)
# If desired, visualise.
if VISUALISE:
obs, tms, states = (
regression_problem.observations,
regression_problem.locations,
regression_problem.solution,
)
fig, (ax1, ax2) = plt.subplots(1, 2)
fig.suptitle("Noisy pendulum model (%.2f " % smoormse +
"< %.2f < %.2f?)" % (filtrmse, obs_rmse))
ax1.set_title("Horizontal position")
ax1.plot(tms[1:], obs[:, 0], ".", alpha=0.25, label="Observations")
ax1.plot(
tms[1:],
np.sin(states)[1:, 0],
"-",
linewidth=4,
alpha=0.5,
label="Truth",
)
ax1.plot(tms[1:], np.sin(filtms)[1:, 0], "-", label="Filter")
ax1.plot(tms[1:], np.sin(smooms)[1:, 0], "-", label="Smoother")
ax1.set_xlabel("time")
ax1.set_ylabel("horizontal pos. = sin(angular)")
ax1.legend()
ax2.set_title("Angular position")
ax2.plot(
tms[1:],
states[1:, 0],
"-",
linewidth=4,
alpha=0.5,
label="Truth",
)
ax2.plot(tms[1:], filtms[1:, 0], "-", label="Filter")
ax2.plot(tms[1:], smooms[1:, 0], "-", label="Smoother")
ax2.set_xlabel("time")
ax2.set_ylabel("angular pos.")
ax2.legend()
plt.show()
# Test if RMSEs behave well.
self.assertLess(smoormse, filtrmse)
self.assertLess(filtrmse, obs_rmse)
def test_filtsmooth_pendulum(self):
# pylint: disable=not-callable
# Set up test problem
dynamod, measmod, initrv, info = pendulum()
delta_t = info["dt"]
tmax = info["tmax"]
tms = np.arange(0, tmax, delta_t)
states, obs = pnss.generate_samples(dynamod, measmod, initrv, tms)
# Linearise problem
ekf_meas = self.linearising_component_pendulum(measmod)
ekf_dyna = self.linearising_component_pendulum(dynamod)
method = pnfs.Kalman(ekf_dyna, ekf_meas, initrv)
# Compute filter/smoother solution
posterior = method.filtsmooth(obs, tms)
filtms = posterior.filtering_posterior.state_rvs.mean
smooms = posterior.state_rvs.mean
# Compute RMSEs
comp = states[:, 0]
normaliser = np.sqrt(comp.size)
filtrmse = np.linalg.norm(filtms[:, 0] - comp) / normaliser
smoormse = np.linalg.norm(smooms[:, 0] - comp) / normaliser
obs_rmse = np.linalg.norm(obs[:, 0] - comp) / normaliser
# If desired, visualise.
if VISUALISE:
fig, (ax1, ax2) = plt.subplots(1, 2)
fig.suptitle("Noisy pendulum model (%.2f " % smoormse +
"< %.2f < %.2f?)" % (filtrmse, obs_rmse))
ax1.set_title("Horizontal position")
ax1.plot(tms[1:], obs[:, 0], ".", alpha=0.25, label="Observations")
ax1.plot(
tms[1:],
np.sin(states)[1:, 0],
"-",
linewidth=4,
alpha=0.5,
label="Truth",
)
ax1.plot(tms[1:], np.sin(filtms)[1:, 0], "-", label="Filter")
ax1.plot(tms[1:], np.sin(smooms)[1:, 0], "-", label="Smoother")
ax1.set_xlabel("time")
ax1.set_ylabel("horizontal pos. = sin(angular)")
ax1.legend()
ax2.set_title("Angular position")
ax2.plot(
tms[1:],
states[1:, 0],
"-",
linewidth=4,
alpha=0.5,
label="Truth",
)
ax2.plot(tms[1:], filtms[1:, 0], "-", label="Filter")
ax2.plot(tms[1:], smooms[1:, 0], "-", label="Smoother")
ax2.set_xlabel("time")
ax2.set_ylabel("angular pos.")
ax2.legend()
plt.show()
# Test if RMSEs behave well.
self.assertLess(smoormse, filtrmse)
self.assertLess(filtrmse, obs_rmse)
def test_filtsmooth_pendulum(self):
# pylint: disable=not-callable
# Set up test problem
# If this measurement variance is not really small, the sampled
# test data can contain an outlier every now and then which
# breaks the test, even though it has not been touched.
regression_problem, statespace_components = filtsmooth_zoo.pendulum(
measurement_variance=0.0001)
# Linearise problem
ekf_meas = self.linearising_component_pendulum(
statespace_components["measurement_model"])
ekf_dyna = self.linearising_component_pendulum(
statespace_components["dynamics_model"])
method = filtsmooth.Kalman(ekf_dyna, ekf_meas,
statespace_components["initrv"])
# Compute filter/smoother solution
posterior, _ = method.filtsmooth(regression_problem)
filtms = posterior.filtering_posterior.states.mean
smooms = posterior.states.mean
# Compute RMSEs
comp = regression_problem.solution[:, 0]
normaliser = np.sqrt(comp.size)
filtrmse = np.linalg.norm(filtms[:, 0] - comp) / normaliser
smoormse = np.linalg.norm(smooms[:, 0] - comp) / normaliser
obs_rmse = (
np.linalg.norm(regression_problem.observations[:, 0] - comp) /
normaliser)
# If desired, visualise.
if self.visualise:
# pylint: disable=import-outside-toplevel
try:
import matplotlib.pyplot as plt
except ImportError as err:
raise ImportError(
"Install matplotlib to visualise the test functions."
) from err
obs, tms, states = (
regression_problem.observations,
regression_problem.locations,
regression_problem.solution,
)
fig, (ax1, ax2) = plt.subplots(1, 2)
fig.suptitle("Noisy pendulum model (%.2f " % smoormse +
"< %.2f < %.2f?)" % (filtrmse, obs_rmse))
ax1.set_title("Horizontal position")
ax1.plot(tms[0:], obs[:, 0], ".", alpha=0.25, label="Observations")
ax1.plot(
tms[1:],
np.sin(states)[1:, 0],
"-",
linewidth=4,
alpha=0.5,
label="Truth",
)
ax1.plot(tms[1:], np.sin(filtms)[1:, 0], "-", label="Filter")
ax1.plot(tms[1:], np.sin(smooms)[1:, 0], "-", label="Smoother")
ax1.set_xlabel("time")
ax1.set_ylabel("horizontal pos. = sin(angular)")
ax1.legend()
ax2.set_title("Angular position")
ax2.plot(
tms[1:],
states[1:, 0],
"-",
linewidth=4,
alpha=0.5,
label="Truth",
)
ax2.plot(tms[1:], filtms[1:, 0], "-", label="Filter")
ax2.plot(tms[1:], smooms[1:, 0], "-", label="Smoother")
ax2.set_xlabel("time")
ax2.set_ylabel("angular pos.")
ax2.legend()
plt.show()
# Test if RMSEs behave well.
self.assertLess(smoormse, filtrmse)
self.assertLess(filtrmse, obs_rmse)
def ivp2ekf0(ivp, prior, evlvar):
"""Computes measurement model and initial distribution for KF based on IVP and
prior.
**Initialdistribution:**
Conditions the initial distribution of the Gaussian filter
onto the initial values.
- If preconditioning is set to ``False``, it conditions
the initial distribution :math:`\\mathcal{N}(0, I)`
on the initial values :math:`(x_0, f(t_0, x_0), ...)` using
as many available deri vatives as possible.
- If preconditioning is set to ``True``, it conditions
the initial distribution :math:`\\mathcal{N}(0, P P^\\top)`
on the initial values :math:`(x_0, f(t_0, x_0), ...)` using
as many available derivatives as possible.
Note that the projection matrices :math:`H_0` and :math:`H_1`
become :math:`H_0 P^{-1}` and :math:`H_1 P^{-1}` which has
to be taken into account during the preconditioning.
**Measurement model:**
Returns a measurement model :math:`\\mathcal{N}(g(m), R)`
involving computing the discrepancy
.. math:: g(m) = H_1 m(t) - f(t, H_0 m(t)).
Then it returns either type of Gaussian filter, each with a
different interpretation of the Jacobian :math:`J_g`:
- EKF0 thinks :math:`J_g(m) = H_1`
- EKF1 thinks :math:`J_g(m) = H_1 - J_f(t, H_0 m(t)) H_0^\\top`
- UKF thinks: ''What is a Jacobian?''
Note that, again, in the case of a preconditioned state space
model, :math:`H_0` and :math:`H_1`
become :math:`H_0 P^{-1}` and :math:`H_1 P^{-1}` which has
to be taken into account. In this case,
- EKF0 thinks :math:`J_g(m) = H_1 P^{-1}`
- EKF1 thinks :math:`J_g(m) = H_1 P^{-1} - J_f(t, H_0 P^{-1} m(t)) (H_0 P^{-1})^\\top`
- UKF again thinks: ''What is a Jacobian?''
**Note:**
The choice between :math:`H_i` and :math:`H_i P^{-1}` is taken care
of within the Prior.
Returns ExtendedKalmanFilter object that is compatible with
the GaussianIVPFilter.
evlvar : float,
measurement variance; in the literature, this is "R"
""" # pylint: disable=line-too-long
ekf_mod = pnfs.DiscreteEKFComponent.from_ode(
ivp,
prior,
evlvar,
ek0_or_ek1=0,
forward_implementation="sqrt",
backward_implementation="sqrt",
)
initrv = _initialdistribution(ivp, prior)
return pnfs.Kalman(prior, ekf_mod, initrv)
