def init_particle_filter(timestamp, initLoc, mapGraph):
"""Initialize model and configs of particle filter """
global LastTimestamp, pfRunner, resultData
# init_loc = numpy.array([-298.0, -772.0])
# Set static random
numpy.random.seed(1)
random.seed(1)
# Initialize LastTimestamp
LastTimestamp = timestamp
########################################################################
############### Algorithm Configs ################
NUM_OF_PARTICLES = 1000
P0 = 20.0
HUMAN_SPEED = 1.0 * 100.0
HUMAN_MAX_HEADING_CHANGE = 30
Q = numpy.asarray((HUMAN_MAX_HEADING_CHANGE, HUMAN_SPEED * 0.1)) # heading, speed variances
# R = numpy.asarray(((0.5,),))
# R = numpy.asarray(((10,),))
R = numpy.asarray(((20000,),))
# R = numpy.asarray(((0 ** 2,),))
RESAMPLING_THRESHOLD = 2.0 / 3.0
########################################################################
# Create the model from set configs
model = PredictionAndObservationModel(Q, R, P0, HUMAN_SPEED, initLoc, mapGraph)
# Create pf runner
pfRunner = simulator.Simulator(model, u=None, y=initLoc)
pfRunner.pt = filter.ParticleTrajectory(pfRunner.model, NUM_OF_PARTICLES, resample=RESAMPLING_THRESHOLD)
# Write initialization data to result log file
threading.Thread(target=append_data, args=([[timestamp, initLoc]])).start()
def calc_g(self, particles, t):
return particles
if __name__ == '__main__':
steps = 80
num = 10
M = 20
P0 = 1.0
Q = 1.0
R = numpy.asarray(((1.0, ), ))
numpy.random.seed(0)
(x, y) = generate_dataset(steps, P0, Q, R)
model = Model(P0, Q, R)
sim = simulator.Simulator(model, None, y)
sim.simulate(num,
M,
filter='PF',
smoother='ancestor',
smoother_options={'R': 50},
meas_first=False)
plt.plot(range(steps + 1), x, 'r-')
plt.plot(range(1, steps + 1), y, 'bx')
plt.plot(range(steps + 1), sim.get_smoothed_estimates()[:, :, 0], 'g.')
plt.ion()
plt.show()
time.sleep(5)
for _ in xrange(30):
plt.clf()
filt = 'APF'
elif (mode.lower() == 'apf'):
filt = 'APF'
else:
pass
for k in range(sims):
# Create reference
numpy.random.seed(k)
(y, e, z) = generate_dataset(steps,
Qz=Qz,
R=R,
Qes=Qes,
Qeb=Qeb)
sim = simulator.Simulator(model=model, u=None, y=y)
for ind, pc in enumerate(part_count):
sim.simulate(pc,
num_traj=1,
res=0.67,
filter=filt,
smoother=None)
avg = numpy.zeros((2, steps + 1))
avg = sim.get_filtered_mean()
sqr_err_eta = (avg[:, 0] - e[0, :])**2
sqr_err_theta = (avg[:, 1] - z[0, :])**2
mean_smooth_min = numpy.zeros((len(dict[t]), T + 1, 1))
mean_smooth_max = numpy.zeros((len(dict[t]), T + 1, 1))
mean_smooth_mean = numpy.zeros((len(dict[t]), T + 1, 1))
mean_filt_min = numpy.zeros((len(dict[t]), T + 1, 1))
mean_filt_max = numpy.zeros((len(dict[t]), T + 1, 1))
mean_filt_mean = numpy.zeros((len(dict[t]), T + 1, 1))
counter_i = 0
for i in dict[t]:
mean_smooth_tmp = numpy.zeros((len(i), T + 1, 1))
mean_filt_tmp = numpy.zeros((len(i), T + 1, 1))
x_mean = numpy.zeros((len(i), T + 1))
counter = 0
for j in i:
model = StdNonLin(P0, Q, R, l, l_counter)
sim = simulator.Simulator(model, u=None, y=y[j])
sim.simulate(N,
M,
filter='PF',
smoother='rsas',
meas_first=True)
(est_filt, w_filt) = sim.get_filtered_estimates()
mean_filt_tmp[counter] = sim.get_filtered_mean()
est_smooth = sim.get_smoothed_estimates()
mean_smooth_tmp[counter] = sim.get_smoothed_mean()
x_mean[counter] = x[j]
counter += 1
# pdb.set_trace()
