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