def setup_cartracking(self):
self.dynmod, self.measmod, self.initrv, info = car_tracking()
self.delta_t = info["dt"]
self.tms = np.arange(0, 20, self.delta_t)
self.states, self.obs = pnss.generate_samples(self.dynmod,
self.measmod,
self.initrv, self.tms)
def data(problem):
"""Create artificial data."""
dynmod, measmod, initrv, info = problem
times = np.arange(0, info["tmax"], info["dt"])
states, obs = pnss.generate_samples(dynmod=dynmod,
measmod=measmod,
initrv=initrv,
times=times)
return obs, times, states
def setup_ornsteinuhlenbeck(self):
self.dynmod, self.measmod, self.initrv, info = ornstein_uhlenbeck()
self.delta_t = info["dt"]
self.tmax = info["tmax"]
self.tms = np.arange(0, self.tmax, self.delta_t)
self.states, self.obs = pnss.generate_samples(dynmod=self.dynmod,
measmod=self.measmod,
initrv=self.initrv,
times=self.tms)
def test_generate_shapes(times, test_ndim):
"""Output shapes are as expected."""
mocktrans = MockTransition(dim=test_ndim)
initrv = pnrv.Constant(np.random.rand(test_ndim))
states, obs = pnss.generate_samples(mocktrans, mocktrans, initrv, times)
assert states.shape[0] == len(times)
assert states.shape[1] == test_ndim
assert obs.shape[0] == len(times)
assert obs.shape[1] == test_ndim
def problem():
"""Car-tracking problem."""
problem = car_tracking()
dynmod, measmod, initrv, info = problem
times = np.arange(0, info["tmax"], info["dt"])
states, obs = pnss.generate_samples(dynmod=dynmod,
measmod=measmod,
initrv=initrv,
times=times)
return dynmod, measmod, initrv, info, obs, times, states
def pendulum_problem():
"""Pendulum problem."""
problem = pendulum()
dynmod, measmod, initrv, info = problem
dynmod = pnfs.DiscreteEKFComponent(dynmod)
measmod = pnfs.DiscreteEKFComponent(measmod)
times = np.arange(0, info["tmax"], info["dt"])
states, obs = pnss.generate_samples(
dynmod=dynmod, measmod=measmod, initrv=initrv, times=times
)
return dynmod, measmod, initrv, info, obs, times, states
def benes_daum(
measurement_variance: FloatArgType = 0.1,
process_diffusion: FloatArgType = 1.0,
time_grid: Optional[np.ndarray] = None,
initrv: Optional[randvars.RandomVariable] = None,
):
r"""Filtering/smoothing setup based on the Beneš SDE.
A non-linear state space model for the dynamics of a Beneš SDE.
Here, we formulate a continuous-discrete state space model:
.. math::
d x(t) &= \tanh(x(t)) d t + L d w(t) \\
y_n &= x(t_n) + r_n
for a driving Wiener process :math:`w(t)` and Gaussian distributed measurement noise
:math:`r_n \sim \mathcal{N}(0, R)` with measurement noise
covariance matrix :math:`R`.
Parameters
----------
measurement_variance
Marginal measurement variance.
process_diffusion
Diffusion constant for the dynamics
time_grid
Time grid for the filtering/smoothing problem.
initrv
Initial random variable.
Returns
-------
regression_problem
``RegressionProblem`` object with time points and noisy observations.
statespace_components
Dictionary containing
* dynamics model
* measurement model
* initial random variable
Notes
-----
In order to generate observations for the returned ``RegressionProblem`` object,
the non-linear Beneš SDE has to be linearized.
Here, a ``ContinuousEKFComponent`` is used, which corresponds to a first-order
linearization as used in the extended Kalman filter.
"""
def f(t, x):
return np.tanh(x)
def df(t, x):
return 1.0 - np.tanh(x) ** 2
def l(t):
return process_diffusion * np.ones(1)
if initrv is None:
initrv = randvars.Normal(np.zeros(1), 3.0 * np.eye(1))
dynamics_model = statespace.SDE(dimension=1, driftfun=f, dispmatfun=l, jacobfun=df)
measurement_model = statespace.DiscreteLTIGaussian(
state_trans_mat=np.eye(1),
shift_vec=np.zeros(1),
proc_noise_cov_mat=measurement_variance * np.eye(1),
)
# Generate data
if time_grid is None:
time_grid = np.arange(0.0, 4.0, step=0.2)
# The non-linear dynamics are linearized according to an EKF in order
# to generate samples.
linearized_dynamics_model = filtsmooth.ContinuousEKFComponent(
non_linear_model=dynamics_model
)
states, obs = statespace.generate_samples(
dynmod=linearized_dynamics_model,
measmod=measurement_model,
initrv=initrv,
times=time_grid,
)
regression_problem = problems.RegressionProblem(
observations=obs, locations=time_grid, solution=states
)
statespace_components = dict(
dynamics_model=dynamics_model,
measurement_model=measurement_model,
initrv=initrv,
)
return regression_problem, statespace_components
def pendulum(
measurement_variance: FloatArgType = 0.1024,
timespan: Tuple[FloatArgType, FloatArgType] = (0.0, 4.0),
step: FloatArgType = 0.0075,
initrv: Optional[randvars.RandomVariable] = None,
):
r"""Filtering/smoothing setup for a (noisy) pendulum.
A non-linear, discretized state space model for a pendulum with unknown forces
acting on the dynamics, modeled as Gaussian noise.
See e.g. Särkkä, 2013 [1]_ for more details.
.. math::
\begin{pmatrix}
x_1(t_n) \\
x_2(t_n)
\end{pmatrix}
&=
\begin{pmatrix}
x_1(t_{n-1}) + x_2(t_{n-1}) \cdot h \\
x_2(t_{n-1}) - g \sin(x_1(t_{n-1})) \cdot h
\end{pmatrix}
+
q_n \\
y_n &\sim \sin(x_1(t_n)) + r_n
for some ``step`` size :math:`h` and Gaussian process noise
:math:`q_n \sim \mathcal{N}(0, Q)` with
.. math::
Q =
\begin{pmatrix}
\frac{h^3}{3} & \frac{h^2}{2} \\
\frac{h^2}{2} & h
\end{pmatrix}
:math:`g` denotes the gravitational constant and :math:`r_n \sim \mathcal{N}(0, R)`
is Gaussian mesurement noise with some covariance :math:`R`.
Parameters
----------
measurement_variance
Marginal measurement variance.
timespan
:math:`t_0` and :math:`t_{\max}` of the time grid.
step
Step size of the time grid.
initrv
Initial random variable.
Returns
-------
regression_problem
``RegressionProblem`` object with time points and noisy observations.
statespace_components
Dictionary containing
* dynamics model
* measurement model
* initial random variable
References
----------
.. [1] Särkkä, Simo. Bayesian Filtering and Smoothing. Cambridge University Press,
2013.
"""
# Graviational constant
g = 9.81
# Define non-linear dynamics and measurements
def f(t, x):
x1, x2 = x
y1 = x1 + x2 * step
y2 = x2 - g * np.sin(x1) * step
return np.array([y1, y2])
def df(t, x):
x1, x2 = x
y1 = [1, step]
y2 = [-g * np.cos(x1) * step, 1]
return np.array([y1, y2])
def h(t, x):
x1, x2 = x
return np.array([np.sin(x1)])
def dh(t, x):
x1, x2 = x
return np.array([[np.cos(x1), 0.0]])
process_noise_cov = (
np.diag(np.array([step ** 3 / 3, step]))
+ np.diag(np.array([step ** 2 / 2]), 1)
+ np.diag(np.array([step ** 2 / 2]), -1)
)
dynamics_model = statespace.DiscreteGaussian(
input_dim=2,
output_dim=2,
state_trans_fun=f,
proc_noise_cov_mat_fun=lambda t: process_noise_cov,
jacob_state_trans_fun=df,
)
measurement_model = statespace.DiscreteGaussian(
input_dim=2,
output_dim=1,
state_trans_fun=h,
proc_noise_cov_mat_fun=lambda t: measurement_variance * np.eye(1),
jacob_state_trans_fun=dh,
)
if initrv is None:
initrv = randvars.Normal(np.ones(2), measurement_variance * np.eye(2))
# Generate data
time_grid = np.arange(*timespan, step=step)
states, obs = statespace.generate_samples(
dynmod=dynamics_model, measmod=measurement_model, initrv=initrv, times=time_grid
)
regression_problem = problems.RegressionProblem(
observations=obs, locations=time_grid, solution=states
)
statespace_components = dict(
dynamics_model=dynamics_model,
measurement_model=measurement_model,
initrv=initrv,
)
return regression_problem, statespace_components
def car_tracking(
measurement_variance: FloatArgType = 0.5,
process_diffusion: FloatArgType = 1.0,
model_ordint: IntArgType = 1,
timespan: Tuple[FloatArgType, FloatArgType] = (0.0, 20.0),
step: FloatArgType = 0.2,
initrv: Optional[randvars.RandomVariable] = None,
forward_implementation: str = "classic",
backward_implementation: str = "classic",
):
r"""Filtering/smoothing setup for a simple car-tracking scenario.
A discrete, linear, time-invariant Gaussian state space model for car-tracking,
based on Example 3.6 in Särkkä, 2013. [1]_
Let :math:`X = (\dot{x}_1, \dot{x}_2, \ddot{x}_1, \ddot{x}_2)`. Then the state
space model has the following discretized formulation
.. math::
X(t_{n}) &=
\begin{pmatrix}
1 & 0 & \Delta t& 0 \\
0 & 1 & 0 & \Delta t \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{pmatrix} X(t_{n-1})
+
q_n \\
y_{n} &=
\begin{pmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 \\
\end{pmatrix} X(t_{n})
+ r_n
where :math:`q_n \sim \mathcal{N}(0, Q)` and :math:`r_n \sim \mathcal{N}(0, R)`
for process noise covariance matrix :math:`Q` and measurement noise covariance
matrix :math:`R`.
Parameters
----------
measurement_variance
Marginal measurement variance.
process_diffusion
Diffusion constant for the dynamics.
model_ordint
Order of integration for the dynamics model. Defaults to one, which corresponds
to a Wiener velocity model.
timespan
:math:`t_0` and :math:`t_{\max}` of the time grid.
step
Step size of the time grid.
initrv
Initial random variable.
Returns
-------
regression_problem
``RegressionProblem`` object with time points and noisy observations.
statespace_components
Dictionary containing
* dynamics model
* measurement model
* initial random variable
References
----------
.. [1] Särkkä, Simo. Bayesian Filtering and Smoothing. Cambridge University Press,
2013.
"""
state_dim = 2
model_dim = state_dim * (model_ordint + 1)
measurement_dim = 2
dynamics_model = statespace.IBM(
ordint=model_ordint,
spatialdim=state_dim,
forward_implementation=forward_implementation,
backward_implementation=backward_implementation,
)
dynamics_model.dispmat *= process_diffusion
discrete_dynamics_model = dynamics_model.discretise(dt=step)
measurement_matrix = np.eye(measurement_dim, model_dim)
measurement_cov = measurement_variance * np.eye(measurement_dim)
measurement_cov_cholesky = np.sqrt(measurement_variance) * np.eye(measurement_dim)
measurement_model = statespace.DiscreteLTIGaussian(
state_trans_mat=measurement_matrix,
shift_vec=np.zeros(measurement_dim),
proc_noise_cov_mat=measurement_cov,
proc_noise_cov_cholesky=measurement_cov_cholesky,
forward_implementation=forward_implementation,
backward_implementation=backward_implementation,
)
if initrv is None:
initrv = randvars.Normal(
np.zeros(model_dim),
measurement_variance * np.eye(model_dim),
cov_cholesky=np.sqrt(measurement_variance) * np.eye(model_dim),
)
# Set up regression problem
time_grid = np.arange(*timespan, step=step)
states, obs = statespace.generate_samples(
dynmod=discrete_dynamics_model,
measmod=measurement_model,
initrv=initrv,
times=time_grid,
)
regression_problem = problems.RegressionProblem(
observations=obs, locations=time_grid, solution=states
)
statespace_components = dict(
dynamics_model=discrete_dynamics_model,
measurement_model=measurement_model,
initrv=initrv,
)
return regression_problem, statespace_components
def ornstein_uhlenbeck(
measurement_variance: FloatArgType = 0.1,
driftspeed: FloatArgType = 0.21,
process_diffusion: FloatArgType = 0.5,
time_grid: Optional[np.ndarray] = None,
initrv: Optional[randvars.RandomVariable] = None,
forward_implementation: str = "classic",
backward_implementation: str = "classic",
):
r"""Filtering/smoothing setup based on an Ornstein Uhlenbeck process.
A linear, time-invariant state space model for the dynamics of a time-invariant
Ornstein-Uhlenbeck process. See e.g. Example 10.19 in Särkkä et. al, 2019. [1]_
Here, we formulate a continuous-discrete state space model:
.. math::
d x(t) &= \lambda x(t) d t + L d w(t) \\
y_n &= x(t_n) + r_n
for a drift constant :math:`\lambda` and a driving Wiener process :math:`w(t)`.
:math:`r_n \sim \mathcal{N}(0, R)` is Gaussian distributed measurement noise
with covariance matrix :math:`R`.
Note that the linear, time-invariant dynamics have an equivalent discretization.
Parameters
----------
measurement_variance
Marginal measurement variance.
driftspeed
Drift parameter of the Ornstein-Uhlenbeck process.
process_diffusion
Diffusion constant for the dynamics
time_grid
Time grid for the filtering/smoothing problem.
initrv
Initial random variable.
Returns
-------
regression_problem
``RegressionProblem`` object with time points and noisy observations.
statespace_components
Dictionary containing
* dynamics model
* measurement model
* initial random variable
References
----------
.. [1] Särkkä, Simo, and Solin, Arno. Applied Stochastic Differential Equations.
Cambridge University Press, 2019
"""
dynamics_model = statespace.IOUP(
ordint=0,
spatialdim=1,
driftspeed=driftspeed,
forward_implementation=forward_implementation,
backward_implementation=backward_implementation,
)
dynamics_model.dispmat *= process_diffusion
measurement_model = statespace.DiscreteLTIGaussian(
state_trans_mat=np.eye(1),
shift_vec=np.zeros(1),
proc_noise_cov_mat=measurement_variance * np.eye(1),
forward_implementation=forward_implementation,
backward_implementation=backward_implementation,
)
if initrv is None:
initrv = randvars.Normal(10.0 * np.ones(1), np.eye(1))
# Set up regression problem
if time_grid is None:
time_grid = np.arange(0.0, 20.0, step=0.2)
states, obs = statespace.generate_samples(
dynmod=dynamics_model, measmod=measurement_model, initrv=initrv, times=time_grid
)
regression_problem = problems.RegressionProblem(
observations=obs, locations=time_grid, solution=states
)
statespace_components = dict(
dynamics_model=dynamics_model,
measurement_model=measurement_model,
initrv=initrv,
)
return regression_problem, statespace_components
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)
