rmse calculation functions for dynamical system origin circle

.gitignore

@@ -5,6 +5,7 @@
 # output figures
 *.gif
 *.png
+*.mp4
 
 # notebook checkpoints
 .ipynb_checkpoints/

src/sandbox.jl → src/dyn_sys.jl

@@ -7,6 +7,7 @@ using LinearAlgebra
 using Random
 using Plots
 using Reel
+using Statistics
 
 function runexp()
 	rng = Random.GLOBAL_RNG
@@ -43,8 +44,13 @@ function runexp()
 	x = [0.0, 1.0, 1.0, 0.0]
 
 	plots = []
-	for i in 1:2    #RpB: was 100 before
-	    print(".")
+
+	num_iter = 100
+	rmse = zeros(num_iter,2)
+	rmse_elites = zeros(num_iter,2)
+
+	for i in 1:num_iter    #RpB: was 100 before
+	    #print(".")
 	    m = mean(b) # b is an array of particles. Each element in b is a 4 element tuple
 	    u = [-m[1], -m[2]] # Control law - try to orbit the origin
 	    x = f(x, u, rng)
@@ -62,18 +68,92 @@ function runexp()
 	    scatter!([p[1] for p in particles(b_cem)], [p[2] for p in particles(b_cem)], 
 		color=:red, markersize=2.0, label="",markershape=:cross)
 	    push!(plots, plt)
+
+	    # Plot the rmse value for the current iteration of particles
+		# Vanilla rmse
+ 	    rmse_sir=calc_rmse(b,x)
+	    rmse_sir_elites = calc_rmse_elites(b,x)
+	    rmse_cem=calc_rmse(b_cem,x)
+	    rmse[i,1] = rmse_sir
+	    rmse[i,2] = rmse_cem
+
+		# Elites calculation
+	    rmse_sir_elites = calc_rmse_elites(b,x)
+	    rmse_cem_elites = calc_rmse_elites(b_cem,x)
+	    rmse_elites[i,1] = rmse_sir_elites
+	    rmse_elites[i,2] = rmse_cem_elites
 	end
+
+	#plot(rmse,labels=["sir","cem"])
+	plot(hcat(rmse,rmse_elites),labels=["sir","cem","sir_el","cem_el"])
+	savefig("rmse.png")
 	return plots
+
 end # End of the runexp function
 
+# Find the mean particle from a collection
+function find_mean_particle(b::ParticleCollection)
+	return mean(b.particles)
+end
+
+"""
+	calc_rmse_old(b:ParticleCollection,x)
+# Arguments
+- x: True state - 4 element array. The first two elems (i.e. x,y coord) are used
+for norm computation
+- b: Collection of particles
+"""
+function calc_rmse_old(b::ParticleCollection,x)
+	#Extract particles from belief
+	particles = b.particles
+	n = n_particles(b)	
+
+	sum_sq_error = 0
+	# Loop over all the particles and store the square norm sum
+	for i in 1:n
+		p = particles[i][1:2]
+		sum_sq_error = sum_sq_error + norm(p-x[1:2])*norm(p-x[1:2])
+	end
+	return sqrt(sum_sq_error/n)
+end
+
+function calc_rmse(b::ParticleCollection,x)
+	norm_vec = calc_norm_squared(b,x)
+	return sqrt(mean(norm_vec))
+end
+
+"""
+Returns an array with each elem being norm squared
+from ground truth to particle
+"""
+function calc_norm_squared(b::ParticleCollection,x)
+	particles = b.particles
+	n = n_particles(b)	
+
+	norm_squared = zeros(n)
+	for i in 1:n
+		p = particles[i][1:2]
+		norm_squared[i] = norm(p-x[1:2])*norm(p-x[1:2])
+	end
+	return norm_squared
+end
+
+function calc_rmse_elites(b::ParticleCollection,x)
+	particles = b.particles
+	n = n_particles(b)
+	n_elites = Int(0.2*n)
+	norm_vec = calc_norm_squared(b,x)
+	elite_particles = particles[sortperm(norm_vec)[1:n_elites]]
+	return calc_rmse(ParticleCollection(elite_particles),x)
+end
+
 function write_particles_gif(plots)
 @show "Making gif"
 	frames = Frames(MIME("image/png"), fps=10)
 	for plt in plots
-	    print(".")
 	    push!(frames, plt)
 	end
-	write("output.gif", frames)
+	write("output.mp4", frames)
 	return nothing
 end # End of the reel gif writing function
 
@@ -82,5 +162,5 @@ plots = runexp()
 
 @show length(plots)
 
-makegif = false
+makegif = true
 if makegif write_particles_gif(plots) end

src/fjord.jl

@@ -71,7 +71,7 @@ function runexp()
 	meas_stdev = 0.1
 	A = [1]
 	B = [0]
-	f(x, u, rng) = x+[1.0]
+	f(x, u, rng) = x #+ [1.0] # Investigating fixed state
 	h(x, rng) = rand(rng, Normal(ypos-ground(x[1]), meas_stdev)) #Generates an observation
 	g(x0, u, x, y) = pdf(Normal(ypos - ground(x[1]), meas_stdev), y) #Creates likelihood
 
@@ -89,7 +89,7 @@ function runexp()
 	plots = []
 plt = plot_terrain_ac_particles(x,ypos,particles(b))
 push!(plots,plt)
-	for i in 1:100    #RpB: was 100 before
+	for i in 1:50    #RpB: was 100 before
 	    print(".")
 	    u = 1
 	    x = f(x, u, rng)

src/resamplers.jl

@@ -54,7 +54,7 @@ struct LowVarianceResampler
 end
 
 function resample(re::LowVarianceResampler, b::AbstractParticleBelief{S}, rng::AbstractRNG) where {S}
-@show "resample triggered alright"
+#@show "resample triggered alright"
     ps = Array{S}(undef, re.n)
     r = rand(rng)*weight_sum(b)/re.n
     c = weight(b,1) # weight of the first particle in the belief set
@@ -102,7 +102,7 @@ end
 
 #XXX Need to un-harcoded the numtop that is fixed now
 function resample(re::CEMResampler, b::AbstractParticleBelief{S}, rng::AbstractRNG) where {S}
-@show "cem resampple triggered alright"
+#@show "cem resampple triggered alright"
 	sortedidx = sortperm(b.weights,rev=true)
 	numtop = Int(0.2*re.n) # Top 20% of the number of particles to be selected as elite
 	best_particles = b.particles[sortedidx[1:numtop]]