File particle_filter.py changed (mode: 100644) (index b2a5ae5..3059050) |
... |
... |
while True: |
190 |
190 |
world.show_robot(nao) |
world.show_robot(nao) |
191 |
191 |
|
|
192 |
192 |
# ---------- Shuffle particles ---------- |
# ---------- Shuffle particles ---------- |
193 |
|
new_particles = [] |
|
194 |
|
|
|
|
193 |
|
|
195 |
194 |
# Aufgabe 3.2: resampling |
# Aufgabe 3.2: resampling |
196 |
195 |
# calculate new samples (new_particles) based on the weigths (p.w) of the old partiucles |
# calculate new samples (new_particles) based on the weigths (p.w) of the old partiucles |
197 |
196 |
# Example: Stochastic universal sampling |
# Example: Stochastic universal sampling |
|
... |
... |
while True: |
221 |
220 |
weight_sum += particles[current_particle_index].w |
weight_sum += particles[current_particle_index].w |
222 |
221 |
current_particle_index += 1 |
current_particle_index += 1 |
223 |
222 |
|
|
224 |
|
# The particle around which to saple a new particle |
|
|
223 |
|
# The particle around which to sample a new particle |
225 |
224 |
p = particles[current_particle_index-1] |
p = particles[current_particle_index-1] |
226 |
225 |
|
|
227 |
226 |
# Sample the new particle with some noise and increment the sum for the next particle |
# Sample the new particle with some noise and increment the sum for the next particle |
|
... |
... |
while True: |
236 |
235 |
new_particles.append(Particle(x, y, p.h, p.w, True)) |
new_particles.append(Particle(x, y, p.h, p.w, True)) |
237 |
236 |
new_part_weitgh_d += dp |
new_part_weitgh_d += dp |
238 |
237 |
|
|
|
238 |
|
weight_sum = 0 |
|
239 |
|
current_particle_index = 0 |
|
240 |
|
|
239 |
241 |
particles = new_particles |
particles = new_particles |
240 |
242 |
|
|
241 |
243 |
# ---------- Move things ---------- |
# ---------- Move things ---------- |