Пример #1
0
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
Пример #2
0
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)
Пример #3
0
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
Пример #4
0
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)
Пример #5
0
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