def cria_particulas(minx=0,
miny=0,
maxx=largura,
maxy=altura,
n_particulas=num_particulas):
"""
Cria uma lista de partículas distribuídas de forma uniforme entre minx, miny, maxx e maxy
"""
return create_particles(robot.pose())
def cria_particulas(minx=0,
miny=0,
maxx=largura,
maxy=altura,
n_particulas=num_particulas):
"""
Cria uma lista de partículas distribuídas de forma uniforme entre minx, miny, maxx e maxy
"""
var_x = (maxx - minx) / 2
var_y = (maxy - miny) / 2
var_theta = math.pi / 3
return create_particles(robot.pose(), var_x, var_y, var_theta,
n_particulas)
import pf as pf
import numpy as np
from numpy.linalg import norm
from filterpy.monte_carlo import systematic_resample
from matplotlib import pyplot as plt
N = 500 # number of particles
x_dim = (0, 20)
y_dim = (0, 20)
std = (.1, .1)
dt = 1 # time step
known_locations = np.array([[-1, 0], [2, 3], [-1, 15]])
# Test Parameters
num_of_iterations = 40
particles = pf.create_particles(N=N, x=x_dim, y=y_dim)
print('++++++++++++++++++++++++++++++++++++++++++++++++++++++++')
weights = np.zeros(N)
robot_position = np.array([0., 0.])
for iteration in range(num_of_iterations):
# Increment robot position
robot_position += (1, 1)
# Distance from robot to each known location
diff = known_locations - robot_position
observation = (norm(diff, axis=1))
pf.predict(particles=particles, std=std, dt=dt)
pf.update(particles=particles,
tri_predictions[i][0]=locate[0]
tri_predictions[i][1]=locate[1]
i += 1
#########################
#
# particle filter
# precition
#
#########################
n_particles = 50000
v_mean=10
v_std=5
tri_pf_predictions = np.empty(tri_predictions.shape)
particles = create_particles(v_mean, v_std, n_particles) # 初始化粒子
weights = np.ones(n_particles) / n_particles # 初始化权重
for i,pos in enumerate(tri_predictions):
pos = pos.copy()
state = run_pf(particles, weights, pos)
tri_pf_predictions[i, :] = state[0:2]
#########################
#
# print results
#
#########################
results=tri_pf_predictions
i=0