Particle Filtering

function hallucinate_a_step(scene_input,particle;car_id=-1)

• Hallucinate one step starting from scene_input using parameters given by particle

Examples

scene = f.traj[1]
particle = [29.,NaN,1.5,5.,0.35,0.1,NaN,1.0]
hallucinate_a_step(f,scene,particle,car_id=6)

function weight_and_resample(start_scene,true_nextpos,true_nextlane,p_mat;car_id=-1)

• Hallucination from start_scene
• Compare against ground truth at true_nextpos, true_nextlane
• Assign weights to particles
• Perform resampling and return a new matrix of particles and associated weight vector

Examples

# Test for one step on initial particle matrix
limits = [10. 40.;0.1 10.;0.5 5.;1. 10.;0. 1.;-1. 1.;0. 20.;0. 1.]
init_pmat = initial_pmat(limits=limits,num_particles=10,seed=4)
id = 6
scene = f.traj[1]
true_next_scene = deepcopy(f.traj[2])
true_nextpos = get_frenet_s(true_next_scene;car_id=id)
true_nextlane = get_lane_id(true_next_scene,id)
weight_and_resample(f,scene,true_nextpos,true_nextlane,init_pmat,car_id=id)

function multistepupdate(f::FilteringEnvironment;carid,startframe,lastframe,num_p=500,seed=1)

• Run filtering over a trajectory by starting from a true scene
• Repeatedly calls weight_and_resample on a demonstration trajectory

Caveats

• Hard coded limits on initial particle distribution generation

Returns

• final_p_mat: Matrix with particles in separate columns. Mean of final particle set
• iterwise_p_mat: A list with the associated particle matrix at each iteration

Examples

final_p_mat,iterwise_p_mat = multistep_update(f,car_id=6,start_frame=1,last_frame=99)

function obtain_driver_models(f::FilteringEnvironment,veh_id_list,num_particles,start_frame,last_frame)

Driver models for each vehicle in vehidlist

Arguments

• veh_id_list: List with vehicle ids
• num_p: Number of particles
• ts: Frame to start filtering from
• te: Frame to end hallucination at

Returns

• models Dict with veh id as key and IDM driver model as value
• final_particles Dict with veh id as key and avg particle over final particle set as value
• mean_dist_mat: Every elem is the mean dist of particle set at that iter (row) for that car (column)

Example

veh_id_list = [6]
f = FilteringEnvironment()
new_models,final_particles,mean_dist_mat = obtain_driver_models(f,veh_id_list=veh_id_list,num_p=50,ts=1,te=5)
avg_over_cars = mean(mean_dist_mat,dims=2)

function initial_pmat(;limits,num_particles,seed)

• Generate initial particle matrix with num_particles particles with every col being a diff particle
• Range of values that parameters can take is specified in limits. Should be num_params rows x 2 cols

Examples

limits = [10. 40.;0.1 10.;0.5 5.;1. 10.;0. 1.;-1. 1.;-20. 20.;0. 1.]
initial_pmat(limits=limits,num_particles=10,seed=4)

• We have a list with corresponding particle matrix at every iteration of filtering
• We shall scatter two columns of said matrix
• We shall make a video with said scatter at every timestep

Examples

seed = 2; num_p = 500
Random.seed!(seed)
final_p_mat,iterwise_p_mat = multistep_update(car_id=1,start_frame=2,last_frame=99,num_p=num_p);
plot_pairwise_particles(iterwise_p_mat,filename="test/media/seed2.gif")