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()