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 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 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):
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, 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)
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)
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)
return R_extended
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, 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 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 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
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))
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)
return (error_extended, error_mehra, error_approx, error_approx_posterior)
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 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 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)
from numpy.random import randn
from noiseestimation.correlator import Correlator
num_runs = 50
arr_length = 200
num_mehra_white = 0
num_ljungbox_white = 0
for i in range(num_runs):
seq = randn(arr_length)
cor = Correlator(seq)
if cor.isWhite():
num_ljungbox_white += 1
if cor.isWhite('mehra'):
num_mehra_white += 1
print("-" * 10)
print("%d / %d white (%.3f %%) using Mehra method" %
(num_mehra_white, num_runs, 100 * float(num_mehra_white) / num_runs))
print(
"%d / %d white (%.3f %%) using Ljund-Box method" %
(num_ljungbox_white, num_runs, 100 * float(num_ljungbox_white) / num_runs))
