Permalink
Browse files

algorithms for localication and move. Unit 1

  • Loading branch information...
0 parents commit 792b0b8e2ffa49e6a3dd52b9e3733596327a36b0 @ivanacostarubio committed Mar 5, 2012
Showing with 95 additions and 0 deletions.
  1. +40 −0 unit_1.py
  2. +55 −0 unit_1.rb
@@ -0,0 +1,40 @@
+#
+# CS373
+#
+
+
+
+p=[0.2, 0.2, 0.2, 0.2, 0.2]
+world=['green', 'red', 'red', 'green', 'green']
+measurements = ['red', 'red']
+motions = [1,1]
+pHit = 0.6
+pMiss = 0.2
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+
+def sense(p, Z):
+ q=[]
+ for i in range(len(p)):
+ hit = (Z == world[i])
+ q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+ s = sum(q)
+ for i in range(len(q)):
+ q[i] = q[i] / s
+ return q
+
+def move(p, U):
+ q = []
+ for i in range(len(p)):
+ s = pExact * p[(i-U) % len(p)]
+ s = s + pOvershoot * p[(i-U-1) % len(p)]
+ s = s + pUndershoot * p[(i-U+1) % len(p)]
+ q.append(s)
+ return q
+
+for k in range(len(measurements)):
+ p = sense(p, measurements[k])
+ p = move(p, motions[k])
+
+print p
@@ -0,0 +1,55 @@
+
+#
+# CS 373
+#
+
+@p=[0.2, 0.2, 0.2, 0.2, 0.2]
+@world=['green', 'red', 'red', 'green', 'green']
+@measurements = ['red', 'red']
+@motions = [1,1]
+@pHit = 0.6
+@pMiss = 0.2
+@pExact = 0.8
+@pOvershoot = 0.1
+@pUndershoot = 0.1
+
+def sense(p, z)
+ q=[]
+ p.size.times do |i|
+ hit = (z == @world[i])
+ if hit == false
+ hit = 0
+ else
+ hit = 1
+ end
+ q << (p[i] * (hit * @pHit + (1-hit) * @pMiss))
+
+ @s = q.inject{|sum, qq| sum += qq}
+
+ end
+
+ q.size.times do |rr|
+ q[rr] = q[rr] / @s
+ end
+
+
+ return q
+end
+
+def move(p, u)
+ q = []
+ p.size.times do |i|
+ s = @pExact * p[(i-u) % p.size]
+ s = s + @pOvershoot * p[(i-u-1) % p.size]
+ s = s + @pUndershoot * p[(i-u+1) % p.size]
+ q << s
+ end
+ return q
+end
+
+@measurements.size.times do |k|
+ @p = sense(@p, @measurements[k])
+ @p = move(@p, @motions[k])
+end
+
+puts @p.inspect

0 comments on commit 792b0b8

Please sign in to comment.