def evaluate_performance(x, mean_f, cov_f, mean_s, cov_s, bootstrap_variance=True):
num_dim, num_step, num_sim, num_alg = mean_f.shape
# simulation-average of time-averaged RMSE
print('RMSE...')
rmseData_f = np.sqrt(np.mean(squared_error(x[..., na], mean_f), axis=1))
rmseData_s = np.sqrt(np.mean(squared_error(x[..., na], mean_s), axis=1))
rmseMean_f = rmseData_f.mean(axis=1).T
rmseMean_s = rmseData_s.mean(axis=1).T
print('NLL and NCI...')
nllData_f = np.zeros((1, num_step, num_sim, num_alg))
nciData_f = nllData_f.copy()
nllData_s = nllData_f.copy()
nciData_s = nllData_f.copy()
for k in range(1, num_step):
for fi in range(num_alg):
mse_mat_f = mse_matrix(x[:, k, :], mean_f[:, k, :, fi])
mse_mat_s = mse_matrix(x[:, k, :], mean_f[:, k, :, fi])
for s in range(num_sim):
# filter scores
nllData_f[:, k, s, fi] = neg_log_likelihood(x[:, k, s], mean_f[:, k, s, fi], cov_f[:, :, k, s, fi])
nciData_f[:, k, s, fi] = log_cred_ratio(x[:, k, s], mean_f[:, k, s, fi], cov_f[:, :, k, s, fi],
mse_mat_f)
# smoother scores
nllData_s[:, k, s, fi] = neg_log_likelihood(x[:, k, s], mean_s[:, k, s, fi], cov_s[:, :, k, s, fi])
nciData_s[:, k, s, fi] = log_cred_ratio(x[:, k, s], mean_s[:, k, s, fi], cov_s[:, :, k, s, fi],
mse_mat_s)
nciData_f, nciData_s = nciData_f.mean(axis=1), nciData_s.mean(axis=1)
nllData_f, nllData_s = nllData_f.mean(axis=1), nllData_s.mean(axis=1)
# average scores (over time and MC simulations)
nciMean_f, nciMean_s = nciData_f.mean(axis=1).T, nciData_s.mean(axis=1).T
nllMean_f, nllMean_s = nllData_f.mean(axis=1).T, nllData_s.mean(axis=1).T
if bootstrap_variance:
print('Bootstrapping variance ...')
num_bs_samples = 10000
rmseStd_f, rmseStd_s = np.zeros((num_alg, 1)), np.zeros((num_alg, 1))
nciStd_f, nciStd_s = rmseStd_f.copy(), rmseStd_f.copy()
nllStd_f, nllStd_s = rmseStd_f.copy(), rmseStd_f.copy()
for f in range(num_alg):
rmseStd_f[f] = 2 * np.sqrt(bootstrap_var(rmseData_f[..., f], num_bs_samples))
rmseStd_s[f] = 2 * np.sqrt(bootstrap_var(rmseData_s[..., f], num_bs_samples))
nciStd_f[f] = 2 * np.sqrt(bootstrap_var(nciData_f[..., f], num_bs_samples))
nciStd_s[f] = 2 * np.sqrt(bootstrap_var(nciData_s[..., f], num_bs_samples))
nllStd_f[f] = 2 * np.sqrt(bootstrap_var(nllData_f[..., f], num_bs_samples))
nllStd_s[f] = 2 * np.sqrt(bootstrap_var(nllData_s[..., f], num_bs_samples))
return rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s, \
rmseStd_f, nciStd_f, nllStd_f, rmseStd_s, nciStd_s, nllStd_s
else:
return rmseMean_f, nciMean_f, nllMean_f, rmseMean_s, nciMean_s, nllMean_s
def hypers_demo(lscale=None):
print(f"Seed = {np.random.get_state()[1][0]}")
# set default lengthscales if unspecified
if lscale is None:
lscale = [1e-3, 3e-3, 1e-2, 3e-2, 1e-1, 3e-1, 1, 3, 1e1, 3e1, 1e2, ]
steps, mc = 500, 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=mc) # generate some data
z = obs.simulate_measurements(x)
num_el = len(lscale)
mean_f, cov_f = np.zeros((dyn.dim_in, steps, mc, num_el)), np.zeros((dyn.dim_in, dyn.dim_in, steps, mc, num_el))
for iel, el in enumerate(lscale):
# kernel parameters
ker_par = np.array([[1.0, el * dyn.dim_in]])
# initialize BHKF with current lenghtscale
f = GaussianProcessKalman(dyn, obs, ker_par, ker_par, points='ut', point_hyp={'kappa': 0.0})
# filtering
for s in range(mc):
mean_f[..., s, iel], cov_f[..., s, iel] = f.forward_pass(z[..., s])
# evaluate RMSE, NCI and NLL
rmseVsEl = squared_error(x[..., na], mean_f)
nciVsEl = rmseVsEl.copy()
nllVsEl = rmseVsEl.copy()
for k in range(steps):
for iel in range(num_el):
mse_mat = mse_matrix(x[:, k, :], mean_f[:, k, :, iel])
for s in range(mc):
nciVsEl[:, k, s, iel] = log_cred_ratio(x[:, k, s], mean_f[:, k, s, iel], cov_f[:, :, k, s, iel],
mse_mat)
nllVsEl[:, k, s, iel] = neg_log_likelihood(x[:, k, s], mean_f[:, k, s, iel], cov_f[:, :, k, s, iel])
# average out time and MC simulations
rmseVsEl = np.sqrt(np.mean(rmseVsEl, axis=1)).mean(axis=1)
nciVsEl = nciVsEl.mean(axis=(1, 2))
nllVsEl = nllVsEl.mean(axis=(1, 2))
# plot influence of changing lengthscale on the RMSE and NCI and NLL filter performance
plt.figure()
plt.semilogx(lscale, rmseVsEl.squeeze(), color='k', ls='-', lw=2, marker='o', label='RMSE')
plt.semilogx(lscale, nciVsEl.squeeze(), color='k', ls='--', lw=2, marker='o', label='NCI')
plt.semilogx(lscale, nllVsEl.squeeze(), color='k', ls='-.', lw=2, marker='o', label='NLL')
plt.grid(True)
plt.legend()
plt.show()
return {'el': lscale, 'rmse': rmseVsEl, 'nci': nciVsEl, 'neg_log_likelihood': nllVsEl}
def eval_perf_scores(x, mf, Pf):
xD, steps, mc_sims, num_filt = mf.shape
# average RMSE over simulations
rmse = np.sqrt(((x[..., na] - mf)**2).sum(axis=0))
rmse_avg = rmse.mean(axis=1)
reg = 1e-6 * np.eye(xD)
# average inclination indicator over simulations
lcr = np.empty((steps, mc_sims, num_filt))
for f in range(num_filt):
for k in range(steps):
mse = mse_matrix(x[:, k, :], mf[:, k, :, f]) + reg
for imc in range(mc_sims):
lcr[k, imc, f] = log_cred_ratio(x[:, k, imc], mf[:, k, imc, f],
Pf[..., k, imc, f], mse)
lcr_avg = lcr.mean(axis=1)
return rmse_avg, lcr_avg
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 lengthscale_filter_demo(lscale):
steps, mc = 500, 20
# 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
x = dyn.simulate_discrete(steps, mc)
z = obs.simulate_measurements(x)
dim = dyn.dim_state
num_el = len(lscale)
# lscale = [1e-3, 3e-3, 1e-2, 3e-2, 1e-1, 3e-1, 1, 3, 1e1, 3e1] # , 1e2, 3e2]
mean_f, cov_f = np.zeros((dim, steps, mc, num_el)), np.zeros(
(dim, dim, steps, mc, num_el))
for iel, el in enumerate(lscale):
# kernel parameters
ker_par = np.array([[1.0, el * dim]])
# initialize BHKF with current lenghtscale
f = GaussianProcessKalman(dyn,
obs,
ker_par,
ker_par,
kernel='rbf',
points='ut')
# filtering
for s in range(mc):
mean_f[..., s, iel], cov_f[..., s, iel] = f.forward_pass(z[..., s])
# evaluate RMSE, NCI and NLL
rmseVsEl = squared_error(x[..., na], mean_f)
nciVsEl = rmseVsEl.copy()
nllVsEl = rmseVsEl.copy()
for k in range(steps):
for iel in range(num_el):
mse_mat = mse_matrix(x[:, k, :], mean_f[:, k, :, iel])
for s in range(mc):
nciVsEl[:, k, s,
iel] = log_cred_ratio(x[:, k, s], mean_f[:, k, s, iel],
cov_f[:, :, k, s, iel], mse_mat)
nllVsEl[:, k, s,
iel] = neg_log_likelihood(x[:, k, s], mean_f[:, k, s,
iel],
cov_f[:, :, k, s, iel])
# average out time and MC simulations
rmseVsEl = np.sqrt(np.mean(rmseVsEl, axis=1)).mean(axis=1)
nciVsEl = nciVsEl.mean(axis=(1, 2))
nllVsEl = nllVsEl.mean(axis=(1, 2))
# plot influence of changing lengthscale on the RMSE and NCI and NLL filter performance
plt.figure()
plt.semilogx(lscale,
rmseVsEl.squeeze(),
color='k',
ls='-',
lw=2,
marker='o',
label='RMSE')
plt.semilogx(lscale,
nciVsEl.squeeze(),
color='k',
ls='--',
lw=2,
marker='o',
label='NCI')
plt.semilogx(lscale,
nllVsEl.squeeze(),
color='k',
ls='-.',
lw=2,
marker='o',
label='NLL')
plt.grid(True)
plt.legend()
plt.show()
plot_data = {
'el': lscale,
'rmse': rmseVsEl,
'nci': nciVsEl,
'neg_log_likelihood': nllVsEl
}
return plot_data
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)))