convergence progress plots, and changed signature for obtain_driver_m…

…odels

scripts/experiments.jl

@@ -14,17 +14,22 @@ veh_id_list = [34,37,40,42,43,49,50];ts=88
 
 print("Get the metrics for particle filering\n")
 pos_pf,vel_pf,collisions_pf = extract_metrics(f,ts=ts,id_list=veh_id_list,
-filename="media/pf_ts_88.mp4")
+#filename="media/pf_ts_88.mp4"
+)
 
 print("Get the metrics for c-idm\n")
 pos_cidm,vel_cidm,collisions_cidm = extract_metrics(f,ts=ts,id_list=veh_id_list,
-modelmaker=make_cidm_models,filename="media/cidm_ts_88.mp4")
+modelmaker=make_cidm_models,
+filename="media/cidm_ts_88.mp4"
+)
 
 print("Get the metrics for idm\n")
 pos_idm,vel_idm,collisions_idm = extract_metrics(f,ts=ts,id_list=veh_id_list,
-modelmaker=make_IDM_models,filename="media/idm_ts_88.mp4")
+modelmaker=make_IDM_models,
+filename="media/idm_ts_88.mp4"
+)
 
-make_plots=true
+make_plots=false
 if(make_plots)
     # Make rmse_pos plot
     p_pos_pf = pgfplot_vector(pos_pf,leg="pf");

scripts/helpers.jl

@@ -1,5 +1,5 @@
 # This file provides helper functions to run experiments
-
+using Distributions # provides `mean`: to compute mean of particle dist over cars
 """
 function extract_metrics(f;ts=1,id_list=[],dur=10.,modelmaker=nothing,filename=[])
 
@@ -28,8 +28,15 @@ function extract_metrics(f;ts=1,id_list=[],dur=10.,modelmaker=nothing,filename=[
     # Otherwise, obtaine driver models using particle filtering
     models = Dict{Int64,DriverModel}()
     if isnothing(modelmaker)
-        print("Let's run particle filtering to create driver models")
-        models, = obtain_driver_models(f,id_list,50,ts,ts+50)
+        print("Let's run particle filtering to create driver models\n")
+        models,p,mean_dist_mat = obtain_driver_models(f,veh_id_list=id_list,num_p=50,ts=ts,te=ts+50)
+        avg_over_cars = mean(mean_dist_mat,dims=2)
+        avg_over_cars = reshape(avg_over_cars,length(avg_over_cars),) # for PGFPlot
+        print("Making filtering progress plot\n")
+        p = PGFPlots.Plots.Linear(collect(1:length(avg_over_cars)),avg_over_cars)
+        ax = PGFPlots.Axis([p],xlabel = "iternum",ylabel = "avg distance",
+        title = "Filtering progress")
+        PGFPlots.save("media/p_prog.pdf",ax)
     else
         print("Lets use idm or c_idm to create driver models\n")
         models = modelmaker(scene_real)

src/AutomotiveInteraction.jl

@@ -34,12 +34,7 @@ export
     test_jumpy_vehicle
 include("Driving/driving_simulation.jl")
 
-export 
-    append_to_curve!,
-    get_new_angle,
-    bound_heading,
-    append_headings,
-    centerlines_txt2tracks,
+export
     make_roadway_interaction,
     make_roadway_ngsim,
     make_roadway_interaction_with_extensions,
@@ -91,8 +86,6 @@ export
 include("Filtering/particle_filtering.jl")
 
 export
-    get_frenet_s,
-    get_lane_id,
     initial_pmat,
     plot_pairwise_particles,
     gen_imitation_traj,

src/Filtering/particle_filtering.jl

@@ -206,9 +206,10 @@ end
 Driver models for each vehicle in veh_id_list
 
 # Arguments
-- `veh_id_list` List with vehicle ids
-- `start_frame` Frame to start filtering from
-- `last_frame` Frame to end hallucination at
+- `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
@@ -219,27 +220,27 @@ Driver models for each vehicle in veh_id_list
 ```julia
 veh_id_list = [6]
 f = FilteringEnvironment()
-new_models,final_particles,mean_dist = obtain_driver_models(f,veh_id_list,500,1,5)
+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 obtain_driver_models(f::FilteringEnvironment,veh_id_list,num_particles,
-    start_frame,last_frame)
+function obtain_driver_models(f::FilteringEnvironment;veh_id_list=[],num_p=50,ts=1,te=100)
 
     models = Dict{Int64,DriverModel}() # key is vehicle id, value is driver model
     final_particles = Dict{Int64,Array{Float64}}() # key is vehicle id, value is final particles
 
-        # Loop over all the cars and get their corresponding IDM parameters by particle filter
+        # Loop over all the cars and get their corresponding parameters by particle filter
     num_cars = length(veh_id_list)
-    num_iters = last_frame-start_frame+2 # NEED TO CONFIRM THIS
+    num_iters = te-ts+2 # NEED TO CONFIRM THIS
 
         # num_iters x num_cars. Every elem is the mean dist of particle set at that iter for that car
     mean_dist_mat = fill(0.,num_iters,num_cars)
 
     for (ii,veh_id) in enumerate(veh_id_list)
         print("obtain_driver_models. vehicle id = $(veh_id) \n")
 
-        mean_particle, iterwise_p_set = multistep_update(f,num_p=num_particles,
-                car_id = veh_id,start_frame = start_frame,last_frame = last_frame)
+        mean_particle, iterwise_p_set = multistep_update(f,num_p=num_p,
+                car_id = veh_id,start_frame = ts,last_frame = te)
         #print("mean_particle", mean_particle, "\n")
 
         final_particles[veh_id] = mean_particle
@@ -250,16 +251,11 @@ function obtain_driver_models(f::FilteringEnvironment,veh_id_list,num_particles,
         # num_iters = length(iterwise_p_set) # SHOULD MATCH OUTSIDE LOOP VARIABLE
         mean_dist_over_iters = fill(0.,num_iters,1)
         for (jj,p_mat) in enumerate(iterwise_p_set)
-            current_mean_particle = mean(p_mat,dims=2)
-            mean_dist_over_iters[jj,1] = norm(current_mean_particle-mean_particle)
+            mean_dist_over_iters[jj,1] = avg_dist_particles(p_mat,mean_particle)
         end
 
         mean_dist_mat[:,ii] = mean_dist_over_iters    
     end
 
-        # Average over the cars and plot the filtering progress over frames
-    # avg_over_cars = mean(mean_dist_mat,dims=2)
-    # plot(avg_over_cars)
-
     return models, final_particles, mean_dist_mat
-end
+end

src/Filtering/utils.jl

@@ -189,15 +189,33 @@ end
 
 """ 
     function avg_dist_particles(p_mat,p_fin)
-
-- Compute the avg distance over particle set from final particle
+- Goal is to show that particle filering converges
+- `p_mat` is a matrix with particles in every column, say at a certain iteration before convergence
+- `p_fin` is the mean of the final particle set i.e. after convergence
+- For every particle in `p_mat`, find its norm to `p_fin`. And return the mean of these distances 
 
 # Arguments
 - `p_mat`: Matrix with particles in every column
-- `p_fin`: 2x1 array with the final particle
+- `p_fin`: num_paramx1 vector with the final particle
+
+# Example
+```julia
+num_iter = length(iterwise_p_mat)
+mean_dist_array = fill(0.,num_iter,1)
+for i in 1:num_iter
+    p_mat = iterwise_p_mat[i]
+    mean_dist_array[i] = avg_dist_particles(p_mat,p_fin)
+end
+```
 """
 function avg_dist_particles(p_mat,p_fin)
-    return sum(sqrt.(sum((p_mat .- p_fin).^2,dims=1)))*1/size(p_mat,2)
+    val = 0
+    num_particles = size(p_mat,2)
+    for i in 1:num_particles # loop over the particles
+        val+= norm(p_mat[:,i]-p_fin) # add norm distance to final particle
+    end
+    return val/num_particles # find mean of the norm distance to final particles
+    #return sum(sqrt.(sum((p_mat .- p_fin).^2,dims=1)))*1/size(p_mat,2)
 end
 
 # function: generate imitation trajectory

test/runtests.jl

@@ -42,7 +42,8 @@ final_p_mat,iterwise_p_mat = multistep_update(f,car_id=6,start_frame=1,last_fram
     # imitation trajectory with leaders using replay
 f = FilteringEnvironment()
 egoids = [28,29]
-new_models,final_particles,mean_dist = obtain_driver_models(f,egoids,30,1,30)
+new_models,final_particles,mean_dist_mat = obtain_driver_models(f,veh_id_list=egoids,
+    num_p=30,ts=1,te=30)
 start_frame = 1
 scene_list_1 = imitation_with_replay(f,new_models,egoids=egoids,replay_ids=[6,8,13],start_frame=start_frame)
 scene_list_2 = f.traj[start_frame:start_frame+length(scene_list_1)-1]