├── README.md
└── particlefilter.py


/README.md:
--------------------------------------------------------------------------------
1 | particlefilter
2 | ==============
3 | 
4 | Simple Particle filter implementation in Python
5 | 


--------------------------------------------------------------------------------
/particlefilter.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/python
  2 | 
  3 | import numpy as np
  4 | import random
  5 | 
  6 | 
  7 | class Particle:
  8 | 	def __init__(self,weight,state):
  9 | 		self.weight = weight
 10 | 		self.state=state
 11 | 
 12 | 
 13 | 
 14 | class ParticleFilter:
 15 | 
 16 | 	def __init__(self, numParticles, transMatrix, obsMatrix):
 17 | 		self.particles = []
 18 | 		self.dim,self.dim=transMatrix.shape
 19 | 		self.numParticles=numParticles
 20 | 		self.transMatrix=transMatrix
 21 | 		self.obsMatrix=obsMatrix
 22 | 
 23 | 		for i in xrange(numParticles):
 24 | 			self.particles.append( Particle(1/numParticles,random.randint(0,self.dim-1)) )
 25 | 			#print self.particles[-1].state
 26 | 
 27 | 		for i in range(10):
 28 | 			self.predict()
 29 | 			self.update([0])
 30 | 			#self.resample()
 31 |                         self.resample()
 32 |                         for x in range(len(self.particles)): print self.particles[x].state,
 33 | 
 34 | 
 35 | 	def predict(self):
 36 | 
 37 | 		#for x in range(len(self.particles)): print self.particles[x].state," ",
 38 | 
 39 | 		temp = []
 40 | 		temp = self.particles
 41 | 
 42 | 		for i in range(self.numParticles):
 43 | 			currentState=self.particles[i].state
 44 | 			rnd=random.random()
 45 | 
 46 | 			for j in range(self.dim):
 47 | 				if rnd<self.transMatrix[currentState][j]:
 48 | 					temp[i].state=j
 49 | 					#print "posizione ",currentState, j
 50 | 					#print "confronto ",rnd," con ",self.transMatrix[currentState][j], "e minore, quindi", currentState, " diventa", j
 51 | 				else:
 52 | 					rnd-=self.transMatrix[currentState][j]
 53 | 
 54 | 		self.particles=temp
 55 | 		print
 56 | 		#for x in range(len(self.particles)): print self.particles[x].state," ",
 57 | 
 58 | 
 59 | 	def update(self,measures):
 60 | 
 61 | 
 62 | 		for i in range(self.numParticles):
 63 | 
 64 | 			self.particles[i].weight =1.0
 65 | 
 66 | 			for j in range(len(measures)):
 67 | 				self.particles[i].weight *= self.obsMatrix[self.particles[i].state][measures[j]]
 68 | 
 69 | 		#for x in range(len(self.particles)): print self.particles[x].weight," ",
 70 | 
 71 | 		temp=[]
 72 | 		for i in range(len(self.particles)):
 73 | 			temp.append(self.particles[i].weight)
 74 | 
 75 | 		norm= np.linalg.norm(temp)
 76 | 		for i in range(len(self.particles)):
 77 | 			self.particles[i].weight /= norm
 78 | 
 79 |                 for x in range(len(self.particles)): print("{0:.3f}".format(self.particles[x].weight)), 
 80 | 		#print
 81 | 
 82 | 
 83 | 	def resample(self):
 84 | 
 85 |         	#print "Before resampling:"
 86 | 		#ifor x in range(len(self.particles)): print self.particles[x].state," ",
 87 | 		#iprint
 88 | 
 89 |         	N=self.numParticles
 90 |         	weights=[]
 91 | 		new_particles=[]
 92 |                 index = int(random.random() * N)
 93 | 
 94 | 		maxW=0
 95 | 
 96 | 		for i in range(N):
 97 |             		weights.append(self.particles[i].weight)
 98 | 
 99 |         	beta = 0.0
100 |         	mw = max(weights)
101 |         	for i in range(N):
102 |             		beta += random.random() * 2.0 * mw
103 |             		#print "beta =", beta
104 |             		while beta > weights[index]:
105 |                 		beta -= weights[index]
106 |                 		index = (index + 1) % N
107 |                 		#print "\tbeta= %f, index = %d, weight = %f" % (beta, index, weights[index])
108 |             		new_particles.append(self.particles[index])
109 | 
110 |         	#print "After resampling:"
111 |                 #for x in range(len(self.particles)): print self.particles[x].state," ",
112 |         	#print
113 | 
114 | 
115 | 
116 | 
117 | ParticleFilter(20,np.array([[0.8,0.2],[0.2,0.8]]),np.array([[0.8,0.1,0.1],[0.4,0.4,0.2]]))
118 | 
119 | 
120 | 


--------------------------------------------------------------------------------