def test_unscented_kalman(self):
"""
Test Unscented KF on range of SSMs.
"""
for ssm_name, data in self.ssm.items():
print('Testing: {} ...'.format(ssm_name.upper()), end=' ')
try:
alg = UnscentedKalman(data['dyn'], data['obs'])
alg.forward_pass(data['y'][..., 0])
alg.backward_pass()
alg.reset()
except BaseException as e:
print('Failed: {}'.format(e))
continue
print('OK')
def reentry_gpq_demo():
mc_sims = 20
disc_tau = 0.5 # discretization period
# ground-truth data generator (true system)
m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 0])
x0 = GaussRV(5, m0, P0)
Q = np.diag([2.4064e-5, 2.4064e-5, 0])
q = GaussRV(3, cov=Q)
sys = ReentryVehicle2DTransition(x0, q, dt=disc_tau)
# radar measurement model
r = GaussRV(2, cov=np.diag([1e-6, 0.17e-6]))
radar_x, radar_y = sys.R0, 0
obs = Radar2DMeasurement(r, 5, radar_loc=np.array([radar_x, radar_y]))
# Generate reference trajectory by Euler-Maruyama integration
x = sys.simulate_continuous(duration=200, dt=disc_tau, mc_sims=mc_sims)
x_ref = x.mean(axis=2)
# generate radar measurements
y = obs.simulate_measurements(x)
# setup SSM for the filter
m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
x0 = GaussRV(5, m0, P0)
q = GaussRV(3, cov=disc_tau*Q)
dyn = ReentryVehicle2DTransition(x0, q, dt=disc_tau)
# Initialize filters
hdyn = np.array([[1.0, 25, 25, 25, 25, 25]])
hobs = np.array([[1.0, 25, 25, 1e4, 1e4, 1e4]])
alg = (
GaussianProcessKalman(dyn, obs, hdyn, hobs, kernel='rbf', points='ut'),
UnscentedKalman(dyn, obs),
)
# Are both filters using the same sigma-points?
# assert np.array_equal(alg[0].tf_dyn.model.points, alg[1].tf_dyn.unit_sp)
num_alg = len(alg)
d, steps, mc_sims = x.shape
mean, cov = np.zeros((d, steps, mc_sims, num_alg)), np.zeros((d, d, steps, mc_sims, num_alg))
for imc in range(mc_sims):
for ia, a in enumerate(alg):
mean[..., imc, ia], cov[..., imc, ia] = a.forward_pass(y[..., imc])
a.reset()
# Plots
plt.figure()
g = GridSpec(2, 4)
plt.subplot(g[:, :2])
# Earth surface w/ radar position
t = 0.02 * np.arange(-1, 4, 0.1)
plt.plot(sys.R0 * np.cos(t), sys.R0 * np.sin(t), color='darkblue', lw=2)
plt.plot(radar_x, radar_y, 'ko')
plt.plot(x_ref[0, :], x_ref[1, :], color='r', ls='--')
# Convert from polar to cartesian
meas = np.stack((radar_x + y[0, ...] * np.cos(y[1, ...]), radar_y + y[0, ...] * np.sin(y[1, ...])), axis=0)
for i in range(mc_sims):
# Vehicle trajectory
# plt.plot(x[0, :, i], x[1, :, i], alpha=0.35, color='r', ls='--')
# Plot measurements
plt.plot(meas[0, :, i], meas[1, :, i], 'k.', alpha=0.3)
# Filtered position estimate
plt.plot(mean[0, 1:, i, 0], mean[1, 1:, i, 0], color='g', alpha=0.3)
plt.plot(mean[0, 1:, i, 1], mean[1, 1:, i, 1], color='orange', alpha=0.3)
# Performance score plots
error2 = mean.copy()
lcr = np.zeros((steps, mc_sims, num_alg))
for a in range(num_alg):
for k in range(steps):
mse = mse_matrix(x[:4, k, :], mean[:4, k, :, a])
for imc in range(mc_sims):
error2[:, k, imc, a] = squared_error(x[:, k, imc], mean[:, k, imc, a])
lcr[k, imc, a] = log_cred_ratio(x[:4, k, imc], mean[:4, k, imc, a], cov[:4, :4, k, imc, a], mse)
# Averaged RMSE and Inclination Indicator in time
pos_rmse_vs_time = np.sqrt((error2[:2, ...]).sum(axis=0)).mean(axis=1)
inc_ind_vs_time = lcr.mean(axis=1)
# Plots
plt.subplot(g[0, 2:])
plt.title('RMSE')
plt.plot(pos_rmse_vs_time[:, 0], label='GPQKF', color='g')
plt.plot(pos_rmse_vs_time[:, 1], label='UKF', color='r')
plt.legend()
plt.subplot(g[1, 2:])
plt.title('Inclination Indicator $I^2$')
plt.plot(inc_ind_vs_time[:, 0], label='GPQKF', color='g')
plt.plot(inc_ind_vs_time[:, 1], label='UKF', color='r')
plt.legend()
plt.show()
print('Average RMSE: {}'.format(pos_rmse_vs_time.mean(axis=0)))
print('Average I2: {}'.format(inc_ind_vs_time.mean(axis=0)))
def ungm_demo(steps=250, mc_sims=100):
# SYSTEM (data generator): dynamics and measurement
x0_cov = 1.0
q_cov_0, q_cov_1 = 10.0, 100.0
r_cov_0, r_cov_1 = 0.01, 1.0
x0 = GaussRV(1, cov=x0_cov)
zero_means = (np.zeros((1, )), np.zeros((1, )))
gm_weights = np.array([0.8, 0.2])
q_covs = (np.atleast_2d(q_cov_0), np.atleast_2d(q_cov_1))
q = GaussianMixtureRV(1, zero_means, q_covs, gm_weights)
dyn = UNGMTransition(x0, q)
r_covs = (np.atleast_2d(r_cov_0), np.atleast_2d(r_cov_1))
r = GaussianMixtureRV(1, zero_means, r_covs, gm_weights)
obs = UNGMMeasurement(r, dyn.dim_state)
# simulate data
x = dyn.simulate_discrete(steps, mc_sims)
z = obs.simulate_measurements(x)
# STUDENT STATE SPACE MODEL: dynamics and measurement
nu = 4.0
x0 = StudentRV(1, scale=(nu - 2) / nu * x0_cov, dof=nu)
q = StudentRV(1, scale=((nu - 2) / nu) * q_cov_0, dof=nu)
dyn = UNGMTransition(x0, q)
r = StudentRV(1, scale=((nu - 2) / nu) * r_cov_0, dof=nu)
obs = UNGMMeasurement(r, dyn.dim_state)
# GAUSSIAN SSM for UKF
x0 = GaussRV(1, cov=x0_cov)
q = GaussRV(1, cov=q_cov_0)
dyn_gauss = UNGMTransition(x0, q)
r = GaussRV(1, cov=r_cov_0)
obs_gauss = UNGMMeasurement(r, dyn.dim_state)
# kernel parameters for TPQ and GPQ filters
# TPQ Student
# par_dyn_tp = np.array([[1.8, 3.0]])
# par_obs_tp = np.array([[0.4, 1.0, 1.0]])
par_dyn_tp = np.array([[3.0, 1.0]])
par_obs_tp = np.array([[3.0, 3.0]])
# GPQ Student
par_dyn_gpqs = par_dyn_tp
par_obs_gpqs = par_obs_tp
# GPQ Kalman
par_dyn_gpqk = np.array([[1.0, 0.5]])
par_obs_gpqk = np.array([[1.0, 1, 10]])
# parameters of the point-set
kappa = 0.0
par_pt = {'kappa': kappa}
# FIXME: TPQ filters give too similar results, unlike in the paper, likely because I fiddled with DoF choice in TPQ
# init filters
filters = (
# ExtendedStudent(dyn, obs),
UnscentedKalman(dyn_gauss, obs_gauss, kappa=kappa),
# FullySymmetricStudent(dyn, obs, kappa=kappa, dof=3.0),
FullySymmetricStudent(dyn, obs, kappa=kappa, dof=4.0),
# FullySymmetricStudent(dyn, obs, kappa=kappa, dof=8.0),
# FullySymmetricStudent(dyn, obs, kappa=kappa, dof=100.0),
StudentProcessStudent(dyn,
obs,
par_dyn_tp,
par_obs_tp,
dof=4.0,
dof_tp=3.0,
point_par=par_pt),
# StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=3.0, dof_tp=4.0, point_par=par_pt),
StudentProcessStudent(dyn,
obs,
par_dyn_tp,
par_obs_tp,
dof=4.0,
dof_tp=10.0,
point_par=par_pt),
# StudentProcessStudent(dyn, obs, par_dyn_tp, par_obs_tp, dof=4.0, dof_tp=100.0, point_par=par_pt),
StudentProcessStudent(dyn,
obs,
par_dyn_tp,
par_obs_tp,
dof=4.0,
dof_tp=500.0,
point_par=par_pt),
# GaussianProcessStudent(dyn, obs, par_dyn_gpqs, par_obs_gpqs, dof=10.0, point_hyp=par_pt),
# StudentProcessKalman(dyn, obs, par_dyn_gpqk, par_obs_gpqk, points='fs', point_hyp=par_pt),
# GaussianProcessKalman(dyn, obs, par_dyn_tp, par_obs_tp, point_hyp=par_pt),
)
itpq = np.argwhere([
isinstance(filters[i], StudentProcessStudent)
for i in range(len(filters))
]).squeeze(axis=1)[0]
# assign weights approximated by MC with lots of samples
# very dirty code
pts = filters[itpq].tf_dyn.model.points
kern = filters[itpq].tf_dyn.model.kernel
wm, wc, wcc, Q = rbf_student_mc_weights(pts, kern, int(1e6), 1000)
for f in filters:
if isinstance(f.tf_dyn, BQTransform):
f.tf_dyn.wm, f.tf_dyn.Wc, f.tf_dyn.Wcc = wm, wc, wcc
f.tf_dyn.Q = Q
pts = filters[itpq].tf_obs.model.points
kern = filters[itpq].tf_obs.model.kernel
wm, wc, wcc, Q = rbf_student_mc_weights(pts, kern, int(1e6), 1000)
for f in filters:
if isinstance(f.tf_obs, BQTransform):
f.tf_obs.wm, f.tf_obs.Wc, f.tf_obs.Wcc = wm, wc, wcc
f.tf_obs.Q = Q
# print kernel parameters
import pandas as pd
parlab = ['alpha'] + ['ell_{}'.format(d + 1) for d in range(x.shape[0])]
partable = pd.DataFrame(np.vstack((par_dyn_tp, par_obs_tp)),
columns=parlab,
index=['dyn', 'obs'])
print()
print(partable)
# run all filters
mf, Pf = run_filters(filters, z)
# compute average RMSE and INC from filtered trajectories
rmse_avg, lcr_avg = eval_perf_scores(x, mf, Pf)
# variance of average metrics
from ssmtoybox.utils import bootstrap_var
var_rmse_avg = np.zeros((len(filters), ))
var_lcr_avg = np.zeros((len(filters), ))
for fi in range(len(filters)):
var_rmse_avg[fi] = bootstrap_var(rmse_avg[:, fi], int(1e4))
var_lcr_avg[fi] = bootstrap_var(lcr_avg[:, fi], int(1e4))
# save trajectories, measurements and metrics to file for later processing (tables, plots)
# data_dict = {
# 'x': x,
# 'z': z,
# 'mf': mf,
# 'Pf': Pf,
# 'rmse_avg': rmse_avg,
# 'lcr_avg': lcr_avg,
# 'var_rmse_avg': var_rmse_avg,
# 'var_lcr_avg': var_lcr_avg,
# 'steps': steps,
# 'mc_sims': mc_sims,
# 'par_dyn_tp': par_dyn_tp,
# 'par_obs_tp': par_obs_tp,
# }
# savemat('ungm_simdata_{:d}k_{:d}mc'.format(steps, mc_sims), data_dict)
f_label = [f.__class__.__name__ for f in filters]
m_label = ['MEAN_RMSE', 'STD(MEAN_RMSE)', 'MEAN_INC', 'STD(MEAN_INC)']
data = np.array([
rmse_avg.mean(axis=0),
np.sqrt(var_rmse_avg),
lcr_avg.mean(axis=0),
np.sqrt(var_lcr_avg)
]).T
table = pd.DataFrame(data, f_label, m_label)
pd.set_option('display.max_columns', 6)
print(table)
def reentry_simple_gpq_demo(dur=30, tau=0.1, mc=100):
"""
A spherical object falls down from high altitude entering the Earth’s atmosphere with a high velocity.
Parameters
----------
tau: float
discretization period for the dynamics ODE integration method
dur: int
Duration of the dynamics simulation
mc: int
Number of Monte Carlo simulations.
Notes
-----
The parameter mc determines the number of trajectories simulated.
Returns
-------
"""
# ground-truth data generator (true system)
m0 = np.array([90, 6, 1.5])
P0 = np.diag([0.0929, 1.4865, 1e-4])
x0 = GaussRV(3, m0, P0)
q = GaussRV(3, cov=np.zeros((3, 3)))
sys = ReentryVehicle1DTransition(x0, q, dt=tau)
# Generate reference trajectory
x = sys.simulate_continuous(dur, mc_sims=mc)
# pick only non-divergent trajectories
x = x[..., np.all(x >= 0, axis=(0, 1))]
# range measurement model
r = GaussRV(1, cov=np.array([[0.03048 ** 2]]))
obs = RangeMeasurement(r, 3)
y = obs.simulate_measurements(x)
# state-space model used by the filter
m0 = np.array([90, 6, 1.7])
P0 = np.diag([0.0929, 1.4865, 1e-4])
x0 = GaussRV(3, m0, P0)
q = GaussRV(3, cov=np.zeros((3, 3)))
dyn = ReentryVehicle1DTransition(x0, q, dt=tau)
# GPQKF kernel parameters
kpar_dyn_ut = np.array([[0.5, 10, 10, 10]])
kpar_obs_ut = np.array([[0.5, 15, 20, 20]])
# Initialize filters
alg = (
GaussianProcessKalman(dyn, obs, kpar_dyn_ut, kpar_obs_ut, kernel='rbf', points='ut'),
UnscentedKalman(dyn, obs),
)
num_alg = len(alg)
d, steps, mc = x.shape
mean, cov = np.zeros((d, steps, mc, num_alg)), np.zeros((d, d, steps, mc, num_alg))
for imc in range(mc):
for ia, a in enumerate(alg):
# Do filtering and reset the filters for each new track
mean[..., imc, ia], cov[..., imc, ia] = a.forward_pass(y[..., imc])
a.reset()
# time index for plotting
time_ind = np.linspace(1, dur, x.shape[1])
# PLOTS: Trajectories
# plt.figure()
# g = GridSpec(4, 2)
# plt.subplot(g[:2, :])
#
# # Earth surface w/ radar position
# t = np.arange(0.48 * np.pi, 0.52 * np.pi, 0.01)
# plt.plot(sys.R0 * np.cos(t), sys.R0 * np.sin(t) - sys.R0, 'darkblue', lw=2)
# plt.plot(sys.sx, sys.sy, 'ko')
#
# xzer = np.zeros(x.shape[1])
# for i in range(mc):
# # Vehicle trajectory
# plt.plot(xzer, x[0, :, i], alpha=0.35, color='r', ls='--', lw=2)
#
# # Filtered position estimate
# plt.plot(xzer, mean[0, :, i, 0], color='g', alpha=0.3)
# plt.plot(xzer, mean[0, :, i, 1], color='orange', alpha=0.3)
# Altitude
# x0 = sys.pars['x0_mean']
# plt.subplot(g[2, :])
# plt.ylim([0, x0[0]])
# for i in range(mc):
# plt.plot(time_ind, x[0, :, i], alpha=0.35, color='b')
# plt.ylabel('altitude [ft]')
# plt.xlabel('time [s]')
#
# # Velocity
# plt.subplot(g[3, :])
# plt.ylim([0, x0[1]])
# for i in range(mc):
# plt.plot(time_ind, x[1, :, i], alpha=0.35, color='b')
# plt.ylabel('velocity [ft/s]')
# plt.xlabel('time [s]')
# Compute Performance Scores
error2 = mean.copy()
pos_lcr = np.zeros((steps, mc, num_alg))
vel_lcr = pos_lcr.copy()
theta_lcr = pos_lcr.copy()
for a in range(num_alg):
for k in range(steps):
pos_mse = mse_matrix(x[0, na, k, :], mean[0, na, k, :, a])
vel_mse = mse_matrix(x[1, na, k, :], mean[1, na, k, :, a])
theta_mse = mse_matrix(x[2, na, k, :], mean[2, na, k, :, a])
for imc in range(mc):
error2[:, k, imc, a] = squared_error(x[:, k, imc], mean[:, k, imc, a])
pos_lcr[k, imc, a] = log_cred_ratio(x[0, k, imc], mean[0, k, imc, a],
cov[0, 0, k, imc, a], pos_mse)
vel_lcr[k, imc, a] = log_cred_ratio(x[1, k, imc], mean[1, k, imc, a],
cov[1, 1, k, imc, a], vel_mse)
theta_lcr[k, imc, a] = log_cred_ratio(x[2, k, imc], mean[2, k, imc, a],
cov[2, 2, k, imc, a], theta_mse)
# Averaged position/velocity RMSE and inclination in time
pos_rmse = np.sqrt(error2[0, na, ...].sum(axis=0))
pos_rmse_vs_time = pos_rmse.mean(axis=1)
pos_inc_vs_time = pos_lcr.mean(axis=1)
vel_rmse = np.sqrt(error2[1, na, ...].sum(axis=0))
vel_rmse_vs_time = vel_rmse.mean(axis=1)
vel_inc_vs_time = vel_lcr.mean(axis=1)
theta_rmse = np.sqrt(error2[2, na, ...].sum(axis=0))
theta_rmse_vs_time = theta_rmse.mean(axis=1)
theta_inc_vs_time = theta_lcr.mean(axis=1)
# PLOTS: RMSE in time for position, velocity and ballistic parameter
plt.figure()
g = GridSpec(6, 3)
# filt_labels = ['UT', 'CUT', 'GPQ-UT', 'GPQ-CUT']
filt_labels = ['GPQKF', 'UKF']
plt.subplot(g[:2, :2])
plt.ylabel('RMSE')
for i in range(num_alg):
plt.plot(time_ind[1:], pos_rmse_vs_time[1:, i], label=filt_labels[i])
plt.legend()
plt.subplot(g[2:4, :2])
plt.ylabel('RMSE')
for i in range(num_alg):
plt.plot(time_ind[1:], vel_rmse_vs_time[1:, i], label=filt_labels[i])
plt.subplot(g[4:, :2])
plt.ylabel('RMSE')
plt.xlabel('time step [k]')
for i in range(num_alg):
plt.plot(time_ind[1:], theta_rmse_vs_time[1:, i], label=filt_labels[i])
# BOX PLOTS: time-averaged RMSE
plt.subplot(g[:2, 2:])
plt.boxplot(pos_rmse.mean(axis=0), labels=filt_labels)
plt.subplot(g[2:4, 2:])
plt.boxplot(vel_rmse.mean(axis=0), labels=filt_labels)
plt.subplot(g[4:, 2:])
plt.boxplot(theta_rmse.mean(axis=0), labels=filt_labels)
plt.show()
# PLOTS: Inclination indicator in time for position, velocity and ballistic parameter
plt.figure()
g = GridSpec(6, 3)
plt.subplot(g[:2, :2])
plt.ylabel(r'$\nu$')
for i in range(num_alg):
plt.plot(time_ind, pos_inc_vs_time[:, i], label=filt_labels[i])
plt.subplot(g[2:4, :2])
plt.ylabel(r'$\nu$')
for i in range(num_alg):
plt.plot(time_ind, vel_inc_vs_time[:, i], label=filt_labels[i])
plt.subplot(g[4:, :2])
plt.ylabel(r'$\nu$')
plt.xlabel('time step [k]')
for i in range(num_alg):
plt.plot(time_ind, theta_inc_vs_time[:, i], label=filt_labels[i])
# BOX PLOTS: time-averaged inclination indicator
plt.subplot(g[:2, 2:])
plt.boxplot(pos_lcr.mean(axis=0), labels=filt_labels)
plt.subplot(g[2:4, 2:])
plt.boxplot(vel_lcr.mean(axis=0), labels=filt_labels)
plt.subplot(g[4:, 2:])
plt.boxplot(theta_lcr.mean(axis=0), labels=filt_labels)
plt.show()
# TODO: pandas tables for printing into latex
np.set_printoptions(precision=4)
print('Average RMSE: {}'.format(np.sqrt(error2.sum(axis=0)).mean(axis=(0, 1))))
def reentry_simple_data(dur=30, tau=0.1, mc=100):
# ground-truth data generator (true system)
m0 = np.array([90, 6, 1.5])
P0 = np.diag([0.0929, 1.4865, 1e-4])
x0 = GaussRV(3, m0, P0)
q = GaussRV(3, cov=np.zeros((3, 3)))
sys = ReentryVehicle1DTransition(x0, q, dt=tau)
# Generate reference trajectory
x = sys.simulate_continuous(dur, mc_sims=mc)
# pick only non-divergent trajectories
x = x[..., np.all(x >= 0, axis=(0, 1))]
mc = x.shape[2]
# range measurement model
r = GaussRV(1, cov=np.array([[0.03048**2]]))
obs = RangeMeasurement(r, 3)
y = obs.simulate_measurements(x)
# state-space model used by the filter
m0 = np.array([90, 6, 1.7])
P0 = np.diag([0.0929, 1.4865, 1e-4])
x0 = GaussRV(3, m0, P0)
q = GaussRV(3, cov=np.zeros((3, 3)))
dyn = ReentryVehicle1DTransition(x0, q, dt=tau)
# GPQKF kernel parameters
hdyn = np.array([[0.5, 10, 10, 10]])
hobs = np.array([[0.5, 15, 20, 20]])
# Initialize filters
alg = (
GaussianProcessKalman(dyn, obs, hdyn, hobs, kernel='rbf', points='ut'),
# CubatureKalman(dyn, obs),
UnscentedKalman(dyn, obs),
)
num_alg = len(alg)
d, steps, mc = x.shape
mean, cov = np.zeros((d, steps, mc, num_alg)), np.zeros((d, d, steps, mc, num_alg))
for imc in range(mc):
for ia, a in enumerate(alg):
# Do filtering and reset the filters for each new track
mean[..., imc, ia], cov[..., imc, ia] = a.forward_pass(y[..., imc])
a.reset()
# compute RMSE, Inclination indicator for velocity, position and ballistic parameter
error2 = mean.copy()
lcr = mean.copy()
print("Calculating scores ...")
for a in range(num_alg):
for k in range(steps):
for imc in range(mc):
error2[:, k, imc, a] = squared_error(x[:, k, imc], mean[:, k, imc, a])
for dim in range(d):
mse = mse_matrix(x[dim, na, k, :], mean[dim, na, k, :, a])
lcr[dim, k, imc, a] = log_cred_ratio(x[dim, k, imc], mean[dim, k, imc, a],
cov[dim, dim, k, imc, a], mse)
# Averaged position/velocity RMSE and inclination in time
rmse_vs_time = np.zeros((d, steps, num_alg))
lcr_vs_time = rmse_vs_time.copy()
for dim in range(d):
rmse_vs_time[dim, ...] = np.sqrt(error2[dim, ...]).mean(axis=1)
lcr_vs_time = lcr.mean(axis=2)
# time index for plotting
time = np.linspace(1, dur, x.shape[1])
# Pack the data into dictionary
# data_scores = dict([(name, eval(name)) for name in ['time', 'x', 'mean', 'cov', 'rmse_vs_time', 'lcr_vs_time']])
data_scores = {
'time': time,
'x': x,
'mean': mean,
'cov': cov,
'rmse_vs_time': rmse_vs_time,
'lcr_vs_time': lcr_vs_time
}
return data_scores
def reentry_demo(dur=200, mc_sims=100, outfile=None):
# use default filename if unspecified
if outfile is None:
outfile = 'reentry_demo_results.dat'
outfile = os.path.join('..', outfile)
if not os.path.exists(outfile) or True:
tau = 0.05
disc_tau = 0.1
# initial condition statistics
m0 = np.array([6500, 350, -1.8, -6.8, 0.7])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 0])
init_rv = GaussRV(5, m0, P0)
# process noise statistics
noise_rv = GaussRV(3, cov=np.diag([2.4e-5, 2.4e-5, 0]))
sys = ReentryVehicle2DTransition(init_rv, noise_rv)
# measurement noise statistics
meas_noise_rv = GaussRV(2, cov=np.diag([1e-6, 0.17e-6]))
obs = Radar2DMeasurement(meas_noise_rv,
5,
radar_loc=np.array([6374, 0.0]))
np.random.seed(0)
# Generate reference trajectory by SDE simulation
x = sys.simulate_continuous(duration=dur, dt=tau, mc_sims=mc_sims)
y = obs.simulate_measurements(x)
# subsample data for the filter and performance score calculation
x = x[:, ::2, ...] # take every second point on the trajectory
y = y[:, ::2, ...] # and corresponding measurement
# Initialize state-space model with mis-specified initial mean
# m0 = np.array([6500.4, 349.14, -1.1093, -6.1967, 0.6932])
m0 = np.array([6500, 350, -1.1, -6.1, 0.7])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
init_rv = GaussRV(5, m0, P0)
noise_rv = GaussRV(3, cov=np.diag([2.4e-5, 2.4e-5, 1e-6]))
dyn = ReentryVehicle2DTransition(init_rv, noise_rv, dt=disc_tau)
# NOTE: higher tau causes higher spread of trajectories at the ends
# Initialize filters
par_dyn = np.array([[1.0, 1, 1, 1, 1, 1]])
par_obs = np.array([[1.0, 0.9, 0.9, 1e4, 1e4,
1e4]]) # np.atleast_2d(np.ones(6))
mul_ut = np.hstack((np.zeros((dyn.dim_in, 1)), np.eye(dyn.dim_in),
2 * np.eye(dyn.dim_in))).astype(np.int)
alg = OrderedDict({
# GaussianProcessKalman(ssm, par_dyn, par_obs, kernel='rbf', points='ut'),
'bsqkf':
BayesSardKalman(dyn,
obs,
par_dyn,
par_obs,
mul_ut,
mul_ut,
points='ut'),
'bsqkf_2e-6':
BayesSardKalman(dyn,
obs,
par_dyn,
par_obs,
mul_ut,
mul_ut,
points='ut'),
'bsqkf_2e-7':
BayesSardKalman(dyn,
obs,
par_dyn,
par_obs,
mul_ut,
mul_ut,
points='ut'),
'ukf':
UnscentedKalman(dyn, obs, beta=0.0),
})
alg['bsqkf'].tf_dyn.model.model_var = np.diag(
[0.0002, 0.0002, 0.0002, 0.0002, 0.0002])
alg['bsqkf'].tf_obs.model.model_var = 0 * np.eye(2)
alg['bsqkf_2e-6'].tf_dyn.model.model_var = 2e-6 * np.eye(5)
alg['bsqkf_2e-6'].tf_obs.model.model_var = 0 * np.eye(2)
alg['bsqkf_2e-7'].tf_dyn.model.model_var = 2e-7 * np.eye(5)
alg['bsqkf_2e-7'].tf_obs.model.model_var = 0 * np.eye(2)
# print experiment setup
print('System (reality)')
print('{:10s}: {}'.format('x0_mean', sys.init_rv.mean))
print('{:10s}: {}'.format('x0_cov', sys.init_rv.cov.diagonal()))
print()
print('State-Space Model')
print('{:10s}: {}'.format('x0_mean', dyn.init_rv.mean))
print('{:10s}: {}'.format('x0_cov', dyn.init_rv.cov.diagonal()))
print()
print('BSQ Expected Model Variance')
print('{:10s}: {}'.format(
'dyn_func', alg['bsqkf'].tf_dyn.model.model_var.diagonal()))
print('{:10s}: {}'.format(
'obs_func', alg['bsqkf'].tf_obs.model.model_var.diagonal()))
print()
num_alg = len(alg)
d, steps, mc_sims = x.shape
mean, cov = np.zeros((d, steps, mc_sims, num_alg)), np.zeros(
(d, d, steps, mc_sims, num_alg))
for ia, a in enumerate(alg):
print('Running {:<5} ... '.format(a.upper()), end='', flush=True)
t0 = time.time()
for imc in range(mc_sims):
mean[..., imc, ia], cov[..., imc,
ia] = alg[a].forward_pass(y[..., imc])
alg[a].reset()
print('{:>30}'.format('Done in {:.2f} [sec]'.format(time.time() -
t0)))
# Performance score plots
print()
print('Computing performance scores ...')
error2 = mean.copy()
lcr_x = np.zeros((steps, mc_sims, num_alg))
lcr_pos = lcr_x.copy()
lcr_vel = lcr_x.copy()
lcr_theta = lcr_x.copy()
for a in range(num_alg):
for k in range(steps):
mse = mse_matrix(x[:, k, :], mean[:, k, :, a])
for imc in range(mc_sims):
error2[:, k, imc,
a] = squared_error(x[:, k, imc], mean[:, k, imc, a])
lcr_x[k, imc,
a] = log_cred_ratio(x[:, k, imc], mean[:, k, imc, a],
cov[:, :, k, imc, a], mse)
lcr_pos[k, imc,
a] = log_cred_ratio(x[:2, k, imc], mean[:2, k, imc,
a],
cov[:2, :2, k, imc,
a], mse[:2, :2])
lcr_vel[k, imc,
a] = log_cred_ratio(x[2:4, k, imc], mean[2:4, k,
imc, a],
cov[2:4, 2:4, k, imc,
a], mse[2:4, 2:4])
lcr_theta[k, imc,
a] = log_cred_ratio(x[4, k, imc], mean[4, k, imc,
a],
cov[4, 4, k, imc, a], mse[4,
4])
# Averaged RMSE and Inclination Indicator in time
x_rmse_vs_time = np.sqrt(error2.sum(axis=0)).mean(axis=1)
pos_rmse_vs_time = np.sqrt((error2[:2, ...]).sum(axis=0)).mean(axis=1)
vel_rmse_vs_time = np.sqrt((error2[2:4, ...]).sum(axis=0)).mean(axis=1)
theta_rmse_vs_time = np.sqrt((error2[4, ...])).mean(axis=1)
x_inc_vs_time = lcr_x.mean(axis=1)
pos_inc_vs_time = lcr_pos.mean(axis=1)
vel_inc_vs_time = lcr_vel.mean(axis=1)
theta_inc_vs_time = lcr_theta.mean(axis=1)
print('{:10s}: {}'.format('RMSE', x_rmse_vs_time.mean(axis=0)))
print('{:10s}: {}'.format('INC', x_inc_vs_time.mean(axis=0)))
# Save the simulation results into outfile
data_dict = {
'duration': dur,
'disc_tau': disc_tau,
'alg_str': list(alg.keys()),
'x': x,
'mean': mean,
'cov': cov,
'state': {
'rmse': x_rmse_vs_time,
'inc': x_inc_vs_time
},
'position': {
'rmse': pos_rmse_vs_time,
'inc': pos_inc_vs_time
},
'velocity': {
'rmse': vel_rmse_vs_time,
'inc': vel_inc_vs_time
},
'parameter': {
'rmse': theta_rmse_vs_time,
'inc': theta_inc_vs_time
},
}
joblib.dump(data_dict, outfile)
# Visualize simulation results, plots, tables
reentry_demo_results(data_dict)
else:
reentry_demo_results(joblib.load(outfile))
def reentry_analysis_demo(dur=50, mc_sims=10):
tau = 0.05
disc_tau = 0.1
# initial condition statistics
m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 0])
init_rv = GaussRV(5, m0, P0)
# process noise statistics
noise_rv = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 0]))
sys = ReentryVehicle2DTransition(init_rv, noise_rv)
# measurement noise statistics
dim_obs = 2
meas_noise_rv = GaussRV(dim_obs, cov=np.diag([1e-6, 0.17e-6]))
obs = Radar2DMeasurement(meas_noise_rv, 5, radar_loc=np.array([6374, 0.0]))
np.random.seed(0)
# Generate reference trajectory by SDE simulation
x = sys.simulate_continuous(duration=dur, dt=tau, mc_sims=mc_sims)
y = obs.simulate_measurements(x)
# subsample data for the filter and performance score calculation
x = x[:, ::2, ...] # take every second point on the trajectory
y = y[:, ::2, ...] # and corresponding measurement
# Initialize state-space model with mis-specified initial mean
m0 = np.array([6500.4, 349.14, -1.1093, -6.1967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
init_rv = GaussRV(5, m0, P0)
noise_rv = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 1e-6]))
dyn = ReentryVehicle2DTransition(init_rv, noise_rv, dt=disc_tau)
# NOTE: higher tau causes higher spread of trajectories at the ends
# Initialize filters
par_dyn = np.array([[1.0, 1, 1, 1, 1, 1]])
par_obs = np.array([[1.0, 0.9, 0.9, 1e4, 1e4,
1e4]]) # np.atleast_2d(np.ones(6))
mul_ut = np.hstack((np.zeros((dyn.dim_in, 1)), np.eye(dyn.dim_in),
2 * np.eye(dyn.dim_in))).astype(np.int)
alg = OrderedDict({
# GaussianProcessKalman(ssm, par_dyn, par_obs, kernel='rbf', points='ut'),
'bsqkf':
BayesSardKalman(dyn,
obs,
par_dyn,
par_obs,
mul_ut,
mul_ut,
points='ut'),
'ukf':
UnscentedKalman(dyn, obs, kappa=0.0, beta=2.0),
})
alg['bsqkf'].tf_dyn.model.model_var = np.diag([0.1, 0.1, 0.1, 0.1, 0.015])
alg['bsqkf'].tf_obs.model.model_var = 0 * np.eye(2)
print('BSQ EMV\ndyn: {} \nobs: {}'.format(
alg['bsqkf'].tf_dyn.model.model_var.diagonal(),
alg['bsqkf'].tf_obs.model.model_var.diagonal()))
# FILTERING
num_alg = len(alg)
d, steps, mc_sims = x.shape
mean, cov = np.zeros((d, steps, mc_sims, num_alg)), np.zeros(
(d, d, steps, mc_sims, num_alg))
mean_x, cov_x = mean.copy(), cov.copy()
mean_y, cov_y = np.zeros((dim_obs, steps, mc_sims, num_alg)), np.zeros(
(dim_obs, dim_obs, steps, mc_sims, num_alg))
cov_yx = np.zeros((dim_obs, d, steps, mc_sims, num_alg))
# y = np.hstack((np.zeros((dim_obs, 1, mc_sims)), y))
for ia, a in enumerate(alg):
print('Running {:<5} ... '.format(a.upper()), end='', flush=True)
t0 = time.time()
for imc in range(mc_sims):
for k in range(steps):
# TIME UPDATE
alg[a]._time_update(k)
# record predictive moments
mean_x[:, k, imc,
ia], cov_x[..., k, imc,
ia] = alg[a].x_mean_pr, alg[a].x_cov_pr
mean_y[:, k, imc,
ia], cov_y[..., k, imc,
ia] = alg[a].y_mean_pr, alg[a].y_cov_pr
cov_yx[..., k, imc, ia] = alg[a].xy_cov
# MEASUREMENT UPDATE
alg[a]._measurement_update(y[:, k, imc])
# record filtered moments
mean[:, k, imc,
ia], cov[..., k, imc,
ia] = alg[a].x_mean_fi, alg[a].x_cov_fi
# reset filter storage
alg[a].reset()
print('{:>30}'.format('Done in {:.2f} [sec]'.format(time.time() - t0)))
def tables():
steps, mc = 500, 100
# initialize UNGM model
dyn = UNGMTransition(GaussRV(1, cov=5.0), GaussRV(1, cov=10.0))
obs = UNGMMeasurement(GaussRV(1, cov=1.0), 1)
# generate some data
np.random.seed(0)
x = dyn.simulate_discrete(steps, mc)
z = obs.simulate_measurements(x)
par_ut = np.array([[3.0, 0.3]])
par_gh5 = np.array([[5.0, 0.6]])
par_gh7 = np.array([[3.0, 0.4]])
mulind_ut = np.array([[0, 1, 2]])
mulind_gh = lambda degree: np.atleast_2d(np.arange(degree))
# initialize filters/smoothers
algorithms = (
# Classical filters
UnscentedKalman(dyn, obs, alpha=1.0, beta=0.0),
GaussHermiteKalman(dyn, obs, deg=5),
GaussHermiteKalman(dyn, obs, deg=7),
# GPQ filters
GaussianProcessKalman(dyn,
obs,
par_ut,
par_ut,
kernel='rbf',
points='ut',
point_hyp={'alpha': 1.0}),
GaussianProcessKalman(dyn,
obs,
par_gh5,
par_gh5,
kernel='rbf',
points='gh',
point_hyp={'degree': 5}),
GaussianProcessKalman(dyn,
obs,
par_gh7,
par_gh7,
kernel='rbf',
points='gh',
point_hyp={'degree': 7}),
# BSQ filters
BayesSardKalman(dyn,
obs,
par_ut,
par_ut,
mulind_ut,
mulind_ut,
points='ut',
point_hyp={'alpha': 1.0}),
BayesSardKalman(dyn,
obs,
par_gh5,
par_gh5,
mulind_gh(5),
mulind_gh(5),
points='gh',
point_hyp={'degree': 5}),
BayesSardKalman(dyn,
obs,
par_gh7,
par_gh7,
mulind_gh(7),
mulind_gh(7),
points='gh',
point_hyp={'degree': 7}),
)
num_algs = len(algorithms)
# space for estimates
dim = dyn.dim_state
mean_f, cov_f = np.zeros((dim, steps, mc, num_algs)), np.zeros(
(dim, dim, steps, mc, num_algs))
mean_s, cov_s = np.zeros((dim, steps, mc, num_algs)), np.zeros(
(dim, dim, steps, mc, num_algs))
# do filtering/smoothing
t0 = time.time() # measure execution time
print('Running filters/smoothers ...')
for a, alg in enumerate(algorithms):
print('{}'.format(
alg.__class__.__name__)) # print filter/smoother name
for sim in range(mc):
mean_f[..., sim, a], cov_f[..., sim, a] = alg.forward_pass(z[...,
sim])
mean_s[..., sim, a], cov_s[..., sim, a] = alg.backward_pass()
alg.reset()
print('Done in {0:.4f} [sec]'.format(time.time() - t0))
# evaluate perfomance
scores = evaluate_performance(x, mean_f, cov_f, mean_s, cov_s)
rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s = scores[:
6]
rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s = scores[6:]
# put data into Pandas DataFrame for fancy printing and latex export
# row_labels = ['SR', 'UT', 'GH-5', 'GH-7', 'GH-10', 'GH-15', 'GH-20']
row_labels = ['UT', 'GH-5', 'GH-7']
num_labels = len(row_labels)
col_labels = [
'Classical', 'GPQ', 'BSQ', 'Classical (2std)', 'GPQ (2std)',
'BSQ (2std)'
]
rmse_table_f = pd.DataFrame(np.hstack(
(rmseMean_f.reshape(3,
num_labels).T, rmseStd_f.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
nci_table_f = pd.DataFrame(np.hstack(
(nciMean_f.reshape(3, num_labels).T, nciStd_f.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
nll_table_f = pd.DataFrame(np.hstack(
(nllMean_f.reshape(3, num_labels).T, nllStd_f.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
rmse_table_s = pd.DataFrame(np.hstack(
(rmseMean_s.reshape(3,
num_labels).T, rmseStd_s.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
nci_table_s = pd.DataFrame(np.hstack(
(nciMean_s.reshape(3, num_labels).T, nciStd_s.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
nll_table_s = pd.DataFrame(np.hstack(
(nllMean_s.reshape(3, num_labels).T, nllStd_s.reshape(3,
num_labels).T)),
index=row_labels,
columns=col_labels)
# print kernel parameters
print('Kernel parameters')
print('{:5}: {}'.format('UT', par_ut))
print('{:5}: {}'.format('GH-5', par_gh5))
print('{:5}: {}'.format('GH-7', par_gh7))
print()
# print tables
pd.set_option('precision', 2, 'display.max_columns', 6)
print('Filter RMSE')
print(rmse_table_f)
print('Filter NCI')
print(nci_table_f)
print('Filter NLL')
print(nll_table_f)
print('Smoother RMSE')
print(rmse_table_s)
print('Smoother NCI')
print(nci_table_s)
print('Smoother NLL')
print(nll_table_s)
# return computed metrics for filters and smoothers
return {
'filter_RMSE': rmse_table_f,
'filter_NCI': nci_table_f,
'filter_NLL': nll_table_f,
'smoother_RMSE': rmse_table_s,
'smoother_NCI': nci_table_s,
'smoother_NLL': nll_table_s
}
def ukf_trunc_demo(mc_sims=50):
disc_tau = 0.5 # discretization period in seconds
duration = 200
# define system
m0 = np.array([6500.4, 349.14, -1.8093, -6.7967, 0.6932])
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 0])
x0 = GaussRV(5, m0, P0)
q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 0]))
sys = ReentryVehicle2DTransition(x0, q, dt=disc_tau)
# define radar measurement model
r = GaussRV(2, cov=np.diag([1e-6, 0.17e-6]))
obs = Radar2DMeasurement(r, sys.dim_state)
# simulate reference state trajectory by SDE integration
x = sys.simulate_continuous(duration, disc_tau, mc_sims)
x_ref = x.mean(axis=2)
# simulate corresponding radar measurements
y = obs.simulate_measurements(x)
# initialize state-space model; uses cartesian2polar as measurement (not polar2cartesian)
P0 = np.diag([1e-6, 1e-6, 1e-6, 1e-6, 1])
x0 = GaussRV(5, m0, P0)
q = GaussRV(3, cov=np.diag([2.4064e-5, 2.4064e-5, 1e-6]))
dyn = ReentryVehicle2DTransition(x0, q, dt=disc_tau)
# initialize UKF and UKF in truncated version
alg = (
UnscentedKalman(dyn, obs),
TruncatedUnscentedKalman(dyn, obs),
)
num_alg = len(alg)
# space for filtered mean and covariance
steps = x.shape[1]
x_mean = np.zeros((dyn.dim_in, steps, mc_sims, num_alg))
x_cov = np.zeros((dyn.dim_in, dyn.dim_in, steps, mc_sims, num_alg))
# filtering estimate of the state trajectory based on provided measurements
from tqdm import trange
for i_est, estimator in enumerate(alg):
for i_mc in trange(mc_sims):
x_mean[..., i_mc, i_est], x_cov[..., i_mc, i_est] = estimator.forward_pass(y[..., i_mc])
estimator.reset()
# Plots
plt.figure()
g = GridSpec(2, 4)
plt.subplot(g[:, :2])
# Earth surface w/ radar position
radar_x, radar_y = dyn.R0, 0
t = 0.02 * np.arange(-1, 4, 0.1)
plt.plot(dyn.R0 * np.cos(t), dyn.R0 * np.sin(t), color='darkblue', lw=2)
plt.plot(radar_x, radar_y, 'ko')
plt.plot(x_ref[0, :], x_ref[1, :], color='r', ls='--')
# Convert from polar to cartesian
meas = np.stack(( + y[0, ...] * np.cos(y[1, ...]), radar_y + y[0, ...] * np.sin(y[1, ...])), axis=0)
for i in range(mc_sims):
# Vehicle trajectory
# plt.plot(x[0, :, i], x[1, :, i], alpha=0.35, color='r', ls='--')
# Plot measurements
plt.plot(meas[0, :, i], meas[1, :, i], 'k.', alpha=0.3)
# Filtered position estimate
plt.plot(x_mean[0, 1:, i, 0], x_mean[1, 1:, i, 0], color='g', alpha=0.3)
plt.plot(x_mean[0, 1:, i, 1], x_mean[1, 1:, i, 1], color='orange', alpha=0.3)
# Performance score plots
error2 = x_mean.copy()
lcr = np.zeros((steps, mc_sims, num_alg))
for a in range(num_alg):
for k in range(steps):
mse = mse_matrix(x[:4, k, :], x_mean[:4, k, :, a])
for imc in range(mc_sims):
error2[:, k, imc, a] = squared_error(x[:, k, imc], x_mean[:, k, imc, a])
lcr[k, imc, a] = log_cred_ratio(x[:4, k, imc], x_mean[:4, k, imc, a], x_cov[:4, :4, k, imc, a], mse)
# Averaged RMSE and Inclination Indicator in time
pos_rmse_vs_time = np.sqrt((error2[:2, ...]).sum(axis=0)).mean(axis=1)
inc_ind_vs_time = lcr.mean(axis=1)
# Plots
plt.subplot(g[0, 2:])
plt.title('RMSE')
plt.plot(pos_rmse_vs_time[:, 0], label='UKF', color='g')
plt.plot(pos_rmse_vs_time[:, 1], label='UKF-trunc', color='r')
plt.legend()
plt.subplot(g[1, 2:])
plt.title('Inclination Indicator $I^2$')
plt.plot(inc_ind_vs_time[:, 0], label='UKF', color='g')
plt.plot(inc_ind_vs_time[:, 1], label='UKF-trunc', color='r')
plt.legend()
plt.show()
print('Average RMSE: {}'.format(pos_rmse_vs_time.mean(axis=0)))
print('Average I2: {}'.format(inc_ind_vs_time.mean(axis=0)))
def tables(steps=500, sims=100):
# setup univariate non-stationary growth model
x0 = GaussRV(1, cov=np.atleast_2d(5.0))
q = GaussRV(1, cov=np.atleast_2d(10.0))
dyn = UNGMTransition(x0, q) # dynamics
r = GaussRV(1)
obs = UNGMMeasurement(r, 1) # observation model
x = dyn.simulate_discrete(steps, mc_sims=sims) # generate some data
z = obs.simulate_measurements(x)
kern_par_sr = np.array([[1.0, 0.3 * dyn.dim_in]])
kern_par_ut = np.array([[1.0, 3.0 * dyn.dim_in]])
kern_par_gh = np.array([[1.0, 0.1 * dyn.dim_in]])
# initialize filters/smoothers
algorithms = (
CubatureKalman(dyn, obs),
UnscentedKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussHermiteKalman(dyn, obs),
GaussianProcessKalman(dyn, obs, kern_par_sr, kern_par_sr, points='sr'),
GaussianProcessKalman(dyn, obs, kern_par_ut, kern_par_ut, points='ut'),
GaussianProcessKalman(dyn, obs, kern_par_sr, kern_par_sr, points='gh', point_hyp={'degree': 5}),
GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 7}),
GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 10}),
GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 15}),
GaussianProcessKalman(dyn, obs, kern_par_gh, kern_par_gh, points='gh', point_hyp={'degree': 20}),
)
num_algs = len(algorithms)
# space for estimates
mean_f, cov_f = np.zeros((dyn.dim_in, steps, sims, num_algs)), np.zeros((dyn.dim_in, dyn.dim_in, steps, sims, num_algs))
mean_s, cov_s = np.zeros((dyn.dim_in, steps, sims, num_algs)), np.zeros((dyn.dim_in, dyn.dim_in, steps, sims, num_algs))
# do filtering/smoothing
t0 = time.time() # measure execution time
print('Running filters/smoothers ...', flush=True)
for a, alg in enumerate(algorithms):
for sim in trange(sims, desc='{:25}'.format(alg.__class__.__name__), file=sys.stdout):
mean_f[..., sim, a], cov_f[..., sim, a] = alg.forward_pass(z[..., sim])
mean_s[..., sim, a], cov_s[..., sim, a] = alg.backward_pass()
alg.reset()
print('Done in {0:.4f} [sec]'.format(time.time() - t0))
# evaluate perfomance
scores = evaluate_performance(x, mean_f, cov_f, mean_s, cov_s)
rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s = scores[:6]
rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s = scores[6:]
# put data into Pandas DataFrame for fancy printing and latex export
row_labels = ['SR', 'UT', 'GH-5', 'GH-7', 'GH-10', 'GH-15',
'GH-20'] # [alg.__class__.__name__ for alg in algorithms]
col_labels = ['Classical', 'Bayesian', 'Classical (2std)', 'Bayesian (2std)']
rmse_table_f = pd.DataFrame(np.hstack((rmseMean_f.reshape(2, 7).T, rmseStd_f.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
nci_table_f = pd.DataFrame(np.hstack((nciMean_f.reshape(2, 7).T, nciStd_f.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
nll_table_f = pd.DataFrame(np.hstack((nllMean_f.reshape(2, 7).T, nllStd_f.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
rmse_table_s = pd.DataFrame(np.hstack((rmseMean_s.reshape(2, 7).T, rmseStd_s.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
nci_table_s = pd.DataFrame(np.hstack((nciMean_s.reshape(2, 7).T, nciStd_s.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
nll_table_s = pd.DataFrame(np.hstack((nllMean_s.reshape(2, 7).T, nllStd_s.reshape(2, 7).T)),
index=row_labels, columns=col_labels)
# print tables
print('Filter RMSE')
print(rmse_table_f)
print('Filter NCI')
print(nci_table_f)
print('Filter NLL')
print(nll_table_f)
print('Smoother RMSE')
print(rmse_table_s)
print('Smoother NCI')
print(nci_table_s)
print('Smoother NLL')
print(nll_table_s)
# return computed metrics for filters and smoothers
return {'filter_RMSE': rmse_table_f, 'filter_NCI': nci_table_f, 'filter_NLL': nll_table_f,
'smoother_RMSE': rmse_table_s, 'smoother_NCI': nci_table_s, 'smoother_NLL': nll_table_s}