def update(self, measurements: np.array):
weights = self._calculate_weights(measurements)
self._updated = NaiveSampleDistribution(self._predicted.samples,
weights)
if self.neff(weights) < self._resampling_threshold:
self._resample()
def test_sample(self):
mean = [10.0, 15.0]
variances = [0.1, 0.5]
multi_distr = GaussDistribution.create_independent(mean, variances)
n = 100
samples = np.zeros((n, 2))
weights = np.zeros((n, ))
for i in range(0, n):
samples[i] = multi_distr.sample()
weights[i] = 1.0
sample_distr = NaiveSampleDistribution(samples, weights)
for i in range(0, n):
self.assertTrue(sample_distr.sample() in samples)
def __init__(self, initial: BaseDistribution, sample_size: int,
resampling_threshold: float):
samples = np.zeros((sample_size, len(initial.mean)))
for i in range(0, sample_size):
samples[i] = initial.sample()
weights = np.full((sample_size, 1), 1.0)
sample_distr = NaiveSampleDistribution(samples, weights)
self._resampling_threshold = resampling_threshold
self._predicted = sample_distr
self._updated = sample_distr
def _resample(self):
samples = self._updated.samples
weights = self._updated.weights
n = samples.shape[0]
cumulative_sum = np.cumsum(weights)
cumulative_sum[-1] = 1.0
indexes = np.searchsorted(cumulative_sum, np.random.random(n))
new_samples = np.zeros((len(indexes), samples.shape[1]))
for i in range(0, len(indexes)):
new_samples[i] = samples[indexes[i]]
new_weights = np.full(weights.shape, 1 / n)
self._updated = NaiveSampleDistribution(new_samples, new_weights)
def test_mean(self):
mean = [10.0, 15.0]
variances = [0.1, 0.5]
multi_distr = GaussDistribution.create_independent(mean, variances)
n = 100
samples = np.zeros((n, 2))
weights = np.zeros((n, ))
for i in range(0, n):
samples[i] = multi_distr.sample()
weights[i] = 1.0
sample_distr = NaiveSampleDistribution(samples, weights)
sample_mean = sample_distr.mean
for i in range(0, len(mean)):
self.assertTrue(
abs(mean[0] - sample_mean[0]) < math.sqrt(variances[0]))
def predict(self, control: np.array):
samples = self._sample_state(control)
weights = np.full((samples.shape[0], 1), 1.0)
self._predicted = NaiveSampleDistribution(samples, weights)