def perform_estimation(residuals, residuals_posterior, tracker, H_arr, Ks): cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) R_mehra = estimate_noise_mehra(correlation, Ks[-1], tracker.F, H_arr[-1]) R_extended = estimate_noise_extended(correlation, Ks, tracker.F, H_arr) R_approx = estimate_noise_approx(correlation[0], H_arr[-1], tracker.P) cor_posterior = Correlator(residuals_posterior) correlation_posterior = cor_posterior.autocorrelation(used_taps) R_approx_posterior = estimate_noise_approx(correlation_posterior[0], H_arr[-1], tracker.P, "posterior") truth = R_proto * measurement_var error_extended = matrix_error(R_extended, truth) error_mehra = matrix_error(R_mehra, truth) error_approx = matrix_error(R_approx, truth) error_approx_posterior = matrix_error(R_approx_posterior, truth) print("Extended estimation:") print("\tEstimated R:", R_extended) print("\tError: %.6f" % error_extended) print("Mehra estimation:") print("\tEstimated R:", R_mehra) print("\tError: %.6f" % error_mehra) print("Approximate estimation:") print("\tEstimated R:", R_approx) print("\tError: %.6f" % error_approx) print("Approximate estimation (posterior):") print("\tEstimated R:", R_approx_posterior) print("\tError: %.6f" % error_approx_posterior) return (error_extended, error_mehra, error_approx, error_approx_posterior)
def perform_estimation(residuals, tracker, F_arr, K_arr): cor = Correlator(residuals) C_arr = cor.autocorrelation(used_taps) # R = estimate_noise_mehra(C_arr, tracker.K, tracker.F, tracker.H) R_approx = estimate_noise_approx(C_arr[0], tracker.H, tracker.P) # R_extended = estimate_noise_extended(C_arr, K_arr, F_arr, tracker.H) return R_approx
def perform_estimation(residuals, tracker): residuals = residuals - np.average(residuals, axis=0) cor = Correlator(residuals) C_arr = cor.autocorrelation(used_taps) # R_ml = estimate_noise_ukf_ml(C_arr[0], tracker.Pz) R_scaled = estimate_noise_ukf_scaling(C_arr[0], tracker.Pz, tracker.R) return R_scaled
def perform_estimation(residuals, tracker, map_estimate): residuals = residuals - np.average(residuals, axis=0) cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) R_ml = estimate_noise_ukf_ml(correlation[0], tracker.Pz) R_scaled = estimate_noise_ukf_scaling(correlation[0], tracker.Pz, tracker.R) R_map = map_estimate truth = R_proto * sim_var error_ml = matrix_error(R_ml, truth) error_scaled = matrix_error(R_scaled, truth) error_map = matrix_error(R_map, truth) # truth_norm = matrix_error(truth, 0) # print("Truth") # print(truth) # print("ML:") # print("", R_ml) # print("\tRelative error: %.6f" % (error_ml / truth_norm)) # print("Scaled:") # print("", R_scaled) # print("\tRelative error: %.6f" % (error_scaled / truth_norm)) # print("MAP:") # print("", R_map) # print("\tRelative error: %.6f" % (error_map / truth_norm)) return error_ml, error_scaled, error_map
def run_tracker(): sim, tracker = setup() readings, truths, filtered, residuals = filtering(sim, tracker) # plot_results(readings, filtered, residuals) # perform estimation cor = Correlator(residuals[-sample_size:]) correlation = cor.autocorrelation(used_taps) R_mehra = estimate_noise_mehra(correlation, tracker.K, tracker.F, tracker.H) R_approx = estimate_noise_approx(correlation[0], tracker.H, tracker.P) abs_err_approx = R_approx - measurement_var rel_err_approx = abs_err_approx / measurement_var abs_err_mehra = R_mehra - measurement_var rel_err_mehra = abs_err_mehra / measurement_var print("True: %.6f" % measurement_var) print("Filter: %.6f" % tracker.R) print("Estimated (approximation): %.6f" % R_approx) print("Absolute error: %.6f" % abs_err_approx) print("Relative error: %.6f %%" % (rel_err_approx * 100)) print("Estimated (mehra): %.6f" % R_mehra) print("Absolute error: %.6f" % abs_err_mehra) print("Relative error: %.6f %%" % (rel_err_mehra * 100)) print("-" * 15) return (abs_err_mehra, rel_err_mehra, abs_err_approx, rel_err_approx)
def perform_estimation(residuals, tracker, map_estimate, map_estimate_convergence): cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) R_ml = estimate_noise_ukf_ml(correlation[0], tracker.Pz) R_scaled = estimate_noise_ukf_scaling(correlation[0], tracker.Pz, tracker.R) R_map = map_estimate R_map_conv = map_estimate_convergence truth = R_proto * measurement_var error_ml = matrix_error(R_ml, truth) error_scaled = matrix_error(R_scaled, truth) error_map = matrix_error(R_map, truth) error_map_conv = matrix_error(R_map_conv, truth) # truth_norm = matrix_error(truth, 0) # print("Truth") # print(truth) # print("ML:") # print("", R_ml) # print("\tRelative error: %.6f" % (error_ml / truth_norm)) # print("Scaled:") # print("", R_scaled) # print("\tRelative error: %.6f" % (error_scaled / truth_norm)) # print("MAP:") # print("", R_map) # print("\tRelative error: %.6f" % (error_map / truth_norm)) return error_ml, error_scaled, error_map, error_map_conv
def perform_estimation(residuals, tracker): cor = Correlator(residuals) C_arr = cor.autocorrelation(used_taps) print("Truth:\n", sim_var) R = estimate_noise_mehra(C_arr, tracker.K, tracker.F, tracker.H) print("Mehra Method:\n", R) R_approx = estimate_noise_approx(C_arr[0], tracker.H, tracker.P) print("Approximated Method:\n", R_approx)
def perform_estimation(residuals, tracker, F_arr, K_arr): cor = Correlator(residuals) C_arr = cor.autocorrelation(used_taps) print("Simulated:\n", R_proto * sim_var) R = estimate_noise_mehra(C_arr, tracker.K, tracker.F, tracker.H) print("Mehra:\n", R) R_approx = estimate_noise_approx(C_arr[0], tracker.H, tracker.P) print("Approximation:\n", R_approx) R_extended = estimate_noise_extended(C_arr, K_arr, F_arr, tracker.H) print("Extended:\n", R_extended)
def perform_estimation(residuals, tracker, sample_size): used_taps = int(sample_size / 2) cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) # R_mehra = estimate_noise_mehra( # correlation, tracker.K, tracker.F, tracker.H) R_approx = estimate_noise_approx(correlation[0], tracker.H, tracker.P) # R_approx_posterior = estimate_noise_approx( # correlation[0], tracker.H, tracker.P, "posterior") return R_approx
def perform_estimation(residuals, tracker): cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) R = estimate_noise_ukf_ml(correlation[0], tracker.Pz) truth = R_proto * measurement_var error = matrix_error(R, truth) print("Truth:\n", truth) print("Estimation:\n", R) print("Error: %.6f" % error) print("-" * 15) return error
def perform_estimation(residuals, tracker, H, F_arr, K_arr): cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) R_extended = estimate_noise_extended(correlation, K_arr, F_arr, H) R_mehra = estimate_noise_mehra(correlation, K_arr[-1], F_arr[-1], H) R_approx = estimate_noise_approx(correlation[0], H, tracker.P) truth = R_proto * measurement_var error_extended = matrix_error(R_extended, truth) error_mehra = matrix_error(R_mehra, truth) error_approx = matrix_error(R_approx, truth) return (error_extended, error_mehra, error_approx)
def run_tracker(): # parameters filter_misestimation_factor = 1.0 sample_size = 100 used_taps = int(sample_size * 0.5) measurement_std = 3.5 # set up sensor simulator dt = 0.1 measurement_std_list = np.asarray([measurement_std] * sample_size) sim = SensorSim(0, 0.1, measurement_std_list, 1, timestep=dt) # set up kalman filter tracker = KalmanFilter(dim_x=2, dim_z=1) tracker.F = np.array([[1, dt], [0, 1]]) q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01) tracker.Q = q tracker.H = np.array([[1, 0]]) tracker.R = measurement_std**2 * filter_misestimation_factor tracker.x = np.array([[0, 0]]).T tracker.P = np.eye(2) * 500 # perform sensor simulation and filtering readings = [] truths = [] mu = [] residuals = [] for _ in measurement_std_list: reading, truth = sim.read() readings.extend(reading.flatten()) truths.extend(truth.flatten()) tracker.predict() tracker.update(reading) mu.extend(tracker.x[0]) residual_posterior = reading - np.dot(tracker.H, tracker.x) residuals.extend(residual_posterior[0]) # error = np.asarray(truths) - mu # plot_results(readings, mu, error, residuals) # perform estimation cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) R_approx = estimate_noise_approx(correlation[0], tracker.H, tracker.P, 'posterior') abs_err = measurement_std**2 - R_approx rel_err = abs_err / measurement_std**2 print("True: %.3f" % measurement_std**2) print("Filter: %.3f" % tracker.R) print("Estimated (approximation): %.3f" % R_approx) print("Absolute error: %.3f" % abs_err) print("Relative error: %.3f %%" % (rel_err * 100)) print("-" * 15) return rel_err
def perform_estimation(residuals, tracker, F_arr, Ks): residuals = residuals - np.average(residuals, axis=0) cor = Correlator(residuals) C_arr = cor.autocorrelation(used_taps) truth = R_proto * (sim_var + measurement_var) R = estimate_noise_mehra(C_arr, tracker.K, tracker.F, tracker.H) error_mehra = matrix_error(R, truth) R_approx = estimate_noise_approx(C_arr[0], tracker.H, tracker.P) error_approx = matrix_error(R_approx, truth) R_extended = estimate_noise_extended(C_arr, Ks, F_arr, tracker.H) error_extended = matrix_error(R_extended, truth) return (error_mehra, error_approx, error_extended)
def perform_estimation(residuals, tracker): residuals = residuals - np.average(residuals, axis=0) cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) # tracker.R = np.array([[1, 1], # [1, 2]]) * sim_var R = estimate_noise_ukf_scaling(correlation[0], tracker.Pz, tracker.R) truth = R_proto * (measurement_var + sim_var) error = matrix_error(R, truth) print("Truth:\n", truth) print("Estimation:\n", R) print("Error: %.6f" % error) print("-" * 15) return error
def perform_estimation(residuals, tracker, lags): cor = Correlator(residuals) correlation = cor.autocorrelation(lags) R, MH_T = estimate_noise(correlation, tracker.K, tracker.F, tracker.H, True) truth = R_proto * measurement_var error = matrix_error(R, truth) # since H^T = H = eye(2) we have MH^T = M print("Estimated state covariance:\n", MH_T) print("True R:\n", truth) print("Estimated R:\n", R) print("Error:\n", matrix_error(R, truth)) print("-" * 15) return error
def perform_estimation(residuals, tracker, lags): cor = Correlator(residuals[-sample_size:]) correlation = cor.autocorrelation(lags) R_mehra = estimate_noise_mehra(correlation, tracker.K, tracker.F, tracker.H) R_approx = estimate_noise_approx(correlation[0], tracker.H, tracker.P, "posterior") truth = R_proto * measurement_var error_mehra = matrix_error(R_mehra, truth) error_approx = matrix_error(R_approx, truth) print("Truth:\n", truth) print("Estimation Mehra:\n", R_mehra) print("Error Mehra:\n", error_mehra) print("Estimation Mohamed:\n", R_approx) print("Error Mohamed:\n", error_approx) print("-" * 15) return (error_mehra, error_approx)
def perform_estimation(residuals, tracker, F_arr, K_arr): cor = Correlator(residuals) correlation = cor.autocorrelation(used_taps) R_extended = estimate_noise_extended(correlation, K_arr, F_arr, tracker.H) R_mehra = estimate_noise_mehra(correlation, K_arr[-1], F_arr[-1], tracker.H) R_approx = estimate_noise_approx(correlation[0], tracker.H, tracker.P) truth = R_proto * measurement_var print("Truth:\n", truth) print("Extended Estimation:\n", R_extended) print("Error:\n", matrix_error(R_extended, truth)) print("Mehra estimation:\n", R_mehra) print("Error:\n", matrix_error(R_mehra, truth)) print("Approximated estimation:\n", R_approx) print("Error:\n", matrix_error(R_approx, truth)) print("-" * 15) error = matrix_error(R_extended, truth) return error
def perform_estimation(residuals, tracker, F_arr, K_arr): residuals = residuals - np.average(residuals, axis=0) cor = Correlator(residuals) C_arr = cor.autocorrelation(used_taps) truth = R_proto * sim_var matrix_size = matrix_error(truth, 0) print("Truth:\n", truth) R = estimate_noise_mehra(C_arr, tracker.K, tracker.F, tracker.H) error_R = matrix_error(R, truth) print("Mehra:\n", R) print("\t Relative error: %.6f" % (error_R / matrix_size)) R_extended = estimate_noise_extended(C_arr, K_arr, F_arr, tracker.H) error_R_extended = matrix_error(R_extended, truth) print("Extended:\n", R_extended) print("\t Relative error: %.6f" % (error_R_extended / matrix_size)) R_approx = estimate_noise_approx(C_arr[0], tracker.H, tracker.P) error_R_approx = matrix_error(R_approx, truth) print("Approximation:\n", R_approx) print("\t Relative error: %.6f" % (error_R_approx / matrix_size))
class TestCorrelator: def setup(self): arr = np.array([[[1]], [[2]], [[1]]]) self.cor = Correlator(arr) arr_2d = [[[1], [2]], [[2], [3]], [[1], [2]]] self.cor_2d = Correlator(arr_2d) def test_basic_constructor(self): pass def test_autocorrelation(self): C = self.cor.autocorrelation(2) assert len(C) == 3 assert C[0] == (1 + 4 + 1) / 3. assert C[1] == (2 + 2) / 3. assert C[2] == 1 / 3. def test_autocorrelation_2d(self): C = self.cor_2d.autocorrelation(2) assert C.shape == (3, 2, 2) # check symmetry assert_array_equal(C[:, 0, 1], C[:, 1, 0]) # check autocorrelation of first element C_upper_left = C[:, 0, 0] assert C_upper_left[0] == (1 + 4 + 1) / 3. assert C_upper_left[1] == (2 + 2) / 3. assert C_upper_left[2] == 1 / 3. # check autocorrelation of second element C_lower_right = C[:, 1, 1] assert C_lower_right[0] == (4 + 9 + 4) / 3. assert C_lower_right[1] == (6 + 6) / 3. assert C_lower_right[2] == 4 / 3. def test_autocorrelation_coefficients(self): rho = self.cor.autocorrelation_coefficients(2) assert len(rho) == 3 assert rho[0] == 1 assert rho[1] == 2. / 3 assert rho[2] == 1. / 6 def test_autocorrelation_coefficients_2d(self): rho = self.cor_2d.autocorrelation_coefficients(2) assert rho.shape == (3, 2, 2) # check symmetry assert_array_equal(rho[:, 0, 1], rho[:, 1, 0]) # check correlation coefficients of first element rho_upper_left = rho[:, 0, 0] assert rho_upper_left[0] == 1 assert rho_upper_left[1] == 2. / 3 assert rho_upper_left[2] == 1. / 6 # Check correlation coefficients of second element rho_lower_right = rho[:, 1, 1] assert rho_lower_right[0] == 1 assert rho_lower_right[1] == 4 / (17. / 3) assert rho_lower_right[2] == 4 / 3. / (17. / 3) # Check correlation coefficients between elements, should not be perfect assert rho[0, 0, 1] != 1 def test_isWhite(self): notwhite = Correlator([.1] * 100) assert not notwhite.isWhite('mehra') assert not notwhite.isWhite('ljung-box') white = Correlator([0] * 50 + [1] + [0] * 50) assert white.isWhite('mehra') assert white.isWhite('ljung-box') with pytest.raises(ValueError): white.isWhite('novalidmethod')
def run_tracker(): # parameters measurement_std = 3.5 filter_misestimation_factor = 1.0 sample_size = 2000 estimation_sample_size = 80 used_taps = int(estimation_sample_size * 0.5) # set up sensor simulator dt = 0.1 # measurement_std_list = np.asarray([measurement_std] * sample_size) measurement_std_list = np.linspace(measurement_std / 5, measurement_std * 1, sample_size / 2) measurement_std_list = np.concatenate( (measurement_std_list, list(reversed(measurement_std_list)))) sim = SensorSim(0, 0.1, measurement_std_list, 1, timestep=dt) # set up kalman filter tracker = KalmanFilter(dim_x=2, dim_z=1) tracker.F = np.array([[1, dt], [0, 1]]) q = Q_discrete_white_noise(dim=2, dt=dt, var=0.01) tracker.Q = q tracker.H = np.array([[1, 0]]) tracker.R = measurement_std**2 * filter_misestimation_factor tracker.x = np.array([[0, 0]]).T tracker.P = np.eye(2) * 500 # perform sensor simulation and filtering readings = [] truths = [] mu = [] residuals = [] Rs = [] for idx, _ in enumerate(measurement_std_list): reading, truth = sim.read() readings.extend(reading.flatten()) truths.extend(truth.flatten()) tracker.predict() tracker.update(reading) mu.extend(tracker.x[0]) residuals.extend(tracker.y[0]) Rs.append(tracker.R) if (idx < estimation_sample_size or idx % (estimation_sample_size / 10) != 0): print(idx) continue cor = Correlator(residuals[-estimation_sample_size:]) used_taps = int(estimation_sample_size / 2) correlation = cor.autocorrelation(used_taps) R = estimate_noise(correlation, tracker.K, tracker.F, tracker.H) R_approx = estimate_noise_approx(correlation[0], tracker.H, tracker.P) abs_err = measurement_std**2 - R rel_err = abs_err / measurement_std**2 print("True: %.3f" % measurement_std**2) print("Filter: %.3f" % tracker.R) print("Estimated: %.3f" % R) print("Estimated (approximation): %.3f" % R_approx) print("Absolute error: %.3f" % abs_err) print("Relative error: %.3f %%" % (rel_err * 100)) print("-" * 15) if (R > 0): tracker.R = R # if(R_approx > 0): # tracker.R = R_approx # error = np.asarray(truths) - mu plot_results(readings, mu, Rs, measurement_std_list)