✨

selflocalization/particle.py

@@ -0,0 +1,85 @@
+import numpy as np
+import random_numbers as rn
+
+
+class Particle(object):
+    """Data structure for storing particle information (state and weight)"""
+    def __init__(self, x=0.0, y=0.0, theta=0.0, weight=0.0):
+        self.x = x
+        self.y = y
+        self.theta = np.mod(theta, 2.0*np.pi)
+        self.weight = weight
+
+    def getX(self):
+        return self.x
+        
+    def getY(self):
+        return self.y
+        
+    def getTheta(self):
+        return self.theta
+        
+    def getWeight(self):
+        return self.weight
+
+    def setX(self, val):
+        self.x = val
+
+    def setY(self, val):
+        self.y = val
+
+    def setTheta(self, val):
+        self.theta = np.mod(val, 2.0*np.pi)
+
+    def setWeight(self, val):
+        self.weight = val
+
+
+def estimate_pose(particles_list):
+    """Estimate the pose from particles by computing the average position and orientation over all particles. 
+    This is not done using the particle weights, but just the sample distribution."""
+    x_sum = 0.0
+    y_sum = 0.0
+    cos_sum = 0.0
+    sin_sum = 0.0
+     
+    for particle in particles_list:
+        x_sum += particle.getX()
+        y_sum += particle.getY()
+        cos_sum += np.cos(particle.getTheta())
+        sin_sum += np.sin(particle.getTheta())
+        
+    flen = len(particles_list)
+    if flen != 0:
+        x = x_sum / flen
+        y = y_sum / flen
+        theta = np.arctan2(sin_sum/flen, cos_sum/flen)
+    else:
+        x = x_sum
+        y = y_sum
+        theta = 0.0
+        
+    return Particle(x, y, theta)
+     
+     
+def move_particle(particle, delta_x, delta_y, delta_theta):
+    """Move the particle by (delta_x, delta_y, delta_theta)"""
+    print("particle.py: move_particle not implemented. You should do this.") 
+
+
+def add_uncertainty(particles_list, sigma, sigma_theta):
+    """Add some noise to each particle in the list. Sigma and sigma_theta is the noise
+    variances for position and angle noise."""
+    for particle in particles_list:
+        particle.x += rn.randn(0.0, sigma)
+        particle.y += rn.randn(0.0, sigma)
+        particle.theta = np.mod(particle.theta + rn.randn(0.0, sigma_theta), 2.0 * np.pi) 
+
+
+def add_uncertainty_von_mises(particles_list, sigma, theta_kappa):
+    """Add some noise to each particle in the list. Sigma and theta_kappa is the noise
+    variances for position and angle noise."""
+    for particle in particles_list:
+        particle.x += rn.randn(0.0, sigma)
+        particle.y += rn.randn(0.0, sigma)
+        particle.theta = np.mod(rn.rand_von_mises(particle.theta, theta_kappa), 2.0 * np.pi) - np.pi