From f6ee0e36c9cc075e5f007c44e95dc3aaa7736a57 Mon Sep 17 00:00:00 2001 From: Greg Brown Date: Sun, 30 Jan 2022 23:50:36 +0000 Subject: Initial commit --- murl/particles/centralised.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 murl/particles/centralised.py (limited to 'murl/particles/centralised.py') diff --git a/murl/particles/centralised.py b/murl/particles/centralised.py new file mode 100644 index 0000000..33be0b3 --- /dev/null +++ b/murl/particles/centralised.py @@ -0,0 +1,30 @@ +from murl.particles import ParticleCloud +from murl.sensors import CONE_WIDTH + +import numpy as np +from scipy.stats import multivariate_normal, norm + +class Cloud(ParticleCloud): + BETA = 1.E+0 + + def calculate_weight(self, angle_dists, robot_guesses, robot_scans): + self._particles[:,3] = 0.0 + mask = self._world.is_valid(self._particles) + self._particles[~mask,3] = float('-inf') + count = np.ma.array(self._particles[:,3], mask=~mask).count() + + for angle, dist in angle_dists: + angles = angle + self._rng.uniform(-CONE_WIDTH/2, CONE_WIDTH/2, + (self.RAY_SAMPLES, count)) + true_dists = self._world.get_dist(self._particles[mask], angles) + + if np.isfinite(dist): + self._particles[mask,3] += norm.logpdf(dist, true_dists, 0.8) + else: + self._particles[mask,3] += norm.logsf(3, true_dists, 0.8) + + for guess, scan in zip(robot_guesses, robot_scans): + angles = scan[1] + self._particles[mask,2] + poses = scan[0] * np.stack([np.cos(angles), np.sin(angles)]).transpose() + self._particles[mask,:2] + update = multivariate_normal.logpdf(poses, guess[0], guess[1]) + self._particles[mask,3] += self.BETA * update -- cgit v1.2.3