Skip to content

Commit

Permalink
rmse calculation functions for dynamical system origin circle
Browse files Browse the repository at this point in the history
  • Loading branch information
Raunak Bhattacharyya committed Jun 11, 2019
1 parent 4717a5b commit b240db9
Show file tree
Hide file tree
Showing 4 changed files with 90 additions and 9 deletions.
1 change: 1 addition & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
# output figures
*.gif
*.png
*.mp4

# notebook checkpoints
.ipynb_checkpoints/
90 changes: 85 additions & 5 deletions src/sandbox.jl → src/dyn_sys.jl
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ using LinearAlgebra
using Random
using Plots
using Reel
using Statistics

function runexp()
rng = Random.GLOBAL_RNG
Expand Down Expand Up @@ -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)
Expand All @@ -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

Expand All @@ -82,5 +162,5 @@ plots = runexp()

@show length(plots)

makegif = false
makegif = true
if makegif write_particles_gif(plots) end
4 changes: 2 additions & 2 deletions src/fjord.jl
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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)
Expand Down
4 changes: 2 additions & 2 deletions src/resamplers.jl
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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]]
Expand Down

0 comments on commit b240db9

Please sign in to comment.