def chi2_overlays(self, plot_nbins=40, num_line_pts=2000):
for cov_type in self.cov_list:
sigmas = self.get_sigmas(cov_type)
title = r"{} {} $\chi^2$ comparison".format(self.seq, cov_type)
chi2_overlay(sigmas**2, state_size(cov_type), title,
num_line_pts=num_line_pts,
plot_nbins=plot_nbins)
meas_noise = np.array([r_noise, r_noise, r_noise, r_noise,
r_heading, r_heading, r_heading, r_heading])
# get system
sys = TwoDLocalizationSystem(beacon_x, beacon_y, x_init, dyn_noise, meas_noise, dt)
est = TwoDLocalizationEKF(beacon_x, beacon_y, x_init_est, P_init, dyn_noise_ekf, meas_noise, dt)
# simulate and make plots for one
if args.mode == "test1":
inputs = all_inputs[:,:,args.case]
simdata = DiscreteTimeSystemSimulation(sys, est, x_init, P_init, 2*len(beacon_x), inputs, nruns)
simdata.analysis()
# Chi2 Overlay Plots
chi2_overlay(simdata.NEES_est[0,:], simdata.state_dim,
r"Overlay with EKF $\rho_k$", plot_nbins=args.plot_nbins, plt_chi2_pdf=False)
chi2_overlay(simdata.NEES_true[0,:], simdata.state_dim,
r"Overlay with EKF Monte-Carlo $\rho_k$", plot_nbins=args.plot_nbins)
# MC Confidence Bounds plot
alpha = 0.95
NEES_est_sum = np.sum(simdata.NEES_est, axis=0)
NEES_true_sum = np.sum(simdata.NEES_true, axis=0)
interval_bounds_plot(NEES_est_sum, simdata.state_dim, alpha,
"{} interval for EKF Estimated NEES".format(alpha), simdata.nruns)
interval_bounds_plot(NEES_true_sum, simdata.state_dim, alpha,
"{} interval for EKF True NEES".format(alpha), simdata.nruns)
# Timeseries plot
simdata.plt_timeseries(stepsize=2)
int_upper_lim=args.int_upper_lim)
print("Mean: {}, std: {}".format(mean_chi2_divs, std_chi2_divs))
# Total sigma counting display
num_1sigma_str = []
num_2sigma_str = []
num_3sigma_str = []
for i in range(state_size(cov_type)):
val1 = num_1sigma[cov_type][i] / num_timesteps * 100.0
val2 = num_2sigma[cov_type][i] / num_timesteps * 100.0
val3 = num_3sigma[cov_type][i] / num_timesteps * 100.0
num_1sigma_str.append("{:4.1f}".format(val1))
num_2sigma_str.append("{:4.1f}".format(val2))
num_3sigma_str.append("{:4.1f}".format(val3))
num_1sigma_str = ", ".join(num_1sigma_str)
num_2sigma_str = ", ".join(num_2sigma_str)
num_3sigma_str = ", ".join(num_3sigma_str)
print("Percent <= 1 sigma: {}".format(num_1sigma_str))
print("Percent <= 2 sigma: {}".format(num_2sigma_str))
print("Percent <= 3 sigma: {}".format(num_3sigma_str))
# Make plots of overall calibration
plot_chi2_pdf = not (args.cov_source == "original")
chi2_overlay(sigmas[k]**2,
df[k],
"",
plot_nbins=args.plot_nbins,
plt_chi2_pdf=plot_chi2_pdf)
plt.show()
print("Scale: {}".format(scale.value))
# Apply scalar mapping to all estimated covariances
s = scale.value
for simdata in simdata_test:
simdata.Pest = s * simdata.Pest
# Evaluate covariance on test data
simdata = simdata_test[0]
# Calculate NEES and divergence
simdata.calc_tot_NEES()
simdata.calc_divergence(nbins=nbins)
simdata.compute_sigma_percentages()
# Chi2 Overlay Plot
chi2_overlay(simdata.NEES_est[0, :],
simdata.state_dim,
"",
plot_nbins=40,
plt_chi2_pdf=True)
# Confidence bounds plot
NEES_est_sum = np.sum(simdata.NEES_est, axis=0)
alpha = 0.95
interval_bounds_plot(NEES_est_sum, simdata.state_dim, alpha,
"{} interval for EKF Scalar-Adjusted NEES".format(alpha),
simdata.nruns)
plt.show()
cov = Q @ Q.T
tri_idx = 0
for l in range(cov_dim):
for m in range(l, cov_dim):
cov[l, m] *= train_output_maxes[tri_idx]
if l != m:
cov[m, l] *= train_output_maxes[tri_idx]
tri_idx += 1
simdata.Pest[i, j, :, :] = cov
idx += 1
# Compute test divergence
simdata.calc_tot_NEES()
(test_divergence, _) = simdata.calc_divergence(nbins=nbins)
simdata.compute_sigma_percentages()
# Make plots
singular_indices = simdata.singular_indices[0]
all_indices = set([i for i in range(simdata.num_timesteps)])
nonsingular_indices = list(all_indices - singular_indices)
nonsingular_indices.sort()
chi2_vars_est = simdata.NEES_est[0, nonsingular_indices]
chi2_overlay(chi2_vars_est,
simdata.state_dim,
"",
plot_nbins=40,
plt_chi2_pdf=True)
plt.show()
k = 4 # spring constant
m = 1 # mass
c = 0.1 # damping
dyn_noise = 0.005
meas_noise = 0.003
(sys, est) = SpringMass(dt, c, k, m, x0, v0, dyn_noise, meas_noise)
simdata = DiscreteTimeSystemSimulation(sys, est, init_state, init_cov, 1,
inputs, nruns)
simdata.analysis()
# Timeseries Plot
simdata.plt_timeseries(stepsize=20)
# Overlays with chi2 pdf
chi2_overlay(simdata.NEES_est[0, :], simdata.state_dim, "", plot_nbins='auto')
chi2_overlay(simdata.NEES_true[0, :], simdata.state_dim, "", plot_nbins='auto')
# Confidence Bounds plot
alpha = 0.95
NEES_est_sum = np.sum(simdata.NEES_est, axis=0)
NEES_true_sum = np.sum(simdata.NEES_true, axis=0)
interval_bounds_plot(NEES_est_sum, simdata.state_dim, alpha,
"{} interval for KF Estimated NEES".format(alpha), nruns)
interval_bounds_plot(NEES_true_sum, simdata.state_dim, alpha,
"{} interval for KF True NEES".format(alpha), nruns)
# White noise visualization
simdata.plt_inn(1 / dt)
plt.show()