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 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)))