JuliaPOMDP / ParticleFilters.jl
Showing 1 of 2 files from the diff.
Other files ignored by Codecov

@@ -1,5 +1,5 @@
Loading
1 1
# Damped spring mass oscillator as another case study
2 -
2 +
# http://rtg.math.ncsu.edu/wp-content/uploads/sites/3/2016/07/Kalman-Filter-Practical.pdf
3 3
# Idea: Plot true position, Kalman filter posterior mean, particle distribution, cem distribution
4 4
5 5
using ParticleFilters
@@ -11,16 +11,28 @@
Loading
11 11
using Reel
12 12
using Statistics
13 13
14 +
"""
15 +
State: x pos and x vel
16 +
Control input: Nothing
17 +
Process noise: None
18 +
Measurement noise: Noisy observation of x position
19 +
"""
14 20
function run_exp()
15 21
	rng = Random.GLOBAL_RNG
16 22
	dt = 0.1
17 23
	m = 10.
18 24
	k = 5.
19 25
	b = 3.
20 -
	A = [  0    1;
26 +
	A = [  0    1;	# From the equations of motion
21 27
	    -k/m -b/m]
22 28
29 +
	B = [0. 0.;
30 +
	     0. 0.]
31 +
32 +
	meas_noise = 2.0 # Measurment noise variance
23 33
	f(x,u,rng) = (Matrix(1.0*Diagonal(I,2)) + dt*A)*x
34 +
	h(x, rng) = rand(rng, Normal(x[1], meas_noise)) #Generates an observation
35 +
	g(x0, u, x, y) = pdf(Normal(x[1], meas_noise), y) #Creates likelihood
24 36
25 37
	# Initial state
26 38
	x = [1.0, 0.0]
@@ -37,12 +49,13 @@
Loading
37 49
38 50
		# Set up for particle filter and cem filter
39 51
	model = ParticleFilterModel{Vector{Float64}}(f, g)
40 -
	N = 100 # Number of particles
52 +
	N = 50 # Number of particles (10 causes posDef exception to throw up)
41 53
42 54
	filter_sir = SIRParticleFilter(model, N) # Vanilla particle filter
43 55
	filter_cem = CEMParticleFilter(model, N) # CEM filter
44 56
45 -
	b = ParticleCollection([1.0*rand(2) for i in 1:N]) # Each particle is 2 element array
57 +
	b_sir = ParticleCollection([rand(2) for i in 1:N]) # Each particle is 2 element array
58 +
	b_cem = b_sir
46 59
47 60
		# Run iterations
48 61
	for i in 1:100
@@ -51,20 +64,18 @@
Loading
51 64
		y = h(x, rng)	# Generate a noisy observation of the state
52 65
		
53 66
			# Kalman filtering estimate
54 -
		mu,sigma = kalman_filter(mu,sigma,u,y,A,B,C,W,V)
67 +
		#mu,sigma = kalman_filter(mu,sigma,u,y,A,B,C,W,V)
55 68
56 69
			# Particle filtering estimate
57 -
		b = update(filter_sir, b, u, y)
70 +
		b_sir = update(filter_sir, b_sir, u, y)
58 71
			
59 72
			# Cross entropy filtering estimate
60 -
		b_cem = update(filter_cem,b,u,y)
61 -
62 -
		plt = scatter([x[1]],[2.0],color=:black,label="true",xlim=(-5,5), ylim=(-5,5))
63 -
		scatter!([mu[1]],[2.0],color=:blue,label="kf",xlim=(-5,5), ylim=(-5,5))
64 -
		scatter!([p[1] for p in particles(b)],[2.0],color=:cyan,
65 -
			label="sir",xlim=(-5,5), ylim=(-5,5))
66 -
		scatter!([p[1] for p in particles(b_cem)],[2.0],color=:magenta,
67 -
			label="cem",xlim=(-5,5), ylim=(-5,5))
73 +
		b_cem = update(filter_cem,b_cem,u,y)
74 +
75 +
		plt = scatter([x[1]],[2.0],color=:black,label="true",xlim=(-5,5), ylim=(-5,5),markersize = 5.0,markershape=:octagon)
76 +
		#scatter!([mu[1]],[2.0],color=:blue,label="kf",xlim=(-5,5), ylim=(-5,5))
77 +
		scatter!([p[1] for p in particles(b_sir)],[1.5 for p in particles(b_sir)],color=:cyan,label="sir",xlim=(-5,5), ylim=(-5,5))
78 +
		scatter!([p[1] for p in particles(b_cem)],[2.5 for p in particles(b_cem)],color=:magenta,markershape = :star,label="cem",xlim=(-5,5), ylim=(-5,5))
68 79
		
69 80
		push!(positions,x[1])
70 81
		push!(plots,plt)
@@ -76,7 +87,7 @@
Loading
76 87
gif making function
77 88
"""
78 89
function make_gif(plots,filename)
79 -
display("Making gif")
90 +
	print("\n video name = $(filename)\n")
80 91
	frames = Frames(MIME("image/png"), fps=5)
81 92
	for plt in plots
82 93
	    push!(frames, plt)
@@ -110,6 +121,6 @@
Loading
110 121
display("Running the spring mass damper system")
111 122
positions,plots = run_exp()
112 123
plot(positions)
113 -
savefig("springmassdamper_RestStart.png")
124 +
savefig("../img/SpringMassDamper/26June/springmassdamper.png")
114 125
gifit = true
115 -
if gifit make_gif(plots,"springmassdamper_RestStart.mp4") end
126 +
if gifit make_gif(plots,"../img/SpringMassDamper/26June/springmassdamper.mp4") end
Files Coverage
src 20.81%
Project Totals (15 files) 20.81%
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