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
class TestSensorSim:
def setup(self):
self.measurement_std = [1, 2, 3]
self.sim1d = SensorSim(0, 0.5, self.measurement_std, 1)
self.sim2d = SensorSim((0, 0), (1, 2), self.measurement_std, 2)
def test_basic_constructor(self):
pass
def test_constructor_measurement_check(self):
with pytest.raises(ValueError):
SensorSim(0, 1, 4, 1)
def test_constructor_position_check(self):
with pytest.raises(ValueError):
SensorSim(0, (1, 1), [4], 2)
def test_constructor_velocity_check(self):
with pytest.raises(ValueError):
SensorSim((0, 0), 1, [4], 2)
def test_read_all_1d(self):
for x in self.measurement_std:
meas, truth = self.sim1d.read()
assert meas.shape == (1, 1)
assert truth.shape == (1, 1)
def test_read_all_2d(self):
for x in self.measurement_std:
meas, truth = self.sim2d.read()
assert meas.shape == (2, 1)
assert truth.shape == (2, 1)
def test_read_too_many(self):
for x in self.measurement_std:
self.sim2d.read()
with pytest.raises(IndexError):
self.sim2d.read()
def test_batch_read_2d(self):
res = self.sim2d.batch_read()
assert len(res) == len(self.measurement_std)
shape = np.asarray(res).shape
assert shape[0] == len(self.measurement_std)
assert shape[1] == 2
assert shape[2] == 2
assert shape[3] == 1
"""
Evaluate the SensorSim class by plotting the result of an increasing
and later decreasing measurement noise variance
"""
import numpy as np
from matplotlib import pyplot as plt
from noiseestimation.sensorsim import SensorSim
measurement_std = np.arange(0, 4, .02)
measurement_std = np.concatenate(
(measurement_std, list(reversed(measurement_std))))
sim = SensorSim(
position=(
0, 0), velocity=(
0.5, 1), measurement_std=measurement_std, dim=2)
readings = np.array([np.array(sim.read()) for _ in measurement_std])
plt.plot(readings[:, 0, 0], readings[:, 0, 1], 'go', label="sensor readings")
plt.plot(readings[:, 1, 0], readings[:, 1, 1],
'm', linewidth=2, label="true values")
plt.legend(loc='lower right')
plt.title("Position sensor readings")
plt.show()
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)