def initParticles(center, N):
global p_filter
global bounds
global var
print 'initialize', N, 'particles'
p_filter = ParticleFilter(N, center, var, bounds=bounds)
drawParticles()
drawRect(center)
plt.draw()
# Information displayed, to be sure software motor configuration matches reality
print("The configuration states that plugged motors are :\n")
for i in List_plugged_motors:
print("Port ", i[0], " for ", i[1], " side motor\n")
# Necessary for the instantiation of the Robot, empty for the moment
List_plugged_sensors = [[2, "SENSOR_ULTRASONIC", 2]]
# Create the instance of the robot, initialize the interface
Simple_robot = Robot(List_plugged_motors, List_plugged_sensors)
# Enable its motors and sensors
Simple_robot.motor_init()
Simple_robot.sensor_init()
# Create a ParticleFilter
PF = ParticleFilter(100)
# Start point coordinates in the arena, which are passed to the robot, and to the particle filters
start_point = [84., 30.]
Simple_robot.set_x(start_point[0])
Simple_robot.set_y(start_point[1])
PF.particles = np.array([[start_point[0], start_point[1], 0]] *
PF.num_particles)
# Lab spec states it should be 20 cm
step_size = 20
# Create a Map
mymap = Map()
self._auto_run()
else:
self._manual_run()
if __name__ == '__main__':
print 'Testing FilterManager...'
origin_pos = np.array([200, 0, np.pi/2])
origin_cov = np.eye(3) * 3 # Initial Covariance
origin_cov[2,2] = 0.02
# Need to call this or gtk will never release locks
gobject.threads_init()
fm = FilterManager()
run_kf = False
run_pf = False
#run_kf = True
run_pf = True
if run_kf:
fm.add_filter(KalmanFilter.KalmanFilter(origin_pos, origin_cov))
if run_pf:
fm.add_filter(ParticleFilter.ParticleFilter(origin_pos, origin_cov))
tg = TestGUI(fm.get_draw())
#tt = TestThread(fm, tg, auto=True)
tt = TestThread(fm, tg, auto=False)
tt.start()
tg.mainloop()
tt.quit = True
vstd = 0.9
hstd = 0.6
#Kalman Filter
xInit1 = 1
yInit1 = 1.3
xInit2 = 1
yInit2 = 0.7
initPosError1 = 0
initPosError2 = 0
sigmaSensor = 0.3
covarianceCoeff = 0.001
#Initialize Filters
if (FILTER == 1):
pf1 = ParticleFilter(500, 10, 10, anchors, sigma, vstd, hstd)
pf2 = ParticleFilter(500, 10, 10, anchors, sigma, vstd, hstd)
elif (FILTER == 2):
kalman1 = KalmanFilter(xInit1, yInit1, initPosError1, sigmaSensor,
covarianceCoeff)
kalman2 = KalmanFilter(xInit2, yInit2, initPosError2, sigmaSensor,
covarianceCoeff)
def loop():
global dt1, dt2, i
"""
if (i < 8):
trueX = 1
trueY = 1
orientation = 0
fusion_range = 5
# source_strength = 2.2e6 * 100 # 100 u Curies in CPM
source_strength = 100
min_particle_strength = 0.5 * source_strength
max_particle_strength = 1.5 * source_strength
# Initialize world
source_locations = world_size * np.random.rand(num_sources, 2)
source_strengths = source_strength * np.ones(num_sources)
sensor_location = np.array([[1., 2.]])
particle_filter = ParticleFilter.ParticleFilter(min_particle_strength,
max_particle_strength,
num_sources,
world_size=world_size,
num_particles=num_particles,
fusion_range=fusion_range)
# particle_filter.render(sensor_location, source_locations)
# plt.pause(1)
# Take a reading
for t in range(300):
source_dist_sq = distance.cdist(sensor_location, source_locations,
'sqeuclidean')
source_expected_intensity = source_strengths / (1 + source_dist_sq)
# reading = np
reading = np.round(np.sum(source_expected_intensity))
particle_filter.step(reading, sensor_location)
def resample(self):
weights = map(lambda p: p.getWeight(), self.particles)
self.particles = pf.ParticleFilter(self.particles, weights)
env = SingleBotLaser2Dgrid(bot_pos, bot_param, 'map_large.png')
# Initialize GridMap
# lo_occ, lo_free, lo_max, lo_min
map_param = [0.4, -0.4, 5.0, -5.0]
m = GridMap(map_param, gsize=1.0)
sensor_data = env.Sensor()
SensorMapping(m, env.bot_pos, env.bot_param, sensor_data)
img = Draw(env.img_map, 1, env.bot_pos, sensor_data, env.bot_param)
mimg = AdaptiveGetMap(m)
cv2.imshow('view',img)
cv2.imshow('map',mimg)
# Initialize Particle
pf = ParticleFilter(bot_pos.copy(), bot_param, copy.deepcopy(m), 10)
# Scan Matching Test
matching_m = GridMap(map_param, gsize=1.0)
SensorMapping(matching_m, env.bot_pos, env.bot_param, sensor_data)
matching_pos = np.array([150.0, 100.0, 0.0])
# Main Loop
while(1):
# Input Control
action = -1
k = cv2.waitKey(1)
if k==ord('w'):
action = 1
if k==ord('s'):
action = 2
'''
Created on Dec 18, 2014
@author: hcmi
'''
from ParticleFilter import *
from motionModels import *
import matplotlib.pyplot as plt
if __name__ == '__main__':
PARTICLE_NUM = 100
pf = ParticleFilter(PARTICLE_NUM, 2, 4, motionModel, sensorModel2)
pos = np.loadtxt('pos.txt')
n_obs1 = np.loadtxt('n_obs1.txt')
n_obs2 = np.loadtxt('n_obs2.txt')
DATA_LEN = n_obs1.shape[0]
fused_obs = np.hstack([n_obs1, n_obs2])
print fused_obs.shape
d = np.random.normal(loc=5, scale=1, size=DATA_LEN)
#theta = np.random.uniform(np.pi/5 - np.pi/36, np.pi/5 + np.pi/36, DATA_LEN)
theta = np.random.uniform(np.pi / 5 - np.pi / 10, np.pi / 5 + np.pi / 10,
DATA_LEN)
world_name = "localization_maze"
env_params = {"env_width": WIDTH, "env_height": HEIGHT}
world_generator = WorldGenerator(WIDTH, HEIGHT, 20, world_name,
scenario, collision)
if use_human_controller:
controller_func = HumanController
# Game loop
while True:
world, robot = world_generator.create_world(random_robot=False)
### TODO initialize stuff for particle filter
robo_pos = (robot.x, robot.y, robot.angle)
grid_world = Grid((WIDTH, HEIGHT), 1)
pf = ParticleFilter.ParticleFilter(deepcopy(list(robo_pos)),
grid_world)
# pf.particle_list[0].world.grid
###
controller = controller_func(robot, scenario)
env_params["world"] = world
env_params["robot"] = robot
env_params["robot_controller"] = controller
env_params["scenario"] = scenario
env_params["particle_filter"] = pf
env_params["debug"] = debug
game = MobileRobotGame(**env_params)
game.init()
game.run(args.snapshot, args.snapshot_dir)
def _set_filter(self, x_range, y_range, z_range, N): self.filter = ParticleFilter(x_range, y_range, z_range, N)
