前十个例子（python）

Mobile-Robot-Foundation/p1/p1.py

@@ -0,0 +1,8 @@
+import numpy as np
+n=10
+A=np.zeros(n)
+# A=[]
+for i in range(n):
+    a=1/n
+    A[i]=a
+print(A)

Mobile-Robot-Foundation/p10/p10.py

@@ -0,0 +1,85 @@
+from matplotlib import pyplot as plt
+import numpy as np
+import math
+
+#字符串的比较函数，两个字符串相同返回1，不同返回值为0
+def cmp(str1,str2):
+    if(str1==str2):
+        return 1
+    else:
+        return 0
+
+#二维数组的索引函数，功能：在q矩阵中找到元素w的位置，并作为返回值返回
+def index_arr(q,w):
+    nCol = np.size(q,1)
+    q_max = np.max(q)
+    v = q.flatten()
+    n = v.tolist()
+    a = n.index(w)
+    i = int(math.modf(a/nCol)[1]) + 1
+    j = a%(nCol) + 1
+    return i,j
+
+#模拟机器人在一次运动过程中对其位置判决的概率分析
+def sense(p,z,world,pSenseCorrect):
+    nRow = np.size(p,0)
+    nCol = np.size(p,1)
+    q = np.zeros((nRow,nCol))
+    for i in range(nRow):
+        for j in range(nCol):
+            hit = cmp(z,(world[i][j]))
+            q[i][j]=p[i][j]*(hit * pSenseCorrect + (1-hit) * (1 - pSenseCorrect))
+    q_sum = np.sum(q)
+    q = q / q_sum
+    return q
+
+world = np.array([('red', 'green', 'green', 'red', 'red'),
+                  ('red', 'red', 'green', 'red', 'red'),
+                  ('red', 'red', 'green', 'green', 'red'),
+                  ('red', 'red', 'red', 'red', 'red')])
+nCol = np.size(world,1)#列数,此处与matlab对应的代码有所不同
+nRow = np.size(world,0)#行数,此处与matlab对应的代码有所不同
+pSenseCorrect = 0.7
+pStart = 0.7
+p = np.ones((nRow,nCol))
+p = (1 - pStart) / (nRow * nCol - 1) * p
+p[2][1] = pStart
+print('The Prior:')
+print(p)
+measurements = np.array(['green'])
+q = sense(p,measurements[0],world,pSenseCorrect)
+print('The probability after sensing:')
+print(q)
+q_max = np.max(q)
+str1 = 'The largest probability '+str(q_max)
+str2 = 'occurs at cell'+str(index_arr(q,q_max))
+print(str1,str2)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

Mobile-Robot-Foundation/p2/p2.py

@@ -0,0 +1,64 @@
+import numpy as np
+from matplotlib import pyplot as plt
+
+def sense(p, z, world, pHit, pMiss):
+    q = np.zeros(np.size(p))
+    for i in range(len(p)):
+        hit = cmp(z, world[i])
+        q[i] = (p[i] * (hit * pHit + (1 - hit) * pMiss))
+    q = q / np.sum(q)
+    return q
+
+def p2plot():
+    plt.figure('The Prior distribution')
+    plt.xlabel('Position')
+    plt.ylabel('Probability of being at the positon')
+    plt.grid()
+    plt.bar(range(len(p)), p, color='green')
+
+    plt.figure('Observation model')
+    plt.xlabel('Position')
+    plt.ylabel('Likelihood')
+    obs = np.zeros(np.size(world))
+    for i in range(len(world)):
+        hit = cmp(z, world[i])
+        obs[i] = hit * pHit + (1 - hit) * pMiss
+    plt.grid()
+    plt.bar(range(len(obs)), obs, color='blue')
+
+    plt.figure('The Posterior distribution')
+    plt.xlabel('Position')
+    plt.ylabel('Probability of being at the positon')
+    plt.grid()
+    plt.bar(range(len(q)), q, color='red')
+
+    plt.figure('Plot all')
+    plt.subplot(311)
+    plt.ylabel('Probability')
+    plt.grid()
+    plt.bar(range(len(p)), p)
+    plt.subplot(312)
+    plt.ylabel('Likelihood')
+    plt.grid()
+    plt.bar(range(len(obs)), obs)
+    plt.subplot(313)
+    plt.ylabel('Probability')
+    plt.xlabel('Position')
+    plt.grid()
+    plt.bar(range(len(q)), q)
+    plt.show()
+
+def cmp(str1,str2):
+    if(str1==str2):
+        return 1
+    else:
+        return 0
+
+world = np.array(['green', 'red', 'red', 'green', 'green'])
+p = np.array([0.2, 0.2, 0.2, 0.2, 0.2]) # Prior
+z = 'red'                       # Observation
+pHit = 0.6                      # Observation model
+pMiss = 0.2
+q = sense(p,z,world,pHit,pMiss)
+print(q)
+p2plot()

Mobile-Robot-Foundation/p3/p3.py

@@ -0,0 +1,53 @@
+import numpy as np
+from matplotlib import pyplot as plt
+
+def sense(p, z, world, pHit, pMiss):
+    q = np.zeros(np.size(p))
+    for i in range(len(p)):
+        hit = cmp(z, world[i])
+        q[i] = (p[i] * (hit * pHit + (1 - hit) * pMiss))
+    q_sum = np.sum(q)
+    q = q / q_sum
+    return q
+
+def p3plot():
+    plt.figure('Plot all')
+    plt.subplot(311)
+    plt.grid()
+    plt.ylabel('Prior')
+    my_title='Measurement:'+measurements[k]
+    plt.title(my_title)
+    plt.bar(range(len(p)), p)
+    plt.subplot(312)
+    plt.grid()
+    plt.ylabel('Observation')
+    obs = np.zeros(np.size(world))
+    for j in range(len(world)):
+        hit= cmp(measurements[k], world[j])
+        obs[j] = hit * pHit + (1 - hit) * pMiss
+    plt.bar(range(len(obs)), obs)
+    plt.subplot(313)
+    plt.grid()
+    plt.ylabel('Posterior')
+    plt.xlabel('Position')
+    plt.bar(range(len(q)), q)
+    plt.show()
+
+def cmp(str1,str2):
+    if(str1==str2):
+        return 1
+    else:
+        return 0
+
+world = np.array(['green', 'red', 'red', 'green', 'green'])
+p = np.array([0.2, 0.2, 0.2, 0.2, 0.2])       # Prior
+measurements=np.array(['red', 'red','green']) # Observation
+pHit = 0.6                                    # Observation model
+pMiss = 0.2
+for k in range(len(measurements)):
+    q = sense(p, measurements[k], world, pHit, pMiss)
+    p3plot()
+    p=q
+print(p)
+p3plot()
+plt.show()

Mobile-Robot-Foundation/p4/p4.py

@@ -0,0 +1,7 @@
+import numpy as np
+p=np.array([1/9,1/3,1/3,1/9,1/9])
+u=6    # move steps
+# 使用numpy中的roll指令对p进行指定步数u的移位操作
+q=np.roll(p,u)
+print('Before motion:',p)
+print('After motion:',q)

Mobile-Robot-Foundation/p5/p5.py

@@ -0,0 +1,12 @@
+import numpy as np
+def move(p, u, pExact, pOvershoot, pUndershoot):
+    q=(np.roll(p,u)*pExact)+(np.roll(p,u+1)*pOvershoot)+(np.roll(p,u-1)*pUndershoot)
+    return q
+p = np.array([0, 0.5, 0, 0.5, 0])
+u = 2
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+q=move(p, u, pExact, pOvershoot, pUndershoot)
+print(p)
+print(q)

Mobile-Robot-Foundation/p6/p6.py

@@ -0,0 +1,27 @@
+import numpy as np
+from matplotlib import pyplot as plt
+
+def move(p, u, pExact, pOvershoot, pUndershoot):
+    q=(np.roll(p,u)*pExact)+(np.roll(p,u+1)*pOvershoot)+(np.roll(p,u-1)*pUndershoot)
+    return q
+p=np.array([1, 0, 0, 0, 0])
+u=1
+step=100
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+for i in range(step):
+    p = move(p, u, pExact, pOvershoot, pUndershoot)
+    print(i+1)
+    print(p)
+    plt.figure(i + 1)
+    plt.ylabel('Probability')
+    my_title = 'step = ' + str(i + 1)
+    print(my_title)
+    plt.title(my_title)
+    plt.bar(range(len(p)), p)
+    plt.grid()  # 添加网格
+    plt.pause(0.2)
+
+
+

Mobile-Robot-Foundation/p7/p7.py

@@ -0,0 +1,35 @@
+import numpy as np
+from matplotlib import pyplot as plt
+
+def move(p, u, pExact, pOvershoot, pUndershoot):
+    q=(np.roll(p,u)*pExact)+(np.roll(p,u+1)*pOvershoot)+(np.roll(p,u-1)*pUndershoot)
+    return q
+
+def ce(p):
+    ce=-1 * np.dot(p, (np.log(p) / np.log(2)))
+    return ce
+
+#使用此函数剔除p中的0元素，剩余的非零元素返回a
+def out0(p):
+    a=[]
+    for index,value in enumerate(p):
+        if (not value == 0):
+            a.append(p[index])
+    return a
+
+p=np.array([1, 0, 0, 0, 0])
+u=1
+step=100
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+entropy=np.zeros(step+1)
+for i in range(1,step+1):
+    p = move(p, u, pExact, pOvershoot, pUndershoot)
+    entropy[i]=ce(out0(p))
+plt.title('The change of entropy in the process of moving')
+plt.plot((range(step+1)),entropy)
+plt.grid()  # 添加网格
+plt.xlabel('Motion step')
+plt.ylabel('Entropy')
+plt.show()

Mobile-Robot-Foundation/p8/p8.py

@@ -0,0 +1,67 @@
+from matplotlib import pyplot as plt
+import numpy as np
+
+def move(p, u, pExact, pOvershoot, pUndershoot):
+    q=(np.roll(p,u)*pExact)+(np.roll(p,u+1)*pOvershoot)+(np.roll(p,u-1)*pUndershoot)
+    return q
+
+def cmp(str1,str2):
+    if(str1==str2):
+        return 1
+    else:
+        return 0
+
+def sense(p, z, world, pHit, pMiss):
+    q = np.zeros(np.size(p))
+    for i in range(len(p)):
+        hit = cmp(z, world[i])
+        q[i] = (p[i] * (hit * pHit + (1 - hit) * pMiss))
+    q_sum = np.sum(q)
+    q = q / q_sum
+    return q
+
+def ce(p):
+    ce=-1 * np.dot(p, (np.log(p) / np.log(2)))
+    return ce
+
+#使用此函数剔除p中的0元素，剩余的非零元素返回a
+def out0(p):
+    a=[]
+    for index,value in enumerate(p):
+        if (not value == 0):
+            a.append(p[index])
+    return a
+
+
+world = np.array(['green', 'red', 'red', 'green', 'green'])
+p = np.array([0.2, 0.2, 0.2, 0.2, 0.2]) # Prior
+measurements=np.array(['red', 'green']) # Observation
+motions = np.array([1, 1])              # Motions
+pHit = 0.6                              # Observation model
+pMiss = 0.2
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+entropy = np.zeros((2,len(motions)))
+for i in range(len(measurements)):
+    p = sense(p, measurements[i], world, pHit, pMiss)
+    a=out0(p)
+    entropy[0,i] = ce(a)
+    p = move(p, motions[i], pExact, pOvershoot, pUndershoot)
+    b=out0(p)
+    entropy[1,i] = ce(b)
+print('The final posterior:')
+print(p)
+print('The final entropy:')
+print(entropy)
+plt.figure(1)
+plt.xlabel('Index of cell')
+plt.ylabel('Posterior')
+
+
+
+
+
+
+
+