def initParticle(self, accel, ori): X = [] particle = Particle() particle.initWithIMU(accel, ori) for i in range(self.M): X.append(particle) return X
def __init__(self, n, sprite):
self.particles = []
self.particleShape = createShape(PShape.GROUP)
for i in range(n):
p = Particle(sprite)
self.particles.append(p)
self.particleShape.addChild(p.getShape())
def _getPairEquivParticle(self, a, b) : result = Particle(a.mass + b.mass) ratio = a.mass / float(b.mass) result.location = getSegmentCut3d(a.location, b.location, ratio) result.velocity = add(a.velocity, b.velocity) return result
def createParticleFromStateVector(self, mu, sigma): X = [] for i in range(self.M): particle = Particle() particle.initWithStateVector(mu, sigma) X.append(particle) return X
def test_particle_dist__bad_particle2():
try:
p = Particle(1., 2., 2., 2., 2.)
p2 = "wrong"
p.dist(p2)
except TypeError:
pass
def test_Particle_speed_should_remain_constant():
almost_eq = approximately_eq(13)
assert_speed_unchanged = assert_evolution(lambda:abs(p.vel), almost_eq)
p = Particle(30, 40, 50, 60, 70)
for dt in xrange(10,30,1):
p.move(dt/1000.0)
assert_speed_unchanged()
def update_child_bests(self, particle, best_neighbor, child):
"""Update the pbest and nbest of a child particle from its parent.
In speculative execution this is necessary because the child
particle doesn't know if the value that it found is better than the
value the parent found at its position; it only updated its pbest in
relation to its previous pbestval.
Also, it doesn't know the value of the function at the nbest position
that it guessed. It has to get that from its parent.
So, this function passes along the iteration 1 nbestval that was
determined in pick_child and updates the iteration 2 pbest. The
child is at iteration 2 (minus nbest) after this method.
Best is passed in separately from particle to be sure that the state
of particle doesn't have to be changed in pick_child, because sometimes
it's just a message that you really don't want to change.
"""
comparator = self.specex.function.comparator
# Passing along the nbestval from iteration 1
if Particle.isbetter(best_neighbor.pbestval, child.nbestval,
comparator):
# In the ReproducePSO case this first line could be an assert, but
# not in the PickBestChild case.
child.nbestpos = best_neighbor.pbestpos
child.nbestval = best_neighbor.pbestval
# Set the pbest for iteration 2, and update the last branch
if Particle.isbetter(particle.pbestval, child.pbestval, comparator):
child.pbestpos = particle.pbestpos
child.pbestval = particle.pbestval
child.lastbranch[0] = False
else:
child.lastbranch[0] = True
def setImageData(self, time_, keypointPairs):
# If IMU data has not been arrived yet, do nothing
if(self.isFirstTimeIMU):
return
# Count
self.count+=1
########################
print("===================================")
print("step "+str(self.step))
###########################
# If first time, save mu and don't do anything else
if(self.isFirstTimeCamera):
self.isFirstTimeCamera = False
self.mu1 = copy.deepcopy(self.mu) # save mu[t] as mu[t-1]
self.t_camera = time_
self.step += 1
return
# Lock IMU process
self.lock = True
# Get current time
self.t1 = self.t
self.t = time_
self.dt = self.t - self.t1
self.t1_camera = self.t_camera
self.t_camera = time_
dt_camera = self.t_camera - self.t1_camera
# create particle from state vector
self.X = self.createParticleFromStateVector(self.mu, self.sigma)
# create X1 from mu[t-1]
X1 = Particle()
X1.initWithMu(self.mu1)
self.saveXYZasCSV(self.X,"1") ##############################
# exec particle filter
self.X = self.pf.pf_step(self.X, X1, self.dt, dt_camera, keypointPairs, self.M)
self.saveXYZasCSV(self.X,"2") ##############################
# create state vector from particle set
self.mu, self.sigma = self.createStateVectorFromParticle(self.X)
# save mu[t] as mu[t-1]
self.mu1 = copy.deepcopy(self.mu)
# Step (camera only observation step)
self.step += 1
# Unlock IMU process
self.lock = False
def test_particle_update():
p = Particle(1., 2., 3., 2., 2.)
dvx, dvy, h = -2.0, 2.0, 1.0
p.update(dvx, dvy, h)
assert p.x == 4.0
assert p.y == 5.0
assert p.vx == 0.0
assert p.vy == 4.0
def test_particle_update_pos():
p = Particle(1., 2., 3., 2., 2.)
p.update_pos(1.0)
assert p.x == 4.0
assert p.y == 5.0
p.update_pos(2.0)
assert p.x == 8.0
assert p.y == 9.0
def test_particle_copy():
p = Particle(1., 2., 3., 2., 2.)
p2 = p.copy()
assert p.m == p2.m
assert p.x == p2.x
assert p.y == p2.y
assert p.vx == p2.vx
assert p.vy == p2.vy
assert p != p2
def initialise(self):
"""
Generate the reference particle and the comparison particles.
"""
self.reference_particle = Particle.factory()
self.particles = [ Particle.factory()
for i in range(0, self.num_particles) ]
self.compute_distances()
self.iteration = 1
def init_particles(n_particles, min_, max_, dimensions):
"""
Declaring the particles.
"""
for _ in range(n_particles):
new_particle = Particle()
val_ = random.sample(range(min_, max_), dimensions)
p_best = objective(val_)
new_particle.set_vals(val_)
new_particle.set_pbest(p_best)
particles.append(new_particle)
def __init__(self, n):
# It's just a list of particle objects.
self.particles = []
# The PShape to group all the particle PShapes.
self.particleShape = createShape(GROUP)
# Make all the Particles.
sprite = loadImage("sprite.png")
for i in range(n):
p = Particle(sprite)
self.particles.append(p)
# Each particle's PShape gets added to the System PShape.
self.particleShape.addChild(p.getShape())
def populate(n):
global bang_force
#I support the big bang theory.
particles = []
for i in range(n):
pos = vec2d((randrange(0,501)*1.00),(randrange(0,501)*1.00))
dir_to_center = vec2d((pos.x-250), (pos.y-250)).normalized()
bang_dir = dir_to_center * bang_force
particle = Particle(pos)
particle.addVec(bang_dir)
particles.append(particle)
particle = None
return particles
def add_particle(self, gridcoord, particle_specs, body_specs):
""" Add a new particle to the model with the given ParticleSpecs and BodySpecs objects.
If there is already a particle at this coordinate, the existing particle's particle
and body types are set to the given ParticleSpecs and BodySpecs objects. """
if not self.has_particle(gridcoord):
particle = Particle(gridcoord, particle_specs, body_specs)
self.gridcoord_to_particle[gridcoord] = particle
self._particles.add(particle)
else:
particle = self.gridcoord_to_particle[gridcoord]
particle.particle_specs = particle_specs
particle.body_specs = body_specs
return particle
def test_particle_dist():
p = Particle(1., 2., 2., 2., 2.)
p2 = Particle(1., 5., 6., 2., 2.)
p3 = Particle(1., 2., 6., 2., 2.)
assert p.dist(p2) == 5.0
assert p2.dist(p) == 5.0
assert p.dist(p3) == 4.0
assert p3.dist(p) == 4.0
assert p2.dist(p3) == 3.0
assert p3.dist(p2) == 3.0
def resample_particles(self):
'''Performs an importance particle resampling step based on the current particle weights.
'''
number_resampled_particles = self.filter_params.number_of_particles - self.filter_params.number_of_random_particles
step = 1. / number_resampled_particles
start_weight = uniform(0, step)
current_particle = 0
current_weight = self.particles[0].weight
weight_sum = 0.
#we add random particles to the particle set
new_particles = list()
for i in xrange(self.filter_params.number_of_random_particles):
particle = Particle.random_particle(self.filter_params.neg_x_limit, self.filter_params.x_limit, self.filter_params.neg_y_limit, self.filter_params.y_limit, 1/self.filter_params.number_of_particles)
weight_sum = weight_sum + particle.weight
new_particles.append(particle)
#we resample particles using low variance resampling
for i in xrange(number_resampled_particles):
weight = start_weight + i * step
while weight > current_weight:
current_particle = current_particle + 1
current_weight = current_weight + self.particles[current_particle].weight
particle = deepcopy(self.particles[current_particle])
weight_sum = weight_sum + particle.weight
new_particles.append(particle)
for i in xrange(self.filter_params.number_of_particles):
new_particles[i].weight = new_particles[i].weight / weight_sum
return new_particles
def predictionAndUpdateOneParticle_firsttime(self, X, dt, dt2, keypoints, step, P):
"""
FastSLAM1.0
"""
weight = 0.0 # weight (return value)
weights = []
# 姿勢予測 prediction of position
X_ = Particle()
X_.landmarks = X.landmarks
X_.x = X.x + dt*X.v + dt2*X.a
X_.v = X.v + dt*X.a
X_.a = X.a
X_.o = X.o
for keypoint in keypoints:
# previous landmark id
prevLandmarkId = (step-1)*10000 + keypoint.prevIndex
# new landmark id
landmarkId = step*10000 + keypoint.index
# The landmark is already observed or not?
if(X_.landmarks.has_key(prevLandmarkId) == False):
# Fisrt observation
# Initialize landmark and append to particle
landmark = Landmark()
landmark.init(X_, keypoint, P, self.focus)
X_.landmarks[landmarkId] = landmark
else:
print("Error on pf_step_camera_firsttime")
self.count+=1
return X_, weight
def reinit_particle(self):
"""
Re initialize the particle with new values
:return:
"""
if self.check_nuclide_validity():
# Here make a particle
zz = self.spinBox_zz.value()
nn = self.spinBox_nn.value()
self.particle = Particle(zz, nn, self.ame_data, self.ring_dict[self.current_ring_name])
self.particle.qq = self.spinBox_qq.value()
self.particle.ke_u = self.doubleSpinBox_energy.value()
self.particle.i_beam_uA = self.doubleSpinBox_beam_current.value()
self.particle.f_analysis_mhz = self.doubleSpinBox_f_analysis.value()
if not self.checkBox_circum.isChecked():
self.particle.path_length_m = self.doubleSpinBox_path_length.value()
else:
self.particle.path_length_m = self.ring_dict[self.current_ring_name].circumference
class ParticleFilterSimulatedRobot:
def __init__(self, world, sensordirections, x, y, orientation):
self._robot = Particle(world, sensordirections)
self._robot._set(x, y, orientation)
self._robot.set_noise(0., 0., 0.) # TODO: use realistic values
self._world = world
self._image = pygame.image.load("assets/robot.png").convert_alpha()
def connect_ultrasone(self, ports=['in2:i2c1'], offsets=[0]):
pass
def connect_ir(self, ports=['in1'], offsets=[0]):
pass
def turn(self, rad):
# calc ticks
self._robot = self._robot.move(rad, 0)
self.draw()
def turn_right(self, rad=0.5*math.pi):
self.turn(rad)
def turn_left(self, rad=0.5*math.pi):
self.turn(-rad)
def drive(self, dist=0.10):
self._robot = self._robot.move(0, dist)
self.draw()
def sense_ultrasone(self):
return self._robot._sense(self._world.screen, True)
def sense_ir(self):
return self._robot._sense(self._world.screen)
def draw(self):
rot_image = pygame.transform.rotate(self._image, float(-self._robot.orientation - (0.5*math.pi)) / (2.*math.pi) * 360)
[x, y] = self._world.scale_point(self._robot.x,self._robot.y)
self._world.screen.blit(rot_image, (x-25, y-22))
def dogfight(self, threats, dt):
if self.current_dogfight_target == None or self.current_dogfight_target.health <= 0:
self.current_dogfight_target = random.choice(threats)
dist = (self.current_dogfight_target.pos - self.pos).sqr_magnitude()
if dist < RANGE ** 2:
self.assault(self.current_dogfight_target, dt)
else:
towards = (self.current_dogfight_target.pos - self.pos).normalized()
target_vector = towards
target_vector += self.get_fleet_target_vector()
target_vector = target_vector.normalized()
self.turn_towards(target_vector, dt)
self.speed_t += dt
speed = math.sin(self.speed_t) * 2 + self.base_speed
speed *= self.owning_civ.upgrade_stats['move_speed'] + 1
self.velocity = V2.from_angle(self._angle) * speed
self.thrust_particle_time += dt
if self.thrust_particle_time > THRUST_PARTICLE_RATE:
pvel = V2(random.random() - 0.5, random.random() - 0.5) * 5
pvel += -self.velocity / 2
p = Particle("assets/thrustparticle.png",1,self.pos,1,pvel)
self.scene.game_group.add(p)
self.thrust_particle_time -= THRUST_PARTICLE_RATE
def motion_update(particles, odom):
""" Particle filter motion update
Arguments:
particles -- input list of particle represents belief p(x_{t-1} | u_{t-1})
before motion update
odom -- odometry to move (dx, dy, dh) in *robot local frame*
Returns: the list of particles represents belief \tilde{p}(x_{t} | u_{t})
after motion update
references: https://github.com/mxie33/CS3630-robotics/tree/1c0d8c1e92d81e7f74f1930b9d2c64a35aab0062
"""
motion_particles = []
for prt in particles:
odomGaussNoiseApplied = add_odometry_noise(odom, ODOM_HEAD_SIGMA,
ODOM_TRANS_SIGMA)
dx = odomGaussNoiseApplied[0]
dy = odomGaussNoiseApplied[1]
dh = odomGaussNoiseApplied[2]
xRotatedH, yRotatedH = rotate_point(dx, dy, prt.h)
newY = prt.y + yRotatedH
newX = prt.x + xRotatedH
newH = prt.h + dh
motion_particles.append(Particle(newX, newY, newH))
return motion_particles
def motion_update(particles, odom):
""" Particle filter motion update
Arguments:
particles -- input list of particle represents belief p(x_{t-1} | u_{t-1})
before motion update
odom -- odometry to move (dx, dy, dh) in *robot local frame*
Returns: the list of particles represents belief \tilde{p}(x_{t} | u_{t})
after motion update
"""
# ---------- Resample particles with motion model ----------
motion_particles = []
for p in particles:
sample_dx = odom[0] + random.gauss(0, ODOM_TRANS_SIGMA)
sample_dy = odom[1] + random.gauss(0, ODOM_TRANS_SIGMA)
sample_dh = odom[2] + random.gauss(0, ODOM_HEAD_SIGMA)
psample_dx, psample_dy = rotate_point(sample_dx, sample_dy, p.h)
psample = Particle(p.x + psample_dx, p.y + psample_dy, p.h + sample_dh)
motion_particles.append(psample)
return motion_particles
def motion_update(particles, odom):
""" Particle filter motion update
Arguments:
particles -- input list of particle represents belief p(x_{t-1} | u_{t-1})
before motion update
odom -- odometry to move (dx, dy, dh) in *robot local frame*
Returns: the list of particles represents belief \tilde{p}(x_{t} | u_{t})
after motion update
"""
motion_particles = []
dx, dy, dh = odom
for particle in particles:
part_x, part_y, part_h = particle.xyh
h = add_gaussian_noise(part_h + dh, ODOM_HEAD_SIGMA)
dx_new, dy_new = rotate_point(add_gaussian_noise(dx, ODOM_TRANS_SIGMA), add_gaussian_noise(dy, ODOM_TRANS_SIGMA), h)
motion_particles.append(Particle(part_x + dx_new, part_y + dy_new, h))
return motion_particles
def q_one_one():
c1 = c2 = 2.05
swarm = [Particle(x_y_range, x_y_range) for i in range(100)]
velocity_position_fn = simple_velocity(x_y_range, x_y_range, c1, c2)
avg_fit, best_fit, g_best = simulation(
termination_condition=termination_condition(iterations),
swarm=swarm,
fitness_function=six_hump_camelback,
velocity_position_update=velocity_position_fn)
test = "Simple PSO"
title = f"Fitness vs Iterations: {test}"
data = [("Average Fitness", avg_fit)]
data.append(("Best Fitness", best_fit))
graph(title, data)
print(test)
print(f"\t c1: {c1}")
print(f"\t c2: {c2}")
print(f"\t iterations: {iterations}")
print(f"\t particles: {particle_count}")
print(f"\t best fitness: {six_hump_camelback(g_best[0],g_best[1])}")
print(f"\t best x,y: {g_best[0]} {g_best[1]}")
def main():
# Keep track of global best solution and its index within the swarm
gBest = 100000
gBestIndex = 0
# Make the swarm - create a list of Particles of size NUM_PARTICLES
swarm = [Particle(0,100,50) for i in range(NUM_PARTICLES)]
# Set the loop counter and stopping condition
generations = 0
converged = False
while(generations < MAX_GENERATIONS and (not converged)):
# Calculate the fitness values for the swarm and check if we have a solution
for i in range(NUM_PARTICLES):
swarm[i].calculateFitness()
converged = converged or (swarm[i].fitness == 0)
# Print some output at this point
print "[] Generation: " + str(generations)
for i in range(NUM_PARTICLES):
print swarm[i]
# Set the global best based on the swarm at this point
for i in range(NUM_PARTICLES):
if (swarm[i].pBest < gBest):
gBest = swarm[i].pBest
gBestIndex = i
# Update all but the global best particle using Eberhart & Kennedy's equation
for i in range(NUM_PARTICLES):
if (i != gBestIndex):
swarm[i].update(gBest)
# Set the next time step
generations = generations + 1
def motion_update(particles, odom):
""" Particle filter motion update
Arguments:
particles -- input list of particle represents belief p(x_{t-1} | u_{t-1})
before motion update
odom -- odometry to move (dx, dy, dh) in *robot local frame*
Returns: the list of particles represents belief \tilde{p}(x_{t} | u_{t})
after motion update
"""
motion_particles = []
for particle in particles:
x, y, h = particle.xyh
dx, dy, dh = odom
c, d = rotate_point(dx, dy, h)
nx, ny, nh = add_odometry_noise((x + c, y + d, h + dh),
heading_sigma=ODOM_HEAD_SIGMA,
trans_sigma=ODOM_TRANS_SIGMA)
newParticle = Particle(nx, ny, nh % 360)
motion_particles.append(newParticle)
return motion_particles
def f_IMU(self, X, dt, dt2, accel, ori, noise): """ Transition model - 状態方程式 x_t = f(x_t-1, u) + w w ~ N(0, sigma) """ X_new = Particle() X_new.landmarks = X.landmarks # Transition with noise (only x,v) X_new.x = X.x + dt*X.v + dt2*X.a + noise X_new.v = X.v + dt*X.a #X_new.v = X.v + dt*X.a + noise*25 #X_new.v = X.v + dt*X.a + noise*50 X_new.a = accel X_new.o = ori return X_new
threading.Thread.__init__(self, daemon=True)
self.filter = particle_filter
self.gui = gui
def run(self):
while True:
estimated = self.filter.update()
self.gui.show_particles(self.filter.particles)
self.gui.show_mean(estimated[0], estimated[1], estimated[2],
estimated[3])
self.gui.show_robot(self.filter.robbie)
self.gui.updated.set()
if __name__ == "__main__":
grid = CozGrid(Map_filename)
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid)
if Use_GUI:
gui = GUIWindow(grid)
filter_thread = ParticleFilterThread(particlefilter, gui)
filter_thread.start()
gui.start()
else:
while True:
particlefilter.update()
def init_molecule():
myParticle1 = Particle(np.array([0.2, 0.2]), 1)
myParticle2 = Particle(np.array([0.8, 0.8]), 2)
myMolecule = Molecule(myParticle1, myParticle2, 1, 0.5)
return myMolecule
def __init__(self, n, sprite):
self.particleShape = createShape(PShape.GROUP)
self.particles = [Particle(sprite) for _ in range(n)]
for p in self.particles:
self.particleShape.addChild(p.getShape())
precisions = []
times = []
for executions in range(0, 100):
print(str(executions) + "%")
start_time = time.time()
particles = []
k_means = KMeans()
# Particles Initialization
for i in range(population_size):
p = Particle()
num_clusters = random.randint(2, 7)
plist = [num_clusters, ]
plist.extend([random.uniform(0, 10) for i in range(0, k_means.dimens * 7)])
p.current_position = array(plist)
p.best_position = p.current_position
p.fitness = 0.0
p.velocity = 0.0
particles.append(p)
i = 0
global_best = Particle()
def predictionAndUpdateOneParticle(self, X, dt, dt2, noise, keypoints, step, P, X1, P1, dt_camera, noise_o):
"""
FastSLAM1.0
"""
weight = 0.0 # weight (return value)
weights = []
count_of_first_observation = 0
# 姿勢予測 prediction of position
X_ = Particle()
X_.landmarks = X.landmarks
X_.x = X.x + dt*X.v + dt2*X.a + noise
#X_.v = X.v + dt*X.a
#X_.v = X.v + dt*X.a + noise*25
#X_.v = X.v + dt*X.a + noise*50
X_.a = X.a
X_.o = X.o
#X_.o = X.o + noise_o
# 速度調整 velocity adjustment
#X_.v = ((X_.x - X1.x)/dt_camera)
X_.v = ((X_.x - X1.x)/dt_camera)*0.25
#X_.v = ((X_.x - X1.x)/dt_camera)*0.5
# 共面条件モデルのための計算 Calc for Coplanarity
xyz = np.array([X_.x[0] - X1.x[0], X_.x[1] - X1.x[1], X_.x[2] - X1.x[2]])
# 前回ランドマークの全てのキーを取得しておく あとで消す
prevLandmarkKeys = []
for key in X_.landmarks:
prevLandmarkKeys.append(key)
for keypoint in keypoints:
# ---------------------------- #
# Calc weight of Inverse Depth #
# ---------------------------- #
# previous landmark id
prevLandmarkId = (step-1)*10000 + keypoint.prevIndex
# new landmark id
landmarkId = step*10000 + keypoint.index
# The landmark is already observed or not?
if(X_.landmarks.has_key(prevLandmarkId) == False):
# Fisrt observation
# Initialize landmark and append to particle
landmark = Landmark()
landmark.initPrev(X1, keypoint, P1, self.focus)
X_.landmarks[landmarkId] = landmark
if(self.count == 0):
count_of_first_observation += 1
else:
# Already observed
X_.landmarks[landmarkId] = X_.landmarks[prevLandmarkId]
#del X_.landmarks[prevLandmarkId]
# Update landmark and calc weight
# Observation z
z = np.array([keypoint.x, keypoint.y])
# Calc h and H (Jacobian matrix of h)
h, Hx, H = X_.landmarks[landmarkId].calcObservation(X_, self.focus)
# Kalman filter (Landmark update)
S = H.dot(X_.landmarks[landmarkId].sigma.dot(H.T)) + self.R
Sinv = np.linalg.inv(S)
K = X_.landmarks[landmarkId].sigma.dot(H.T.dot(Sinv))
X_.landmarks[landmarkId].mu += K.dot(z - h)
X_.landmarks[landmarkId].sigma = X_.landmarks[landmarkId].sigma - K.dot(H.dot(X_.landmarks[landmarkId].sigma))
# Calc weight
w = 0.0
try:
#w = (1.0 / (math.sqrt( np.linalg.det(2.0 * math.pi * S) ))) * np.exp( -0.5 * ( (z-h).T.dot(Sinv.dot(z-h)) ) )
#w = (1.0 / (math.sqrt( np.linalg.det(2.0 * math.pi * self.R) ))) * np.exp( -0.5 * ( (z-h).T.dot(self.Rinv.dot(z-h)) ) )
# log likelihood
#w = np.log(1.0 / (math.sqrt( np.linalg.det(2.0 * math.pi * self.R) ))) + ( -0.5 * ( (z-h).T.dot(self.Rinv.dot(z-h)) ) )
w = ( -0.5 * ( (z-h).T.dot(self.Rinv.dot(z-h)) ) )
except:
print("Error on calc inverse weight ******")
w = 0.0
# -------------------------- #
# Calc weight of Coplanarity #
# -------------------------- #
# Generate uvw1 (time:t-1)
uvf1 = np.array([keypoint.x1, -keypoint.y1, -self.focus]) # Camera coordinates -> Device coordinates
rotX = Util.rotationMatrixX(X1.o[0])
rotY = Util.rotationMatrixY(X1.o[1])
rotZ = Util.rotationMatrixZ(X1.o[2])
# uvw1 = R(z)R(y)R(x)uvf1
uvw1 = np.dot(rotZ,np.dot(rotY,np.dot(rotX,uvf1)))
uvw1 /= 100.0 # Adjust scale to decrease calculation error. This doesn't have an influence to estimation.
# Generate uvw2 (time:t)
uvf2 = np.array([keypoint.x, -keypoint.y, -self.focus]) # Camera coordinates -> Device coordinates
rotX = Util.rotationMatrixX(X_.o[0])
rotY = Util.rotationMatrixY(X_.o[1])
rotZ = Util.rotationMatrixZ(X_.o[2])
# uvw2 = R(z)R(y)R(x)uvf2
uvw2 = np.dot(rotZ,np.dot(rotY,np.dot(rotX,uvf2)))
uvw2 /= 100.0 # Adjust scale to decrease calculation error. This doesn't have an influence to estimation.
# Generate coplanarity matrix
coplanarity_matrix = np.array([xyz,uvw1,uvw2])
# Calc determinant
determinant = np.linalg.det(coplanarity_matrix)
# Weight
w_coplanarity = 0.0
try:
#w_coplanarity = (1.0 / (math.sqrt( 2.0 * math.pi * self.noise_coplanarity**2 ))) * np.exp((determinant**2) / (-2.0 * (self.noise_coplanarity**2)) )
# log likelihood
#w_coplanarity = np.log(1.0 / (math.sqrt( 2.0 * math.pi * self.noise_coplanarity**2 ))) + ((determinant**2) / (-2.0 * (self.noise_coplanarity**2)) )
w_coplanarity = ((determinant**2) / (-2.0 * (self.noise_coplanarity**2)) )
except:
print("Error on calc coplanarity weight ******")
w_coplanarity = 0.0
# --------------------------- #
# inverse depth * coplanarity #
# --------------------------- #
if(self.count == 0):
#print(w),
#print(w_coplanarity)
pass
w = w + w_coplanarity # use inverse depth * coplanarity log likelihood
#w = w_coplanarity # use only coplanarity log likelihood
#w = w # use only inverse depth log likelihood
weights.append(w)
# ----------------------------------- #
# sum log likelihood of all keypoints #
# ----------------------------------- #
for i,w in enumerate(weights):
if(i==0):
weight = w
else:
weight += w
# ----------------------------- #
# Average of log likelihood sum #
# ----------------------------- #
weight /= float(len(weights))
###############################
#print("weight "+str(weight))
if(self.count == 0):
#print("weight "+str(weight))
#print("first_ratio "+str(float(count_of_first_observation)/float(len(keypoints)))),
#print("("+str(count_of_first_observation)+"/"+str(len(keypoints))+")")
pass
###########################
# 前回ランドマークをすべて消す
for key in prevLandmarkKeys:
del X_.landmarks[key]
self.count+=1
return X_, weight
from particle import Particle
if __name__ == '__main__':
alpha = [0.1, 0.1]
n_particle = 20
x, y = 9, 3
lengthLim = [[2 * x / 5, x], [2 * y / 11, y]]
iter_ = 0
maxIter_ = 200
minAvgFitDiff = 0.05
minAvgPosiDiff = 0.075
particles = [
Particle([
rand.uniform(lengthLim[0][0], lengthLim[0][1]),
rand.uniform(lengthLim[1][0], lengthLim[1][1])
]) for _ in range(n_particle)
]
# initial global best position is position of first particle
globBestPosi = particles[0].posi
fig = plt.figure()
plot = fig.add_subplot(111, projection='3d')
s = plotGraph(plot, particles, x, y, lengthLim)
while iter_ < maxIter_ and calAvgFitDiff(
x, y, particles) > minAvgFitDiff and calAvgPosiDiff(
x, y, particles) > minAvgPosiDiff:
for p in particles:
p.updateBest(x, y)
globBestPosi = compFit(globBestPosi, p.posi, x, y)
def add_particle(self):
self.particles.append(Particle(self.origin))
def measurement_update(particles, measured_marker_list, grid):
""" Particle filter measurement update
Arguments:
particles -- input list of particle represents belief \tilde{p}(x_{t} | u_{t})
before meansurement update (but after motion update)
measured_marker_list -- robot detected marker list, each marker has format:
measured_marker_list[i] = (rx, ry, rh)
rx -- marker's relative X coordinate in robot's frame
ry -- marker's relative Y coordinate in robot's frame
rh -- marker's relative heading in robot's frame, in degree
* Note that the robot can only see markers which is in its camera field of view,
which is defined by ROBOT_CAMERA_FOV_DEG in setting.py
* Note that the robot can see mutliple markers at once, and may not see any one
grid -- grid world map, which contains the marker information,
see grid.py and CozGrid for definition
Can be used to evaluate particles
Returns: the list of particles represents belief p(x_{t} | u_{t})
after measurement update
"""
measured_particles = []
if len(measured_marker_list
) <= 0: # If there are no markers seen, do not update measurement
return particles
weights = []
for particle in particles:
# If the particle is not on the map, give it a weight of zero
if not grid.is_in(particle.x, particle.y) or not grid.is_free(
particle.x, particle.y):
weights.append(0)
continue
probability = 1.0
# If there are no markers in the secondary list, but they are in actual measurement, do not use this particle
secondary_marker_list = particle.read_markers(grid)
if len(secondary_marker_list) <= 0:
weights.append(0)
continue
# Pair observarble markets by distance
for measured_marker in measured_marker_list:
highest_probability = -0.1
max_probability_pair = None
# Current Particle observes fewer markers than the robot
if len(secondary_marker_list) <= 0:
break
for observed_marker in secondary_marker_list:
distance_between_markers = math.sqrt(
measured_marker[0]**2 +
measured_marker[1]**2) - math.sqrt(observed_marker[0]**2 +
observed_marker[1]**2)
angle_between_markers = diff_heading_deg(
measured_marker[2], observed_marker[2])
current_probability = np.exp(-(distance_between_markers**2) /
(2 * MARKER_TRANS_SIGMA**2) -
(angle_between_markers**2) /
(2 * MARKER_ROT_SIGMA**2))
if current_probability > highest_probability:
highest_probability = current_probability
max_probability_pair = observed_marker
if max_probability_pair != None:
secondary_marker_list.remove(max_probability_pair)
probability *= highest_probability
weights.append(probability)
# Normalize all current weights
weights = np.divide(weights, np.sum(weights))
# Resample all particles, 3% random noise
measured_particles = np.random.choice(particles,
size=PARTICLE_COUNT -
int(PARTICLE_COUNT * 0.03),
replace=True,
p=weights)
# Generate random noise to help with the kidnapped robot problem, 3% of actual number of particles
measured_particles = np.ndarray.tolist(measured_particles) \
+ Particle.create_random(int(PARTICLE_COUNT*0.03), grid)
return measured_particles
def measurement_update(particles, measured_marker_list, grid):
""" Particle filter measurement update
Arguments:
particles -- input list of particle represents belief \tilde{p}(x_{t} | u_{t})
before meansurement update (but after motion update)
measured_marker_list -- robot detected marker list, each marker has format:
measured_marker_list[i] = (rx, ry, rh)
rx -- marker's relative X coordinate in robot's frame
ry -- marker's relative Y coordinate in robot's frame
rh -- marker's relative heading in robot's frame, in degree
* Note that the robot can only see markers which is in its camera field of view,
which is defined by ROBOT_CAMERA_FOV_DEG in setting.py
* Note that the robot can see mutliple markers at once, and may not see any one
grid -- grid world map, which contains the marker information,
see grid.py and CozGrid for definition
Can be used to evaluate particles
Returns: the list of particles represents belief p(x_{t} | u_{t})
after measurement update
"""
num_random_sample = 25
measured_particles = []
weight = []
if len(measured_marker_list) > 0:
for particle in particles:
x, y = particle.xy
if grid.is_in(x, y) and grid.is_free(x, y):
markers_visible_to_particle = particle.read_markers(grid)
markers_visible_to_robot = measured_marker_list.copy()
marker_pairs = []
while len(markers_visible_to_particle) > 0 and len(markers_visible_to_robot) > 0:
all_pairs = product(markers_visible_to_particle, markers_visible_to_robot)
pm, rm = min(all_pairs, key=lambda p: grid_distance(p[0][0], p[0][1], p[1][0], p[1][1]))
marker_pairs.append((pm, rm))
markers_visible_to_particle.remove(pm)
markers_visible_to_robot.remove(rm)
prob = 1.
for pm, rm in marker_pairs:
d = grid_distance(pm[0], pm[1], rm[0], rm[1])
h = diff_heading_deg(pm[2], rm[2])
exp1 = (d**2)/(2*setting.MARKER_TRANS_SIGMA**2)
exp2 = (h**2)/(2*setting.MARKER_ROT_SIGMA**2)
likelihood = math.exp(-(exp1+exp2))
# The line is the key to this greedy algorithm
# prob *= likelihood
print(setting.DETECTION_FAILURE_RATE)
prob *= max(likelihood, setting.DETECTION_FAILURE_RATE*setting.SPURIOUS_DETECTION_RATE)
# In this case, likelihood is automatically 0, and max(0, DETECTION_FAILURE_RATE) = DETECTION_FAILURE_RATE
prob *= (setting.DETECTION_FAILURE_RATE**len(markers_visible_to_particle))
# Probability for the extra robot observation to all be spurious
prob *= (setting.SPURIOUS_DETECTION_RATE**len(markers_visible_to_robot))
weight.append(prob)
else:
weight.append(0.)
else:
weight = [1.]*len(particles)
norm = float(sum(weight))
if norm != 0:
weight = [i/norm for i in weight]
measured_particles = Particle.create_random(num_random_sample, grid)
measured_particles += np.random.choice(particles, setting.PARTICLE_COUNT-num_random_sample, p=weight).tolist()
else:
measured_particles = Particle.create_random(setting.PARTICLE_COUNT, grid)
return measured_particles
def pname(lbl: str, name: str) -> (GenParticle, Particle):
""" GenParticle and Particle from particle label """
p = Particle.findall(lbl)[0]
return (GenParticle(name, p.mass), p)
def __init__(self, world, sensordirections, x, y, orientation):
self._robot = Particle(world, sensordirections)
self._robot._set(x, y, orientation)
self._robot.set_noise(0., 0., 0.) # TODO: use realistic values
self._world = world
self._image = pygame.image.load("assets/robot.png").convert_alpha()
def test_particle_update__bad_dvy():
try:
p = Particle(1., 2., 2., 2., 2.)
p.update(-2.0, 2.0, "lol")
except TypeError:
pass
def measurement_update(particles, measured_marker_list, grid):
""" Particle filter measurement update
Arguments:
particles -- input list of particle represents belief \tilde{p}(x_{t} | u_{t})
before meansurement update (but after motion update)
measured_marker_list -- robot detected marker list, each marker has format:
measured_marker_list[i] = (rx, ry, rh)
rx -- marker's relative X coordinate in robot's frame
ry -- marker's relative Y coordinate in robot's frame
rh -- marker's relative heading in robot's frame, in degree
* Note that the robot can only see markers which is in its camera field of view,
which is defined by ROBOT_CAMERA_FOV_DEG in setting.py
* Note that the robot can see mutliple markers at once, and may not see any one
grid -- grid world map, which contains the marker information,
see grid.py and CozGrid for definition
Can be used to evaluate particles
Returns: the list of particles represents belief p(x_{t} | u_{t})
after measurement update
"""
weights = [1.0 for _ in range(len(particles))]
for i, particle in enumerate(particles):
if not grid.is_in(particle.x, particle.y) or not grid.is_free(
particle.x, particle.y):
weights[i] = 0.0
continue
particle_markers = particle.read_markers(grid)
for robot_marker in measured_marker_list:
new_robot_marker = add_marker_measurement_noise(
robot_marker, MARKER_TRANS_SIGMA, MARKER_ROT_SIGMA)
if len(particle_markers) > 0:
min_distance, min_angle, min_particle_marker = closest_particle_marker(
particle_markers, new_robot_marker)
particle_markers.remove(min_particle_marker)
exponent = ((min_distance ** 2) / (2 * (MARKER_TRANS_SIGMA ** 2))) + \
((min_angle ** 2) / (2 * (MARKER_ROT_SIGMA ** 2)))
weights[i] *= numpy.exp(-exponent)
else:
weights[i] *= DETECTION_FAILURE_RATE
for _ in particle_markers:
weights[i] *= SPURIOUS_DETECTION_RATE
weights[i] = max(weights[i],
SPURIOUS_DETECTION_RATE * DETECTION_FAILURE_RATE)
weight_sum = sum(weights)
if weight_sum == 0:
weights = [1.0 / len(particles) for _ in range(len(particles))]
else:
for i in range(len(weights)):
weights[i] /= weight_sum
random_particles = Particle.create_random(250, grid)
chosen_particles = numpy.random.choice(
particles,
len(particles) - len(random_particles), True, weights)
measured_particles = numpy.concatenate(
(chosen_particles, random_particles))
return measured_particles
def test_particle_update__h_lt_0():
try:
p = Particle(1., 2., 2., 2., 2.)
p.update(-2.0, 2.0, -1.0)
except ValueError:
pass
tofill[k] = np.nan
tofill['min_bq_pt'] = min([ip.pt()
for ip in bq]) if len(bq) else np.nan
tofill['max_bq_eta'] = max([abs(ip.eta())
for ip in bq]) if len(bq) else np.nan
tofill['n_jpsi'] = len(jpsis)
daus = [
jpsi.daughter(idau).pdgId()
for idau in range(jpsi.numberOfDaughters())
]
if verbose:
print('\t%s %s pt %3.2f,\t genealogy: ' %
(Particle.from_pdgid(jpsi.pdgId()), str(daus), jpsi.pt()),
end='')
ancestors = []
if verbose: print('\t', printAncestors(jpsi, ancestors, verbose=True))
else: printAncestors(jpsi, ancestors, verbose=False)
# only save jpsi->mumu
if sum([abs(dau) == 13 for dau in daus]) < 2: continue
if len(ancestors) == 0: continue
first_ancestor = ancestors[-1]
if first_ancestor.pdgId() in diquarks:
first_ancestor = ancestors[-2]
# compute the distance between primary and secondary vtx
def task_b():
p1 = Particle(Vec3(-1, 1, 0.0), Vec3(0, 0, 0.0), 5.0)
p2 = Particle(Vec3(3, 1, 0.0), Vec3(0, 0, 0.0), 4.0)
p3 = Particle(Vec3(-1.0, -2.0, 0.0), Vec3(0, 0, 0.0), 3.0)
# This particle setup was calculated by hand
n_body_system = NParticleSimulation([p1, p2, p3])
n_steps = 100000
h = 0.0001
# in unserem Beispile ist e_kin_alt natürlich 0
e_kin_alt = energie_kin([p.mass for p in n_body_system.particles], [p.velocity for p in n_body_system.particles])
dist1 = [distance(p1.position, p2.position)]
dist2 = [distance(p2.position, p3.position)]
dist3 = [distance(p1.position, p3.position)]
positions = [[p.position for p in n_body_system.particles]]
# Berechnung des totalen Energiefehlers mit Hilfe der beiden Energie Funktionen
energy_tot = [energie_pot(positions[0], positions[0], [p1.mass, p2.mass, p3.mass]) + energie_kin(
[p.mass for p in n_body_system.particles], [p.velocity for p in n_body_system.particles]) - e_kin_alt]
for i in range(n_steps):
print("\rcalculation progress: t = {:.2f} / {:.2f} ({:.2f}%)".format(i * h, n_steps * h, 100 * i / n_steps),
end="")
n_body_system.step_rk4(h)
positions.append([p.position for p in n_body_system.particles])
dist1.append(distance(p1.position, p2.position))
dist2.append(distance(p2.position, p3.position))
dist3.append(distance(p1.position, p3.position))
energy_tot.append(
energie_pot(positions[0], positions[i + 1], [p.mass for p in n_body_system.particles]) + energie_kin(
[p.mass for p in n_body_system.particles],
[p.velocity for p in n_body_system.particles]) - e_kin_alt)
minima = (minima_der_minima(minima_suchen(dist1, h) + minima_suchen(dist2, h) + minima_suchen(dist3, h)))
with open('mini{}.txt'.format(h), 'w+') as schreiberling:
schreiberling.writelines("%s\n" % element for element in minima)
print("\rcalculation progress: done.")
xax = np.linspace(0, h * n_steps, int(n_steps + 1))
plt.figure(1, figsize=(8, 8))
plt.plot(xax, dist1, label='distance part 1, part 2')
plt.plot(xax, dist2, label='distance part 2, part 3')
plt.plot(xax, dist3, label='distance part 1, part 3')
plt.yscale('log')
plt.xlabel('time')
plt.ylabel('distance')
plt.legend()
plt.savefig('distances{}.png'.format(h))
plt.figure(2, figsize=(8, 8))
plt.plot(xax, energy_tot)
plt.yscale('log')
plt.xlabel('timestep')
plt.ylabel('error of the total energy of the system')
plt.title('development of the total energy over time')
plt.savefig('energyerror{}.png'.format(h))
plot_animation(positions, h, save_as="particle_animation{}.mp4".format(h))
def __init__(self, x, y, orien, particle_size = 50):
self.world = World()
self.particles = [Particle(x, y, random.random()* 2.*math.pi) for i in range(particle_size)]
self.robot = Particle(x, y, orien, is_robot=True)
self.particle_size = particle_size
round(circleR), 1)
my_particles = []
# initialising the new particle size and position values
radius, pos_x, pos_y = randomParticleParm()
for i in range(PN):
while outsideCircle(circleX, circleY, radius, pos_x, pos_y) or overlap(
my_particles, radius, pos_x, pos_y):
radius, pos_x, pos_y = randomParticleParm()
p = Particle(screen=screen,
x=pos_x,
y=pos_y,
radius=radius,
thickness=THICKNESS,
angle=100,
speed=2)
my_particles.append(p)
radius, pos_x, pos_y = randomParticleParm()
for p in my_particles:
p.display()
pygame.display.flip()
running = True
while running:
for event in pygame.event.get():
def grade(Robot_init_pose, Dh_circular, Robot_speed):
grid = CozGrid(Map_filename)
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid)
score = 0
# 1. steps to build tracking
steps_built_track = 9999
for i in range(0, Steps_build_tracking):
est_pose = particlefilter.update(Dh_circular, Robot_speed)
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot \
and i+1 < steps_built_track:
steps_built_track = i + 1
#print("steps_built_track =", steps_built_track)
if steps_built_track < 50:
score = 50
elif steps_built_track < 100:
score = 100 - steps_built_track
else:
score = 0
print("\nPhrase 1")
print("Number of steps to build track :", steps_built_track, "/",
Steps_build_tracking)
acc_err_trans, acc_err_rot = 0.0, 0.0
max_err_trans, max_err_rot = 0.0, 0.0
step_track = 0
# 2. test tracking
for i in range(0, Steps_stable_tracking):
est_pose = particlefilter.update(Dh_circular, Robot_speed)
err_trans = grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y)
acc_err_trans += err_trans
if max_err_trans < err_trans:
max_err_trans = err_trans
err_rot = math.fabs(diff_heading_deg(est_pose[2], robbie.h))
acc_err_rot += err_rot
if max_err_rot < err_rot:
max_err_rot = err_rot
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot:
step_track += 1
if step_track > 90:
score += 50
elif step_track > 40:
score += step_track - 40
else:
score += 0
print("\nPhrase 2")
print("Number of steps error in threshold :", step_track, "/",
Steps_stable_tracking)
print("Average translational error :{:.2f}".format(acc_err_trans /
Steps_stable_tracking))
print("Average rotational error :{:.2f} deg".format(acc_err_rot /
Steps_stable_tracking))
print("Max translational error :{:.2f}".format(max_err_trans))
print("Max rotational error :{:.2f} deg".format(max_err_rot))
print("\nexample score =", score)
return score
class mainWindow(QMainWindow, Ui_MainWindow, UI_Interface):
"""
The main class for the GUI window
"""
def __init__(self):
"""
The constructor and initiator.
:return:
"""
# initial setup
super(mainWindow, self).__init__()
self.setupUi(self)
# create an instance of the table data and give yourself as UI Interface
self.ame_data = AMEData(self)
# take care about ring combo
self.ring_dict = Ring.get_ring_dict()
keys = list(self.ring_dict.keys())
keys.sort()
self.comboBox_ring.addItems(keys)
self.current_ring_name = ''
self.comboBox_ring.setCurrentIndex(4)
self.on_comboBox_ring_changed(4)
# fill combo box with names
self.comboBox_name.addItems(self.ame_data.names_list)
# UI related stuff
self.setup_table_view()
self.connect_signals()
# A particle to begin with
self.particle = None
self.comboBox_name.setCurrentIndex(6)
self.reinit_particle()
def setup_table_view(self):
# setup table view
self.tableView_model = QStandardItemModel(40, 3)
self.tableView_model.clear()
self.tableView_model.setHorizontalHeaderItem(0, QStandardItem('Name'))
self.tableView_model.setHorizontalHeaderItem(1, QStandardItem('Value'))
self.tableView_model.setHorizontalHeaderItem(2, QStandardItem('Unit'))
# self.tableView.verticalHeader().setVisible(False)
self.tableView.setModel(self.tableView_model)
def connect_signals(self):
"""
Connects signals.
:return:
"""
self.actionClear_results.triggered.connect(self.on_pushButton_clear)
self.actionSave_results.triggered.connect(self.save_file_dialog)
self.actionCalculate.triggered.connect(self.on_pushButton_calculate)
self.actionShow_table_data.triggered.connect(self.on_pushButton_table_data)
self.actionIdentify.triggered.connect(self.on_pushButton_identify)
self.actionIsotopes.triggered.connect(self.show_isotopes)
self.actionIsobars.triggered.connect(self.show_isobars)
self.actionIsotones.triggered.connect(self.show_isotones)
# Action about and Action quit will be shown differently in OSX
self.actionAbout.triggered.connect(self.show_about_dialog)
self.actionQuit.triggered.connect(QCoreApplication.instance().quit)
self.pushButton_copy_table.clicked.connect(self.on_pushButton_copy_table)
self.pushButton_clear.clicked.connect(self.on_pushButton_clear)
self.pushButton_isotopes.clicked.connect(self.show_isotopes)
self.pushButton_isotones.clicked.connect(self.show_isotones)
self.pushButton_isobars.clicked.connect(self.show_isobars)
self.pushButton_save.clicked.connect(self.save_file_dialog)
self.pushButton_calculate.clicked.connect(self.on_pushButton_calculate)
self.pushButton_table_data.clicked.connect(self.on_pushButton_table_data)
self.pushButton_identify.clicked.connect(self.on_pushButton_identify)
self.pushButton_nav_n.clicked.connect(self.on_nav_n_pressed)
self.pushButton_nav_ne.clicked.connect(self.on_nav_ne_pressed)
self.pushButton_nav_e.clicked.connect(self.on_nav_e_pressed)
self.pushButton_nav_se.clicked.connect(self.on_nav_se_pressed)
self.pushButton_nav_s.clicked.connect(self.on_nav_s_pressed)
self.pushButton_nav_sw.clicked.connect(self.on_nav_sw_pressed)
self.pushButton_nav_w.clicked.connect(self.on_nav_w_pressed)
self.pushButton_nav_nw.clicked.connect(self.on_nav_nw_pressed)
self.spinBox_qq.valueChanged.connect(self.on_spinBox_qq_changed)
self.spinBox_nn.valueChanged.connect(self.on_spinBox_nn_changed)
self.spinBox_zz.valueChanged.connect(self.on_spinBox_zz_changed)
self.comboBox_name.currentIndexChanged.connect(self.on_comboBox_name_changed)
self.comboBox_ring.currentIndexChanged.connect(self.on_comboBox_ring_changed)
def show_about_dialog(self):
about_dialog = QDialog()
about_dialog.ui = Ui_AbooutDialog()
about_dialog.ui.setupUi(about_dialog)
about_dialog.ui.label_version.setText('Version: {}'.format(__version__))
about_dialog.exec_()
about_dialog.show()
def show_message(self, message):
"""
Implementation of an abstract method:
Show text in status bar
:param message:
:return:
"""
self.statusbar.showMessage(message)
def show_message_box(self, text):
"""
Implementation of an abstract method:
Display a message box.
:param text:
:return:
"""
reply = QMessageBox.question(self, 'Message',
text, QMessageBox.Yes |
QMessageBox.No, QMessageBox.No)
if reply == QMessageBox.Yes:
return True
else:
return False
def show_isotopes(self):
"""
SLOT
show isotopes
:return:
"""
self.reinit_particle()
p_array = self.particle.get_isotopes()
text = 'Isotopes of {} are:\n'.format(self.particle) + '\n'.join(map(str, p_array)) + '\n'
self.plainTextEdit.appendPlainText(text)
def show_isotones(self):
"""
SLOT
show isotones
:return:
"""
self.reinit_particle()
text = 'Isotones of {} are:\n'.format(self.particle) + '\n'.join(map(str, self.particle.get_isotones())) + '\n'
self.plainTextEdit.appendPlainText(text)
def show_isobars(self):
"""
SLOT
show isobars
:return:
"""
self.reinit_particle()
text = 'Isobars of {} are:\n'.format(self.particle) + '\n'.join(map(str, self.particle.get_isobars())) + '\n'
self.plainTextEdit.appendPlainText(text)
def save_file_dialog(self):
"""
Show a save file dialog
:return:
"""
file_name, _ = QFileDialog.getSaveFileName(self, "Save results", '',
"Text file (*.txt)")
if not file_name:
return
self.save_results(file_name)
def save_results(self, file_name):
"""
Save results to fiven filename.
:param file_name:
:return:
"""
if not self.plainTextEdit.toPlainText():
self.show_message('No results to save.')
return
with open(file_name, 'w') as f:
f.write(str(self.plainTextEdit.toPlainText()))
self.show_message('Wrote to file {}.'.format(file_name))
def check_nuclide_validity(self):
"""
Check if the given nuclide exists in the table
:return:
"""
nuclide_validity = True
if '{}_{}'.format(self.spinBox_zz.value(), self.spinBox_nn.value()) in self.ame_data.zz_nn_names_dic:
aa = self.spinBox_zz.value() + self.spinBox_nn.value()
self.label_name.setText(
'{} {} {}+'.format(aa, self.ame_data.zz_nn_names_dic[
'{}_{}'.format(self.spinBox_zz.value(), self.spinBox_nn.value())], self.spinBox_qq.value()))
self.show_message('Valid nuclide')
else:
self.label_name.setText('------')
self.show_message('Not a valid nuclide')
nuclide_validity = False
return nuclide_validity
def reinit_particle(self):
"""
Re initialize the particle with new values
:return:
"""
if self.check_nuclide_validity():
# Here make a particle
zz = self.spinBox_zz.value()
nn = self.spinBox_nn.value()
self.particle = Particle(zz, nn, self.ame_data, self.ring_dict[self.current_ring_name])
self.particle.qq = self.spinBox_qq.value()
self.particle.ke_u = self.doubleSpinBox_energy.value()
self.particle.i_beam_uA = self.doubleSpinBox_beam_current.value()
self.particle.f_analysis_mhz = self.doubleSpinBox_f_analysis.value()
if not self.checkBox_circum.isChecked():
self.particle.path_length_m = self.doubleSpinBox_path_length.value()
else:
self.particle.path_length_m = self.ring_dict[self.current_ring_name].circumference
def keyPressEvent(self, event):
"""
Keypress event handler
:return:
"""
if type(event) == QKeyEvent:
# here accept the event and do something
if event.key() == Qt.Key_Return or event.key() == Qt.Key_Enter: # code enter key
# self.do_calculate()
self.on_pushButton_calculate()
event.accept()
if event.key() == Qt.Key_Space:
# self.on_pushButton_table_data()
event.accept()
if event.key() == Qt.Key_Up:
print('up')
event.accept()
else:
event.ignore()
def on_spinBox_zz_changed(self):
"""
SLOT
:return:
"""
self.spinBox_zz.setMinimum(1)
self.spinBox_zz.setMaximum(self.ame_data.zz_max)
self.comboBox_name.setCurrentIndex(self.spinBox_zz.value())
if self.spinBox_qq.value() > self.spinBox_zz.value():
self.spinBox_qq.setValue(self.spinBox_zz.value())
self.check_nuclide_validity()
def on_spinBox_nn_changed(self):
"""
SLOT
:return:
"""
self.spinBox_nn.setMinimum(0)
self.spinBox_nn.setMaximum(self.ame_data.nn_max)
self.check_nuclide_validity()
def on_spinBox_qq_changed(self):
"""
SLOT
:return:
"""
self.spinBox_qq.setMinimum(1)
self.spinBox_qq.setMaximum(self.ame_data.zz_max)
if self.spinBox_qq.value() > self.spinBox_zz.value():
self.spinBox_qq.setValue(self.spinBox_zz.value())
self.check_nuclide_validity()
def on_comboBox_name_changed(self, idx):
"""
SLOT
:return:
"""
self.spinBox_zz.setValue(idx)
self.spinBox_nn.setValue(idx)
self.spinBox_qq.setValue(idx)
def on_comboBox_ring_changed(self, idx):
# ignore index for now
self.current_ring_name = self.comboBox_ring.itemText(idx)
def on_pushButton_calculate(self):
"""
SLOT
:return:
"""
self.do_calculate()
def on_pushButton_clear(self):
self.plainTextEdit.clear()
self.setup_table_view()
def do_calculate(self):
"""
SLOT
Do the actual calculation
:return:
"""
if self.check_nuclide_validity():
self.show_message('Valid nuclide.')
self.reinit_particle()
self.update_table_view()
else:
self.show_message('Not a valid nuclide.')
def on_pushButton_copy_table(self):
self.plainTextEdit.appendPlainText(self.particle.calculate_from_energy())
def on_pushButton_table_data(self):
"""
SLOT
:return:
"""
self.reinit_particle()
self.plainTextEdit.appendPlainText(self.particle.get_table_data())
def on_pushButton_identify(self):
# update rings etc...
self.reinit_particle()
try:
f_actual = float(self.lineEdit_f_actual.text())
f_unknown = float(self.lineEdit_f_unknown.text())
except(ValueError):
self.show_message('Please enter valid frequencies in the text field.')
return
range_zz = self.spinBox_range_zz.value()
range_nn = self.spinBox_range_nn.value()
max_ee = self.spinBox_max_ee.value()
accuracy = self.doubleSpinBox_accuracy.value()
self.plainTextEdit.appendPlainText(
self.particle.identify(float(f_actual), float(f_unknown), range_zz, range_nn, max_ee, accuracy))
self.show_message('You may narrow your search either by reducing search area or the sensitivity radius.')
def on_nav_n_pressed(self):
"""
SLOT
:return:
"""
zz = self.spinBox_zz.value()
self.spinBox_zz.setValue(zz + 1)
def on_nav_ne_pressed(self):
"""
SLOT
:return:
"""
zz = self.spinBox_zz.value()
nn = self.spinBox_nn.value()
self.spinBox_zz.setValue(zz + 1)
self.spinBox_nn.setValue(nn + 1)
def on_nav_e_pressed(self):
"""
SLOT
:return:
"""
nn = self.spinBox_nn.value()
self.spinBox_nn.setValue(nn + 1)
def on_nav_se_pressed(self):
"""
SLOT
:return:
"""
zz = self.spinBox_zz.value()
nn = self.spinBox_nn.value()
self.spinBox_zz.setValue(zz - 1)
self.spinBox_nn.setValue(nn + 1)
def on_nav_s_pressed(self):
"""
SLOT
:return:
"""
zz = self.spinBox_zz.value()
self.spinBox_zz.setValue(zz - 1)
def on_nav_sw_pressed(self):
"""
SLOT
:return:
"""
zz = self.spinBox_zz.value()
nn = self.spinBox_nn.value()
self.spinBox_zz.setValue(zz - 1)
self.spinBox_nn.setValue(nn - 1)
def on_nav_w_pressed(self):
"""
SLOT
:return:
"""
nn = self.spinBox_nn.value()
self.spinBox_nn.setValue(nn - 1)
def on_nav_nw_pressed(self):
"""
SLOT
:return:
"""
zz = self.spinBox_zz.value()
nn = self.spinBox_nn.value()
self.spinBox_zz.setValue(zz + 1)
self.spinBox_nn.setValue(nn - 1)
def update_table_view(self):
lst = self.particle.calculate_from_energy_list()
for i in range(len(lst)):
for j in range(len(lst[0])):
item = QStandardItem(lst[i][j])
self.tableView_model.setItem(i, j, item)
self.tableView.resizeColumnsToContents()
def generate_new_particle_set(self):
'''Discards the current particle set and creates a new set of random particles.
'''
self.particles = list()
for i in xrange(self.filter_params.number_of_particles):
self.particles.append(Particle.random_particle(self.filter_params.neg_x_limit, self.filter_params.x_limit, self.filter_params.neg_y_limit, self.filter_params.y_limit, 1./self.filter_params.number_of_particles))
threading.Thread.__init__(self, daemon=True)
self.filter = particle_filter
self.gui = gui
def run(self):
while True:
estimated = self.filter.update()
self.gui.show_particles(self.filter.particles)
self.gui.show_mean(estimated[0], estimated[1], estimated[2],
estimated[3])
self.gui.show_robot(self.filter.robbie)
self.gui.updated.set()
if __name__ == "__main__":
grid = CozGrid(Map_filename)
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(setting.PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid)
if Use_GUI:
gui = GUIWindow(grid)
filter_thread = ParticleFilterThread(particlefilter, gui)
filter_thread.start()
gui.start()
else:
while True:
particlefilter.update()
def run_test_case(Robot_init_pose, Dh_circular, Robot_speed):
grid = CozGrid(Map_filename)
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(setting.PARTICLE_COUNT, grid)
robbie = Robot(Robot_init_pose[0], Robot_init_pose[1], Robot_init_pose[2])
particlefilter = ParticleFilter(particles, robbie, grid)
score = 0
# 1. steps to build tracking
steps_built_track = 9999
for i in range(0, Steps_build_tracking):
est_pose = particlefilter.update()
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot \
and i+1 < steps_built_track:
steps_built_track = i + 1
#print("steps_built_track =", steps_built_track)
if steps_built_track < 50:
score = 45
elif steps_built_track < Steps_build_tracking:
score = Steps_build_tracking - steps_built_track
else:
score = 0
print("\nPhase 1")
print("Number of steps to build track :", steps_built_track, "/",
Steps_build_tracking)
acc_err_trans, acc_err_rot = 0.0, 0.0
max_err_trans, max_err_rot = 0.0, 0.0
step_track = 0
# 2. test tracking
score_per_track = 45.0 / Steps_stable_tracking
for i in range(0, Steps_stable_tracking):
est_pose = particlefilter.update()
err_trans = grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y)
acc_err_trans += err_trans
if max_err_trans < err_trans:
max_err_trans = err_trans
err_rot = math.fabs(diff_heading_deg(est_pose[2], robbie.h))
acc_err_rot += err_rot
if max_err_rot < err_rot:
max_err_rot = err_rot
if grid_distance(est_pose[0], est_pose[1], robbie.x, robbie.y) < Err_trans \
and math.fabs(diff_heading_deg(est_pose[2], robbie.h)) < Err_rot:
step_track += 1
score += score_per_track
print("\nPhase 2")
print("Number of steps error in threshold :", step_track, "/",
Steps_stable_tracking)
print("Average translational error :",
acc_err_trans / Steps_stable_tracking)
print("Average rotational error :", acc_err_rot / Steps_stable_tracking,
"deg")
print("Max translational error :", max_err_trans)
print("Max rotational error :", max_err_rot, "deg")
return score
class FastSlam(object):
"""Main class that implements the FastSLAM1.0 algorithm"""
def __init__(self, x, y, orien, particle_size=50):
self.world = World()
self.particles = [
Particle(x, y,
random.random() * 2. * math.pi)
for i in xrange(particle_size)
]
self.robot = Particle(x, y, orien, is_robot=True)
self.particle_size = particle_size
def run_simulation(self):
while True:
for event in self.world.pygame.event.get():
self.world.test_end(event)
self.world.clear()
key_pressed = self.world.pygame.key.get_pressed()
if self.world.move_forward(key_pressed):
self.move_forward(2)
obs = self.robot.sense(self.world.landmarks, 2)
for p in self.particles:
p.update(obs)
self.particles = self.resample_particles()
if self.world.turn_left(key_pressed):
self.turn_left(5)
if self.world.turn_right(key_pressed):
self.turn_right(5)
self.world.render(self.robot, self.particles,
self.get_predicted_landmarks())
def move_forward(self, step):
self.robot.forward(step)
for p in self.particles:
p.forward(step)
def turn_left(self, angle):
self.robot.turn_left(angle)
for p in self.particles:
p.turn_left(angle)
def turn_right(self, angle):
self.robot.turn_right(angle)
for p in self.particles:
p.turn_right(angle)
def resample_particles(self):
new_particles = []
weight = [p.weight for p in self.particles]
index = int(random.random() * self.particle_size)
beta = 0.0
mw = max(weight)
for i in xrange(self.particle_size):
beta += random.random() * 2.0 * mw
while beta > weight[index]:
beta -= weight[index]
index = (index + 1) % self.particle_size
new_particle = deepcopy(self.particles[index])
new_particle.weight = 1
new_particles.append(new_particle)
return new_particles
def get_predicted_landmarks(self):
return self.particles[0].landmarks
def safe_name(name):
try:
return Particle.from_string(name).html_name
except:
return name
def __init__(self, data, args, debug=False):
# [n, pi-, pi-, pi+, an, pi+, pi+, pi-]
p = [Particle(data[:, 5 * i:5 * i + 4]) for i in range(8)]
cols = []
def get_tau1(p):
p_tau1_nu = p[0]
l_tau1_pi = p[1:4] # pi-, pi-, pi+
p_tau1_a1 = sum(l_tau1_pi)
p_tau1 = p_tau1_nu + p_tau1_a1
l_tau1_rho = [None] * 2
l_tau1_rho[0] = l_tau1_pi[0] + l_tau1_pi[2] # pi1- + pi+
l_tau1_rho[1] = l_tau1_pi[1] + l_tau1_pi[2] # pi2- + pi+
return p_tau1_nu, p_tau1_a1, l_tau1_pi, l_tau1_rho, p_tau1
def get_tau2(p):
p_tau2_nu = p[4]
l_tau2_pi = p[5:8]
p_tau2_a1 = sum(l_tau2_pi)
p_tau2 = p_tau2_nu + p_tau2_a1
l_tau2_rho = [None] * 2
l_tau2_rho[0] = l_tau2_pi[0] + l_tau2_pi[2] # pi1+ + pi-
l_tau2_rho[1] = l_tau2_pi[1] + l_tau2_pi[2] # pi2+ + pi-
return p_tau2_nu, p_tau2_a1, l_tau2_pi, l_tau2_rho, p_tau2
p_tau1_nu, p_tau1_a1, l_tau1_pi, l_tau1_rho, p_tau1 = get_tau1(p)
p_tau1_rho = sum(l_tau1_pi)
p_tau2_nu, p_tau2_a1, l_tau2_pi, l_tau2_rho, p_tau2 = get_tau2(p)
p_tau2_rho = sum(l_tau2_pi)
p_a1_a1 = sum(l_tau1_pi + l_tau2_pi)
PHI, THETA = calc_angles(p_tau1_a1, p_a1_a1)
beta_noise = args.BETA
for i, idx in enumerate([0, 1, 2, 3, 4, 5, 6, 7]):
part = boost_and_rotate(p[idx], PHI, THETA, p_a1_a1)
if args.FEAT in [
"Variant-1.0", "Variant-1.1", "Variant-2.0", "Variant-2.1",
"Variant-2.2", "Variant-3.0", "Variant-3.1", "Variant-3.2",
"Variant-4.0", "Variant-4.1"
]:
if idx not in [0, 4]:
cols.append(part.vec)
if args.FEAT == "Variant-All":
cols.append(part.vec)
if args.FEAT == "Variant-4.0":
part = boost_and_rotate(p_tau1, PHI, THETA, p_a1_a1)
cols.append(part.vec)
part = boost_and_rotate(p_tau2, PHI, THETA, p_a1_a1)
cols.append(part.vec)
if args.FEAT == "Variant-4.1":
p_tau1_approx = scale_lifetime(p_tau1)
part = boost_and_rotate(p_tau1_approx, PHI, THETA, p_a1_a1)
cols.append(part.vec)
p_tau2_approx = scale_lifetime(p_tau2)
part = boost_and_rotate(p_tau2_approx, PHI, THETA, p_a1_a1)
cols.append(part.vec)
# rho particles
if args.FEAT == "Variant-1.1":
for i, rho in enumerate(l_tau1_rho + l_tau2_rho):
rho = boost_and_rotate(rho, PHI, THETA, p_a1_a1)
cols.append(rho.vec)
# recalculated masses
if args.FEAT == "Variant-1.1":
for i, part in enumerate(l_tau1_rho + l_tau2_rho +
[p_tau1_a1, p_tau2_a1]):
part = boost_and_rotate(part, PHI, THETA, p_a1_a1)
cols.append(part.recalculated_mass)
if args.FEAT == "Variant-1.1":
for i in [1, 2]:
for ii in [5, 6]:
rho = p[i] + p[3]
other_pi = p[2 if i == 1 else 1]
rho2 = p[ii] + p[7]
other_pi2 = p[6 if i == 5 else 5]
rho_rho = rho + rho2
a1_rho = p_tau1_a1 + rho2
rho_a1 = rho + p_tau2_a1
cols += [
get_acoplanar_angle(p[i], p[3], p[ii], p[7], rho_rho),
get_acoplanar_angle(rho, other_pi, p[ii], p[7],
a1_rho),
get_acoplanar_angle(p[i], p[3], rho2, other_pi2,
rho_a1),
get_acoplanar_angle(rho, other_pi, rho2, other_pi2,
p_a1_a1)
]
cols += [
get_y(p[i], p[3], rho_rho),
get_y(p[ii], p[7], rho_rho),
get_y2(p_tau1_a1, rho, other_pi, a1_rho),
get_y(p[ii], p[7], a1_rho),
get_y(p[i], p[3], rho_a1),
get_y2(p_tau2_a1, rho2, other_pi2, rho_a1),
get_y2(p_tau1_a1, rho, other_pi, p_a1_a1),
get_y2(p_tau2_a1, rho2, other_pi2, p_a1_a1)
]
#------------------------------------------------------------
pb_tau1_h = boost_and_rotate(p_tau1_rho, PHI, THETA, p_a1_a1)
pb_tau2_h = boost_and_rotate(p_tau2_rho, PHI, THETA, p_a1_a1)
pb_tau1_nu = boost_and_rotate(p_tau1_nu, PHI, THETA, p_a1_a1)
pb_tau2_nu = boost_and_rotate(p_tau2_nu, PHI, THETA, p_a1_a1)
#------------------------------------------------------------
v_ETmiss_x = p_tau1_nu.x + p_tau2_nu.x
v_ETmiss_y = p_tau1_nu.y + p_tau2_nu.y
if args.FEAT == "Variant-2.2":
cols += [v_ETmiss_x, v_ETmiss_y]
vr_ETmiss_x, vr_ETmiss_y = rotate_xy(v_ETmiss_x, v_ETmiss_y, PHI)
#------------------------------------------------------------
if args.METHOD == "A":
va_alpha1, va_alpha2 = approx_alpha_A(v_ETmiss_x, v_ETmiss_y,
p_tau1_rho, p_tau2_rho)
elif args.METHOD == "B":
va_alpha1, va_alpha2 = approx_alpha_B(v_ETmiss_x, v_ETmiss_y,
p_tau1_rho, p_tau2_rho)
elif args.METHOD == "C":
va_alpha1, va_alpha2 = approx_alpha_C(v_ETmiss_x, v_ETmiss_y,
p_tau1_rho, p_tau2_rho)
#------------------------------------------------------------
va_tau1_nu_long = va_alpha1 * pb_tau1_h.z
va_tau2_nu_long = va_alpha2 * pb_tau2_h.z
#------------------------------------------------------------
va_tau1_nu_E = approx_E_nu(pb_tau1_h, va_tau1_nu_long)
va_tau2_nu_E = approx_E_nu(pb_tau2_h, va_tau2_nu_long)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
va_tau1_nu_trans = np.sqrt(
np.square(va_tau1_nu_E) - np.square(va_tau1_nu_long))
va_tau2_nu_trans = np.sqrt(
np.square(va_tau2_nu_E) - np.square(va_tau2_nu_long))
lambda_noise = args.BETA
v_tau1_nu_phi = np.arctan2(pb_tau1_nu.x, pb_tau1_nu.y) #boosted
v_tau2_nu_phi = np.arctan2(pb_tau2_nu.x, pb_tau2_nu.y)
vn_tau1_nu_phi = smear_exp(v_tau1_nu_phi, beta_noise)
vn_tau2_nu_phi = smear_exp(v_tau2_nu_phi, beta_noise)
if args.FEAT in ["Variant-3.2"]:
vn_tau1_nu_phi = smear_expnorm(v_tau1_nu_phi, args.BETA,
args.smear_loc, args.smear_scale)
vn_tau2_nu_phi = smear_expnorm(v_tau2_nu_phi, args.BETA,
args.smear_loc, args.smear_scale)
tau1_sin_phi = np.sin(vn_tau1_nu_phi)
tau1_cos_phi = np.cos(vn_tau1_nu_phi)
tau2_sin_phi = np.sin(vn_tau2_nu_phi)
tau2_cos_phi = np.cos(vn_tau2_nu_phi)
ve_x1_cms = pb_tau1_h.z / (pb_tau1_h + pb_tau1_nu).z
ve_x2_cms = pb_tau2_h.z / (pb_tau2_h + pb_tau2_nu).z
ve_alpha1_cms = 1 / ve_x1_cms - 1
ve_alpha2_cms = 1 / ve_x2_cms - 1
ve_tau1_nu_long = ve_alpha1_cms * pb_tau1_h.z
ve_tau2_nu_long = ve_alpha2_cms * pb_tau2_h.z
ve_tau1_nu_E = approx_E_nu(pb_tau1_h, ve_tau1_nu_long)
ve_tau2_nu_E = approx_E_nu(pb_tau2_h, ve_tau2_nu_long)
ve_tau1_nu_trans = np.sqrt(
np.square(ve_tau1_nu_E) - np.square(ve_tau1_nu_long))
ve_tau2_nu_trans = np.sqrt(
np.square(ve_tau2_nu_E) - np.square(ve_tau2_nu_long))
if args.FEAT in ["Variant-2.1", "Variant-2.2"]:
cols += [
va_tau1_nu_long, va_tau2_nu_long, va_tau1_nu_E, va_tau2_nu_E,
va_tau1_nu_trans, va_tau2_nu_trans
]
elif args.FEAT in ["Variant-3.0", "Variant-3.1", "Variant-3.2"]:
cols += [
va_tau1_nu_long, va_tau2_nu_long, va_tau1_nu_E, va_tau2_nu_E,
va_tau1_nu_trans * tau1_sin_phi,
va_tau2_nu_trans * tau2_sin_phi,
va_tau1_nu_trans * tau1_cos_phi,
va_tau2_nu_trans * tau2_cos_phi
]
elif args.FEAT == "Variant-2.0":
cols += [
ve_tau1_nu_long, ve_tau2_nu_long, ve_tau1_nu_E, ve_tau2_nu_E,
ve_tau1_nu_trans, ve_tau2_nu_trans
]
print(ve_tau1_nu_long)
# filter
filt = (p_tau1_a1.pt >= 20) & (p_tau2_a1.pt >= 20)
for part in (l_tau1_pi + l_tau2_pi):
filt = filt & (part.pt >= 1)
#filt = filt.astype(np.float32)
if args.FEAT in [
"Variant-1.0", "Variant-1.1", "Variant-All", "Variant-4.0",
"Variant-4.1"
]:
cols += [filt]
elif args.FEAT in [
"Variant-2.1", "Variant-2.2", "Variant-3.0", "Variant-3.1",
"Variant-3.2"
]:
isFilter = np.full(p_tau1_a1.e.shape, True, dtype=bool)
va_alpha1_A, va_alpha2_A = approx_alpha_A(v_ETmiss_x, v_ETmiss_y,
p_tau1_rho, p_tau2_rho)
va_alpha1_B, va_alpha2_B = approx_alpha_B(v_ETmiss_x, v_ETmiss_y,
p_tau1_rho, p_tau2_rho)
va_alpha1_C, va_alpha2_C = approx_alpha_C(v_ETmiss_x, v_ETmiss_y,
p_tau1_rho, p_tau2_rho)
va_tau1_nu_long_A = va_alpha1_A * pb_tau1_h.z
va_tau1_nu_long_B = va_alpha1_B * pb_tau1_h.z
va_tau1_nu_long_C = va_alpha1_C * pb_tau1_h.z
va_tau2_nu_long_A = va_alpha2_A * pb_tau2_h.z
va_tau2_nu_long_B = va_alpha2_B * pb_tau2_h.z
va_tau2_nu_long_C = va_alpha2_C * pb_tau2_h.z
va_tau1_nu_E_A = approx_E_nu(pb_tau1_h, va_tau1_nu_long_A)
va_tau1_nu_E_B = approx_E_nu(pb_tau1_h, va_tau1_nu_long_B)
va_tau1_nu_E_C = approx_E_nu(pb_tau1_h, va_tau1_nu_long_C)
va_tau2_nu_E_A = approx_E_nu(pb_tau2_h, va_tau2_nu_long_A)
va_tau2_nu_E_B = approx_E_nu(pb_tau2_h, va_tau2_nu_long_B)
va_tau2_nu_E_C = approx_E_nu(pb_tau2_h, va_tau2_nu_long_C)
va_tau1_nu_trans_A = np.sqrt(
np.square(va_tau1_nu_E_A) - np.square(va_tau1_nu_long_A))
va_tau1_nu_trans_B = np.sqrt(
np.square(va_tau1_nu_E_B) - np.square(va_tau1_nu_long_B))
va_tau1_nu_trans_C = np.sqrt(
np.square(va_tau1_nu_E_C) - np.square(va_tau1_nu_long_C))
va_tau2_nu_trans_A = np.sqrt(
np.square(va_tau2_nu_E_A) - np.square(va_tau2_nu_long_A))
va_tau2_nu_trans_B = np.sqrt(
np.square(va_tau2_nu_E_B) - np.square(va_tau2_nu_long_B))
va_tau2_nu_trans_C = np.sqrt(
np.square(va_tau2_nu_E_C) - np.square(va_tau2_nu_long_C))
for alpha in [
va_alpha1_A, va_alpha1_B, va_alpha1_C, va_alpha2_A,
va_alpha2_B, va_alpha2_C
]:
isFilter *= (alpha > 0)
for energy in [
va_tau1_nu_E_A, va_tau1_nu_E_B, va_tau1_nu_E_C,
va_tau2_nu_E_A, va_tau2_nu_E_B, va_tau2_nu_E_C
]:
isFilter *= (energy > 0)
for trans_comp in [
va_tau1_nu_trans_A, va_tau1_nu_trans_B, va_tau1_nu_trans_C,
va_tau2_nu_trans_A, va_tau2_nu_trans_B, va_tau2_nu_trans_C
]:
isFilter *= np.logical_not(np.isnan(trans_comp))
cols += [filt * isFilter]
elif args.FEAT in ["Variant-2.0"]:
isFilter = np.full(p_tau1_a1.e.shape, True, dtype=bool)
for alpha in [ve_alpha1_cms, ve_alpha2_cms]:
isFilter *= (alpha > 0)
for energy in [ve_tau1_nu_E, ve_tau2_nu_E]:
isFilter *= (energy > 0)
for trans_comp in [ve_tau1_nu_trans, ve_tau2_nu_trans]:
isFilter *= np.logical_not(np.isnan(trans_comp))
cols += [filt * isFilter]
for i in range(len(cols)):
if len(cols[i].shape) == 1:
cols[i] = cols[i].reshape([-1, 1])
self.cols = np.concatenate(cols, 1)
if args.BETA > 0:
vn_tau1_nu_phi = smear_polynomial(v_tau1_nu_phi, args.BETA,
args.pol_b, args.pol_c)
vn_tau2_nu_phi = smear_polynomial(v_tau2_nu_phi, args.BETA,
args.pol_b, args.pol_c)
tau1_sin_phi = np.sin(vn_tau1_nu_phi)
tau1_cos_phi = np.cos(vn_tau1_nu_phi)
tau2_sin_phi = np.sin(vn_tau2_nu_phi)
tau2_cos_phi = np.cos(vn_tau2_nu_phi)
self.valid_cols = [
va_tau1_nu_trans * tau1_sin_phi, va_tau2_nu_trans * tau2_sin_phi,
va_tau1_nu_trans * tau1_cos_phi, va_tau2_nu_trans * tau2_cos_phi
]
def measurement_update(particles, measured_marker_list, grid):
""" Particle filter measurement update
Arguments:
particles -- input list of particle represents belief \tilde{p}(x_{t} | u_{t})
before meansurement update
measured_marker_list -- robot detected marker list, each marker has format:
measured_marker_list[i] = (rx, ry, rh)
rx -- marker's relative X coordinate in robot's frame
ry -- marker's relative Y coordinate in robot's frame
rh -- marker's relative heading in robot's frame, in degree
grid -- grid world map, which contains the marker information,
see grid.h and CozGrid for definition
Returns: the list of particle represents belief p(x_{t} | u_{t})
after measurement update
"""
measured_particles = []
counter = 0
weightArr = []
if len(measured_marker_list) != 0:
for particle in particles:
# Obtain list of localization markers
visibleMarkers = particle.read_markers(grid)
shouldAppendParticle = particle.x >= grid.width or particle.x < 0 or particle.y >= grid.height or particle.y < 0 or (
particle.x, particle.y) in grid.occupied
if shouldAppendParticle:
weightArr.append((particle, 0))
else:
mmlLength = len(measured_marker_list)
vmLength = len(visibleMarkers)
pairs = []
for measuredMarker in measured_marker_list:
if len(visibleMarkers) != 0:
# find closest marker
nearestMarker = findNearestMarker(
measuredMarker, visibleMarkers)
# remove from possible future pairings
visibleMarkers.remove(nearestMarker)
# store pairings
pairs.append((nearestMarker, measuredMarker))
weightArr.append(
(particle, getProbability(pairs, mmlLength, vmLength)))
counter2 = 0
remove = int(PARTICLE_COUNT / 100)
# update weights
weightArr.sort(key=lambda x: x[1])
weightArr = weightArr[remove:]
for i, j in weightArr:
if j != 0:
counter2 = counter2 + j
if j == 0:
counter = counter + 1
weightArr = weightArr[counter:]
counter = counter + remove
else:
counter2 = 1
for p in particles:
weightArr.append((p, 1 / len(particles)))
particleList = []
weightList = []
for i, j in weightArr:
weight = j / counter2
weightList.append(weight)
particleList.append(Particle(i.x, i.y, i.h))
# Create new particle list by random
newParticleList = []
if particleList != []:
newParticleList = numpy.random.choice(particleList,
size=len(particleList),
replace=True,
p=weightList)
measured_particles = getMeasuredParticles(
Particle.create_random(counter, grid)[:], newParticleList)
return measured_particles
WIN_RF1 = "Robot view"
cv2.namedWindow(WIN_RF1)
cv2.moveWindow(WIN_RF1, 50, 50)
WIN_World = "World view"
cv2.namedWindow(WIN_World)
cv2.moveWindow(WIN_World, 500, 50)
# Initialize particles
print "Initializing particles .."
num_particles = 1000
particles = []
for i in range(num_particles):
# Random starting points. (x,y) \in [-1000, 1000]^2, theta \in [-pi, pi].
p = Particle(2000.0 * np.random.ranf() - 1000,
2000.0 * np.random.ranf() - 1000,
2.0 * np.pi * np.random.ranf() - np.pi, 1.0 / num_particles)
particles.append(p)
est_pose = estimate_pose(particles) # The estimate of the robots current pose
# Driving parameters
velocity = 0.0 # cm/sec
angular_velocity = 0.0 # radians/sec
# Initialize the robot (XXX: You do this)
# Allocate space for world map
world = np.zeros((500, 500, 3), dtype=np.uint8)
# Draw map
import numpy as np
import pandas as pd
from particle import Particle
from typing import Union
Rho0 = Particle.from_pdgid(113)
Pi0 = Particle.from_pdgid(111)
PiPlus = Particle.from_pdgid(211)
pi_pl_mass = PiPlus.mass / 1000
bckg_y = None
def bw(x: Union[np.ndarray, pd.Series], M: float, G: float,
amp: float) -> Union[np.ndarray, pd.Series]:
"""
Breit-Wigner function with params
M - mass of a resonance
G - $\Gamma$ of a resonance
amp - amplitude coefficient
x - Energy
"""
q = np.sqrt((x**2) / 4 - pi_pl_mass**2)
q_rho = np.sqrt((M**2) / 4 - pi_pl_mass**2)
ratrho1 = ((q / q_rho)**3) / ((q / q_rho)**2 + 1) * 2
g_tot = G * ratrho1
fff = amp * G * g_tot * x * M / ((x**2 - M**2)**2 + M**2 * g_tot**2)
return fff
def __init__(self, grid):
self.particles = Particle.create_random(PARTICLE_COUNT, grid)
self.grid = grid
def test_particle_update__bad_dvx():
try:
p = Particle(1., 2., 2., 2., 2.)
p.update("lol", 2.0, -1.0)
except TypeError:
pass
