JuliaPOMDP / ParticleFilters.jl
Showing 3 of 4 files from the diff.

@@ -100,12 +100,20 @@
Loading
100 100
    return [A[i, :] for i in 1:size(A,1)]
101 101
end
102 102
103 -
#XXX Need to un-harcoded the numtop that is fixed now
104 103
function resample(re::CEMResampler, b::AbstractParticleBelief{S}, rng::AbstractRNG) where {S}
105 104
#@show "cem resampple triggered alright"
105 +
	#print("\nStart of resampling\n")
106 +
	#print_particles(ParticleCollection(particles(b)))	
106 107
	sortedidx = sortperm(b.weights,rev=true)
108 +
	#@show sortedidx
107 109
	numtop = Int(0.2*re.n) # Top 20% of the number of particles to be selected as elite
108 110
	best_particles = b.particles[sortedidx[1:numtop]]
111 +
		#XXX: Printing things
112 +
		#print("After selecting the best particles \n")
113 +
		#@show length(best_particles)
114 +
115 +
		#print_particles(ParticleCollection(best_particles))	
116 +
	
109 117
	temp = hcat(best_particles...)'
110 118
	best_particles = temp'
111 119
@@ -113,9 +121,22 @@
Loading
113 121
		p_distb = fit(MvNormal,best_particles)
114 122
		new_p_mat = rand(p_distb,re.n)
115 123
		new_p_array = slicematrix(new_p_mat')
124 +
			#XXX Printing things
125 +
			#print("\nFitted distb: $(p_distb)\n")
126 +
			#@show p_distb			
127 +
			#print("\n after sampling from fitted distribution\n")
128 +
			#print_particles(ParticleCollection(new_p_array))
116 129
		return ParticleCollection(new_p_array)
117 130
	catch
118 -
		display("posdef exception was thrown")		
131 +
		print("\n posdef exception was thrown\n")		
119 132
		return ParticleCollection(b.particles)
120 133
	end
121 134
end
135 +
136 +
#XXX: Move this particle printing function to a more apt file as comapred to resample.jl
137 +
function print_particles(b::ParticleCollection)
138 +
	for p in particles(b)
139 +
		print("\n$(p)")
140 +
	end
141 +
	return nothing
142 +
end

@@ -15,6 +15,7 @@
Loading
15 15
    for i in 1:n_particles(b)
16 16
        x1 = particle(b, i)
17 17
        pm[i] = m.f(x1, u, rng)
18 +
	#print("\n propagated particle $(i)= $(pm[i])\n")
18 19
    end
19 20
end
20 21

@@ -8,6 +8,54 @@
Loading
8 8
using Reel
9 9
using Statistics
10 10
11 +
# To calculate the distance between kalman filter mean and true position
12 +
function calc_dist(mu::Array,x::Array)
13 +
	return norm(mu-x[1:2])
14 +
end
15 +
16 +
# Uses the norm squared calculation function to find the rmse
17 +
function calc_rmse(b::ParticleCollection,x)
18 +
	norm_vec = calc_norm_squared(b,x)
19 +
	return sqrt(mean(norm_vec))
20 +
end
21 +
22 +
"""
23 +
Returns an array with each elem being norm squared
24 +
from ground truth to particle
25 +
"""
26 +
function calc_norm_squared(b::ParticleCollection,x)
27 +
	particles = b.particles
28 +
	n = n_particles(b)	
29 +
30 +
	norm_squared = zeros(n)
31 +
	for i in 1:n
32 +
		p = particles[i][1:2]
33 +
		norm_squared[i] = norm(p-x[1:2])*norm(p-x[1:2])
34 +
	end
35 +
	return norm_squared
36 +
end
37 +
38 +
# Calc the rmse of the top 20% particles in the distribution
39 +
function calc_rmse_elites(b::ParticleCollection,x)
40 +
	particles = b.particles
41 +
	n = n_particles(b)
42 +
	n_elites = Int(0.2*n)
43 +
	norm_vec = calc_norm_squared(b,x)
44 +
	elite_particles = particles[sortperm(norm_vec)[1:n_elites]]
45 +
	return calc_rmse(ParticleCollection(elite_particles),x)
46 +
end
47 +
48 +
function write_particles_gif(plots,filename)
49 +
print("video name = $(filename)")
50 +
	frames = Frames(MIME("image/png"), fps=10)
51 +
	for plt in plots
52 +
	    push!(frames, plt)
53 +
	end
54 +
	write(filename, frames)
55 +
	return nothing
56 +
end # End of the reel gif writing function
57 +
58 +
11 59
"""
12 60
Q = V measurement noise covariance
13 61
R = W process noise covariance
@@ -24,6 +72,9 @@
Loading
24 72
	return mu_new,sigma_new
25 73
end
26 74
75 +
"""
76 +
Experiment to run the Kalman filter
77 +
"""
27 78
function run_kf(mu_0,sig_0,num_iter)
28 79
	display("Running kalman filter for $(num_iter) iterations")
29 80
	rng = Random.GLOBAL_RNG
@@ -67,6 +118,9 @@
Loading
67 118
	return plots
68 119
end
69 120
121 +
"""
122 +
Run an experiment using all 3 filters
123 +
"""
70 124
function runexp(;num_particles,num_iter,meascov)
71 125
	rng = Random.GLOBAL_RNG
72 126
@@ -89,7 +143,7 @@
Loading
89 143
	W = Matrix(0.001*Diagonal{Float64}(I, 4)) # Process noise covariance	
90 144
	V = Matrix(meascov*Diagonal{Float64}(I, 2)) # Measurement noise covariance
91 145
92 -
	f(x, u, rng) = A*x + rand(rng, MvNormal(W))
146 +
	f(x, u, rng) = A*x + 0.0*rand(rng, MvNormal(W))
93 147
94 148
	h(x, rng) = rand(rng, MvNormal(x[1:2], V)) #Generates an observation
95 149
	g(x0, u, x, y) = pdf(MvNormal(x[1:2], V), y) #Creates likelihood
@@ -102,7 +156,12 @@
Loading
102 156
	filter_cem = CEMParticleFilter(model, N) # CEM filter
103 157
104 158
	b = ParticleCollection([4.0*rand(4).-2.0 for i in 1:N])
105 -
159 +
	b_cem = b
160 +
	
161 +
		# XXX: Printing particles
162 +
		#print("Initial particle set \n")
163 +
		#print_particles(b)
164 +
	
106 165
		# Parameters setting up the Kalman filtering parameters
107 166
	mu = [0.,0.,0.,0.]
108 167
	sigma = Matrix(1.0*Diagonal{Float64}(I, 4))
@@ -116,16 +175,18 @@
Loading
116 175
	rmse_elites = zeros(num_iter,2)
117 176
118 177
	for i in 1:num_iter    #RpB: was 100 before
119 -
		#@show i
178 +
		#print("\nIteration number $(i) \n")
120 179
		m = mean(b) # b is an array of particles. Each element in b is a 4 element tuple
121 180
		u = [-m[1], -m[2]] # Control law - try to orbit the origin	
122 181
		x = f(x, u, rng)
123 182
		y = h(x, rng)
124 183
		b = update(filter_sir, b, u, y)
125 -
		#display("SIR update done")
126 184
		
127 -
		b_cem = update(filter_cem,b,u,y)
128 -
		#@show "cem update done"
185 +
		
186 +
		b_cem = update(filter_cem,b_cem,u,y)
187 +
			# XXX Printing particles			
188 +
			#print("\nHere is the cem update\n")
189 +
			#print_particles(b_cem)
129 190
130 191
			# Kalman filter update
131 192
		mu,sigma = kalman_filter(mu,sigma,u,y,A,B,C,W,V)
@@ -170,52 +231,6 @@
Loading
170 231
	
171 232
end # End of the runexp function
172 233
173 -
# To calculate the distance between kalman filter mean and true position
174 -
function calc_dist(mu::Array,x::Array)
175 -
	return norm(mu-x[1:2])
176 -
end
177 -
178 -
# Uses the norm squared calculation function to find the rmse
179 -
function calc_rmse(b::ParticleCollection,x)
180 -
	norm_vec = calc_norm_squared(b,x)
181 -
	return sqrt(mean(norm_vec))
182 -
end
183 -
184 -
"""
185 -
Returns an array with each elem being norm squared
186 -
from ground truth to particle
187 -
"""
188 -
function calc_norm_squared(b::ParticleCollection,x)
189 -
	particles = b.particles
190 -
	n = n_particles(b)	
191 -
192 -
	norm_squared = zeros(n)
193 -
	for i in 1:n
194 -
		p = particles[i][1:2]
195 -
		norm_squared[i] = norm(p-x[1:2])*norm(p-x[1:2])
196 -
	end
197 -
	return norm_squared
198 -
end
199 -
200 -
# Calc the rmse of the top 20% particles in the distribution
201 -
function calc_rmse_elites(b::ParticleCollection,x)
202 -
	particles = b.particles
203 -
	n = n_particles(b)
204 -
	n_elites = Int(0.2*n)
205 -
	norm_vec = calc_norm_squared(b,x)
206 -
	elite_particles = particles[sortperm(norm_vec)[1:n_elites]]
207 -
	return calc_rmse(ParticleCollection(elite_particles),x)
208 -
end
209 -
210 -
function write_particles_gif(plots,filename)
211 -
@show "Making gif"
212 -
	frames = Frames(MIME("image/png"), fps=10)
213 -
	for plt in plots
214 -
	    push!(frames, plt)
215 -
	end
216 -
	write(filename, frames)
217 -
	return nothing
218 -
end # End of the reel gif writing function
219 234
220 235
# Run the filtering multiple times and average the results from all the experiments
221 236
# Third dimension of the `data` data structure denotes experiment number
@@ -226,7 +241,7 @@
Loading
226 241
	data = zeros(num_iter,3,num_exp) #3 columns (vanilla,cem,kf)
227 242
	for i in 1:num_exp
228 243
		if i%20 == 0.
229 -
			@show i
244 +
			print("\nExp num = $(i)\n")
230 245
		end		
231 246
		plt,data[:,:,i] = runexp(num_particles=num_particles,
232 247
					num_iter=num_iter,meascov=meascov)
@@ -235,7 +250,7 @@
Loading
235 250
	rmse_avg = mean(data,dims=3)[:,:,1] #Extract 100x3 array from 100x3x1 array
236 251
237 252
	plot(rmse_avg,labels=["sir","cem","kf"],xlabel="iteration",ylabel="rmse")
238 -
	savefig("../img/$(num_exp)exps_$(num_particles)particles_$(meascov)meascov_$(num_iter)iterations.png")
253 +
	savefig("../img/25June_$(num_exp)exps_$(num_particles)particles_$(meascov)meascov_$(num_iter)iterations.png")
239 254
	return nothing
240 255
end
241 256
@@ -246,16 +261,21 @@
Loading
246 261
	# Single experiment and make associated video	
247 262
	display("Running a single experiment and making associated video")
248 263
	num_particles = 1000
249 -
	num_iter = 50
264 +
	num_iter = 100
250 265
	meascov = 5	
251 266
	plots, rmse = runexp(num_particles=num_particles,num_iter=num_iter,meascov=meascov)
252 267
	@show length(plots) # Should be equal to the number of iterations of the particle filter
253 268
	makegif = true
254 -
	if makegif write_particles_gif(plots,"../img/$(num_particles)_particles_all3.mp4") end
269 +
	if makegif write_particles_gif(plots,"../img/25June_$(num_particles)particles_$(num_iter)iterations_$(meascov)meascov.mp4") end
255 270
end
256 271
if runmanyexp
257 272
	# Mulitple experiments to make average rmse plot
258 -
	run_many_exps(num_exp = 100, num_particles = 50,num_iter=100,meascov=1)
273 +
	num_particles = 1000
274 +
	num_iter = 100
275 +
	meascov = 5
276 +
	num_exp = 100
277 +
	run_many_exps(num_exp = num_exp, num_particles = num_particles,
278 +
			num_iter=num_iter,meascov=meascov)
259 279
end
260 280
if runkf
261 281
	mu_0 = [1.,1.,1.,1.]
Files Coverage
src 20.90%
Project Totals (16 files) 20.90%
Sunburst
The inner-most circle is the entire project, moving away from the center are folders then, finally, a single file. The size and color of each slice is representing the number of statements and the coverage, respectively.
Icicle
The top section represents the entire project. Proceeding with folders and finally individual files. The size and color of each slice is representing the number of statements and the coverage, respectively.
Grid
Each block represents a single file in the project. The size and color of each block is represented by the number of statements and the coverage, respectively.
Loading