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
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(particle.theta, sigma_theta),
2.0 * np.pi)
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
) # - np.pi # TODO: Is this correct? We need to enforce that angles are from -pi to pi
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)
new_theta = np.degrees(particle.theta) + np.random.normal(
0, sigma_theta)
if new_theta < -180.0:
particle.theta = np.radians(new_theta + 360.0)
elif new_theta >= 180.0:
particle.theta = np.radians(new_theta - 360.0)
else:
particle.theta = np.radians(new_theta)
def add_uncertainty(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