From b6f35352a13235054128fbf861b94311e39bf16b Mon Sep 17 00:00:00 2001 From: liyaochong <823098528@qq.com> Date: Wed, 29 Nov 2017 12:50:01 +0800 Subject: [PATCH 1/3] code by python --- Mobile-Robot-Foundation/p1/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p1/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p1/.idea/p1.iml | 12 + Mobile-Robot-Foundation/p1/.idea/p1.py | 8 + .../p1/.idea/workspace.xml | 321 ++++++++++++++++++ Mobile-Robot-Foundation/p10/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p10/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p10/.idea/p10.iml | 12 + Mobile-Robot-Foundation/p10/.idea/p10.py | 85 +++++ .../p10/.idea/workspace.xml | 187 ++++++++++ Mobile-Robot-Foundation/p2/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p2/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p2/.idea/p2.iml | 12 + Mobile-Robot-Foundation/p2/.idea/p2.py | 64 ++++ .../p2/.idea/workspace.xml | 190 +++++++++++ Mobile-Robot-Foundation/p3/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p3/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p3/.idea/p3.iml | 12 + Mobile-Robot-Foundation/p3/.idea/p3.py | 53 +++ .../p3/.idea/workspace.xml | 185 ++++++++++ Mobile-Robot-Foundation/p4/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p4/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p4/.idea/p4.iml | 12 + Mobile-Robot-Foundation/p4/.idea/p4.py | 7 + .../p4/.idea/workspace.xml | 163 +++++++++ Mobile-Robot-Foundation/p5/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p5/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p5/.idea/p5.iml | 12 + Mobile-Robot-Foundation/p5/.idea/p5.py | 12 + .../p5/.idea/workspace.xml | 207 +++++++++++ Mobile-Robot-Foundation/p6/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p6/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p6/.idea/p6.iml | 12 + Mobile-Robot-Foundation/p6/.idea/p6.py | 27 ++ .../p6/.idea/workspace.xml | 147 ++++++++ Mobile-Robot-Foundation/p7/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p7/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p7/.idea/p7.iml | 12 + Mobile-Robot-Foundation/p7/.idea/p7.py | 35 ++ .../p7/.idea/workspace.xml | 181 ++++++++++ Mobile-Robot-Foundation/p8/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p8/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p8/.idea/p8.iml | 12 + Mobile-Robot-Foundation/p8/.idea/p8.py | 67 ++++ .../p8/.idea/workspace.xml | 297 ++++++++++++++++ Mobile-Robot-Foundation/p9/.idea/misc.xml | 4 + Mobile-Robot-Foundation/p9/.idea/modules.xml | 8 + Mobile-Robot-Foundation/p9/.idea/p9.iml | 12 + Mobile-Robot-Foundation/p9/.idea/p9.py | 52 +++ .../p9/.idea/workspace.xml | 179 ++++++++++ 50 files changed, 2707 insertions(+) create mode 100644 Mobile-Robot-Foundation/p1/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p1/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p1/.idea/p1.iml create mode 100644 Mobile-Robot-Foundation/p1/.idea/p1.py create mode 100644 Mobile-Robot-Foundation/p1/.idea/workspace.xml create mode 100644 Mobile-Robot-Foundation/p10/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p10/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p10/.idea/p10.iml create mode 100644 Mobile-Robot-Foundation/p10/.idea/p10.py create mode 100644 Mobile-Robot-Foundation/p10/.idea/workspace.xml create mode 100644 Mobile-Robot-Foundation/p2/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p2/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p2/.idea/p2.iml create mode 100644 Mobile-Robot-Foundation/p2/.idea/p2.py create mode 100644 Mobile-Robot-Foundation/p2/.idea/workspace.xml create mode 100644 Mobile-Robot-Foundation/p3/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p3/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p3/.idea/p3.iml create mode 100644 Mobile-Robot-Foundation/p3/.idea/p3.py create mode 100644 Mobile-Robot-Foundation/p3/.idea/workspace.xml create mode 100644 Mobile-Robot-Foundation/p4/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p4/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p4/.idea/p4.iml create mode 100644 Mobile-Robot-Foundation/p4/.idea/p4.py create mode 100644 Mobile-Robot-Foundation/p4/.idea/workspace.xml create mode 100644 Mobile-Robot-Foundation/p5/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p5/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p5/.idea/p5.iml create mode 100644 Mobile-Robot-Foundation/p5/.idea/p5.py create mode 100644 Mobile-Robot-Foundation/p5/.idea/workspace.xml create mode 100644 Mobile-Robot-Foundation/p6/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p6/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p6/.idea/p6.iml create mode 100644 Mobile-Robot-Foundation/p6/.idea/p6.py create mode 100644 Mobile-Robot-Foundation/p6/.idea/workspace.xml create mode 100644 Mobile-Robot-Foundation/p7/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p7/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p7/.idea/p7.iml create mode 100644 Mobile-Robot-Foundation/p7/.idea/p7.py create mode 100644 Mobile-Robot-Foundation/p7/.idea/workspace.xml create mode 100644 Mobile-Robot-Foundation/p8/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p8/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p8/.idea/p8.iml create mode 100644 Mobile-Robot-Foundation/p8/.idea/p8.py create mode 100644 Mobile-Robot-Foundation/p8/.idea/workspace.xml create mode 100644 Mobile-Robot-Foundation/p9/.idea/misc.xml create mode 100644 Mobile-Robot-Foundation/p9/.idea/modules.xml create mode 100644 Mobile-Robot-Foundation/p9/.idea/p9.iml create mode 100644 Mobile-Robot-Foundation/p9/.idea/p9.py create mode 100644 Mobile-Robot-Foundation/p9/.idea/workspace.xml diff --git a/Mobile-Robot-Foundation/p1/.idea/misc.xml b/Mobile-Robot-Foundation/p1/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p1/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p1/.idea/modules.xml b/Mobile-Robot-Foundation/p1/.idea/modules.xml new file mode 100644 index 0000000..e1c3031 --- /dev/null +++ b/Mobile-Robot-Foundation/p1/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p1/.idea/p1.iml b/Mobile-Robot-Foundation/p1/.idea/p1.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p1/.idea/p1.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p1/.idea/p1.py b/Mobile-Robot-Foundation/p1/.idea/p1.py new file mode 100644 index 0000000..7d7622a --- /dev/null +++ b/Mobile-Robot-Foundation/p1/.idea/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) diff --git a/Mobile-Robot-Foundation/p1/.idea/workspace.xml b/Mobile-Robot-Foundation/p1/.idea/workspace.xml new file mode 100644 index 0000000..ca21f17 --- /dev/null +++ b/Mobile-Robot-Foundation/p1/.idea/workspace.xml @@ -0,0 +1,321 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509008167442 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p10/.idea/misc.xml b/Mobile-Robot-Foundation/p10/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p10/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p10/.idea/modules.xml b/Mobile-Robot-Foundation/p10/.idea/modules.xml new file mode 100644 index 0000000..1c35428 --- /dev/null +++ b/Mobile-Robot-Foundation/p10/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p10/.idea/p10.iml b/Mobile-Robot-Foundation/p10/.idea/p10.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p10/.idea/p10.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p10/.idea/p10.py b/Mobile-Robot-Foundation/p10/.idea/p10.py new file mode 100644 index 0000000..7d35ca4 --- /dev/null +++ b/Mobile-Robot-Foundation/p10/.idea/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) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Mobile-Robot-Foundation/p10/.idea/workspace.xml b/Mobile-Robot-Foundation/p10/.idea/workspace.xml new file mode 100644 index 0000000..3fd8219 --- /dev/null +++ b/Mobile-Robot-Foundation/p10/.idea/workspace.xml @@ -0,0 +1,187 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509325324839 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p2/.idea/misc.xml b/Mobile-Robot-Foundation/p2/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p2/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p2/.idea/modules.xml b/Mobile-Robot-Foundation/p2/.idea/modules.xml new file mode 100644 index 0000000..b0262c3 --- /dev/null +++ b/Mobile-Robot-Foundation/p2/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p2/.idea/p2.iml b/Mobile-Robot-Foundation/p2/.idea/p2.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p2/.idea/p2.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p2/.idea/p2.py b/Mobile-Robot-Foundation/p2/.idea/p2.py new file mode 100644 index 0000000..3048740 --- /dev/null +++ b/Mobile-Robot-Foundation/p2/.idea/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() diff --git a/Mobile-Robot-Foundation/p2/.idea/workspace.xml b/Mobile-Robot-Foundation/p2/.idea/workspace.xml new file mode 100644 index 0000000..42de739 --- /dev/null +++ b/Mobile-Robot-Foundation/p2/.idea/workspace.xml @@ -0,0 +1,190 @@ + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509008747122 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p3/.idea/misc.xml b/Mobile-Robot-Foundation/p3/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p3/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p3/.idea/modules.xml b/Mobile-Robot-Foundation/p3/.idea/modules.xml new file mode 100644 index 0000000..ac30163 --- /dev/null +++ b/Mobile-Robot-Foundation/p3/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p3/.idea/p3.iml b/Mobile-Robot-Foundation/p3/.idea/p3.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p3/.idea/p3.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p3/.idea/p3.py b/Mobile-Robot-Foundation/p3/.idea/p3.py new file mode 100644 index 0000000..3dfbd4e --- /dev/null +++ b/Mobile-Robot-Foundation/p3/.idea/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() diff --git a/Mobile-Robot-Foundation/p3/.idea/workspace.xml b/Mobile-Robot-Foundation/p3/.idea/workspace.xml new file mode 100644 index 0000000..27e091a --- /dev/null +++ b/Mobile-Robot-Foundation/p3/.idea/workspace.xml @@ -0,0 +1,185 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509107360330 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p4/.idea/misc.xml b/Mobile-Robot-Foundation/p4/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p4/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p4/.idea/modules.xml b/Mobile-Robot-Foundation/p4/.idea/modules.xml new file mode 100644 index 0000000..a1ba0bc --- /dev/null +++ b/Mobile-Robot-Foundation/p4/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p4/.idea/p4.iml b/Mobile-Robot-Foundation/p4/.idea/p4.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p4/.idea/p4.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p4/.idea/p4.py b/Mobile-Robot-Foundation/p4/.idea/p4.py new file mode 100644 index 0000000..f0f7ef4 --- /dev/null +++ b/Mobile-Robot-Foundation/p4/.idea/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) diff --git a/Mobile-Robot-Foundation/p4/.idea/workspace.xml b/Mobile-Robot-Foundation/p4/.idea/workspace.xml new file mode 100644 index 0000000..5435988 --- /dev/null +++ b/Mobile-Robot-Foundation/p4/.idea/workspace.xml @@ -0,0 +1,163 @@ + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509116951671 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p5/.idea/misc.xml b/Mobile-Robot-Foundation/p5/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p5/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p5/.idea/modules.xml b/Mobile-Robot-Foundation/p5/.idea/modules.xml new file mode 100644 index 0000000..2f041d3 --- /dev/null +++ b/Mobile-Robot-Foundation/p5/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p5/.idea/p5.iml b/Mobile-Robot-Foundation/p5/.idea/p5.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p5/.idea/p5.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p5/.idea/p5.py b/Mobile-Robot-Foundation/p5/.idea/p5.py new file mode 100644 index 0000000..466dbc4 --- /dev/null +++ b/Mobile-Robot-Foundation/p5/.idea/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) diff --git a/Mobile-Robot-Foundation/p5/.idea/workspace.xml b/Mobile-Robot-Foundation/p5/.idea/workspace.xml new file mode 100644 index 0000000..e2a2590 --- /dev/null +++ b/Mobile-Robot-Foundation/p5/.idea/workspace.xml @@ -0,0 +1,207 @@ + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509117756532 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p6/.idea/misc.xml b/Mobile-Robot-Foundation/p6/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p6/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p6/.idea/modules.xml b/Mobile-Robot-Foundation/p6/.idea/modules.xml new file mode 100644 index 0000000..9e174be --- /dev/null +++ b/Mobile-Robot-Foundation/p6/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p6/.idea/p6.iml b/Mobile-Robot-Foundation/p6/.idea/p6.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p6/.idea/p6.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p6/.idea/p6.py b/Mobile-Robot-Foundation/p6/.idea/p6.py new file mode 100644 index 0000000..b7a11e7 --- /dev/null +++ b/Mobile-Robot-Foundation/p6/.idea/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) + + + diff --git a/Mobile-Robot-Foundation/p6/.idea/workspace.xml b/Mobile-Robot-Foundation/p6/.idea/workspace.xml new file mode 100644 index 0000000..6465e91 --- /dev/null +++ b/Mobile-Robot-Foundation/p6/.idea/workspace.xml @@ -0,0 +1,147 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509155687819 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/misc.xml b/Mobile-Robot-Foundation/p7/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p7/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/modules.xml b/Mobile-Robot-Foundation/p7/.idea/modules.xml new file mode 100644 index 0000000..d204a9d --- /dev/null +++ b/Mobile-Robot-Foundation/p7/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/p7.iml b/Mobile-Robot-Foundation/p7/.idea/p7.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p7/.idea/p7.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/p7.py b/Mobile-Robot-Foundation/p7/.idea/p7.py new file mode 100644 index 0000000..4daff53 --- /dev/null +++ b/Mobile-Robot-Foundation/p7/.idea/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() \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/workspace.xml b/Mobile-Robot-Foundation/p7/.idea/workspace.xml new file mode 100644 index 0000000..c0aea40 --- /dev/null +++ b/Mobile-Robot-Foundation/p7/.idea/workspace.xml @@ -0,0 +1,181 @@ + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509164964972 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p8/.idea/misc.xml b/Mobile-Robot-Foundation/p8/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p8/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p8/.idea/modules.xml b/Mobile-Robot-Foundation/p8/.idea/modules.xml new file mode 100644 index 0000000..d657844 --- /dev/null +++ b/Mobile-Robot-Foundation/p8/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p8/.idea/p8.iml b/Mobile-Robot-Foundation/p8/.idea/p8.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p8/.idea/p8.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p8/.idea/p8.py b/Mobile-Robot-Foundation/p8/.idea/p8.py new file mode 100644 index 0000000..2de9a94 --- /dev/null +++ b/Mobile-Robot-Foundation/p8/.idea/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') + + + + + + + + diff --git a/Mobile-Robot-Foundation/p8/.idea/workspace.xml b/Mobile-Robot-Foundation/p8/.idea/workspace.xml new file mode 100644 index 0000000..2164ba3 --- /dev/null +++ b/Mobile-Robot-Foundation/p8/.idea/workspace.xml @@ -0,0 +1,297 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509192875588 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p9/.idea/misc.xml b/Mobile-Robot-Foundation/p9/.idea/misc.xml new file mode 100644 index 0000000..b5eb451 --- /dev/null +++ b/Mobile-Robot-Foundation/p9/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p9/.idea/modules.xml b/Mobile-Robot-Foundation/p9/.idea/modules.xml new file mode 100644 index 0000000..055813e --- /dev/null +++ b/Mobile-Robot-Foundation/p9/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p9/.idea/p9.iml b/Mobile-Robot-Foundation/p9/.idea/p9.iml new file mode 100644 index 0000000..6f63a63 --- /dev/null +++ b/Mobile-Robot-Foundation/p9/.idea/p9.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p9/.idea/p9.py b/Mobile-Robot-Foundation/p9/.idea/p9.py new file mode 100644 index 0000000..d1a8c81 --- /dev/null +++ b/Mobile-Robot-Foundation/p9/.idea/p9.py @@ -0,0 +1,52 @@ +from matplotlib import pyplot as plt +import numpy as np +import math + +#二维数组的索引函数，功能：在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 move(p,u,pMoveCorrect): + nCol = np.size(p, 1) # 列数,此处与'matlab'对应的代码有所不同 + nRow = np.size(p, 0) # 行数,此处与'matlab'对应的代码有所不同 + q = np.zeros((nRow, nCol)) + for i in range(nRow): + for j in range(nCol): + q[i][j] = pMoveCorrect * p[(i-u[0]) % nRow][(j-u[1]) % nCol] + (1 - pMoveCorrect) * p[i][j] + 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对应的代码有所不同 +stop = np.array([0, 0]) +right = np.array([0, 1]) +left = np.array([0,-1]) +down = np.array([1, 0]) +up = np.array([-1,0]) +pMoveCorrect = 0.8 +pStart = 0.7 +p=np.ones((nRow,nCol)) +p = (1 - pStart) / (nRow * nCol - 1) * p +p[2][1]=pStart +print('The Prior:') +print(p) +motions = right +q = move(p,motions,pMoveCorrect) +print('The probability after moving:') +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) + + diff --git a/Mobile-Robot-Foundation/p9/.idea/workspace.xml b/Mobile-Robot-Foundation/p9/.idea/workspace.xml new file mode 100644 index 0000000..4b1bb85 --- /dev/null +++ b/Mobile-Robot-Foundation/p9/.idea/workspace.xml @@ -0,0 +1,179 @@ + + + + + + + + + + + + + + + + + + + + + true + DEFINITION_ORDER + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1509199150190 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 4263cb5f5fb53c5b4eef326f2554a950e5c7867e Mon Sep 17 00:00:00 2001 From: liyaochong <823098528@qq.com> Date: Wed, 29 Nov 2017 12:57:01 +0800 Subject: [PATCH 2/3] code by python --- Mobile-Robot-Foundation/p1/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p1/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p1/.idea/p1.iml | 12 - Mobile-Robot-Foundation/p1/.idea/p1.py | 8 - .../p1/.idea/workspace.xml | 321 ------------------ Mobile-Robot-Foundation/p10/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p10/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p10/.idea/p10.iml | 12 - Mobile-Robot-Foundation/p10/.idea/p10.py | 85 ----- .../p10/.idea/workspace.xml | 187 ---------- Mobile-Robot-Foundation/p2/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p2/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p2/.idea/p2.iml | 12 - Mobile-Robot-Foundation/p2/.idea/p2.py | 64 ---- .../p2/.idea/workspace.xml | 190 ----------- Mobile-Robot-Foundation/p3/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p3/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p3/.idea/p3.iml | 12 - Mobile-Robot-Foundation/p3/.idea/p3.py | 53 --- .../p3/.idea/workspace.xml | 185 ---------- Mobile-Robot-Foundation/p4/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p4/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p4/.idea/p4.iml | 12 - Mobile-Robot-Foundation/p4/.idea/p4.py | 7 - .../p4/.idea/workspace.xml | 163 --------- Mobile-Robot-Foundation/p5/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p5/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p5/.idea/p5.iml | 12 - Mobile-Robot-Foundation/p5/.idea/p5.py | 12 - .../p5/.idea/workspace.xml | 207 ----------- Mobile-Robot-Foundation/p6/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p6/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p6/.idea/p6.iml | 12 - Mobile-Robot-Foundation/p6/.idea/p6.py | 27 -- .../p6/.idea/workspace.xml | 147 -------- Mobile-Robot-Foundation/p7/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p7/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p7/.idea/p7.iml | 12 - Mobile-Robot-Foundation/p7/.idea/p7.py | 35 -- .../p7/.idea/workspace.xml | 181 ---------- Mobile-Robot-Foundation/p8/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p8/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p8/.idea/p8.iml | 12 - Mobile-Robot-Foundation/p8/.idea/p8.py | 67 ---- .../p8/.idea/workspace.xml | 297 ---------------- Mobile-Robot-Foundation/p9/.idea/misc.xml | 4 - Mobile-Robot-Foundation/p9/.idea/modules.xml | 8 - Mobile-Robot-Foundation/p9/.idea/p9.iml | 12 - Mobile-Robot-Foundation/p9/.idea/p9.py | 52 --- .../p9/.idea/workspace.xml | 179 ---------- 50 files changed, 2707 deletions(-) delete mode 100644 Mobile-Robot-Foundation/p1/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p1/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p1/.idea/p1.iml delete mode 100644 Mobile-Robot-Foundation/p1/.idea/p1.py delete mode 100644 Mobile-Robot-Foundation/p1/.idea/workspace.xml delete mode 100644 Mobile-Robot-Foundation/p10/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p10/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p10/.idea/p10.iml delete mode 100644 Mobile-Robot-Foundation/p10/.idea/p10.py delete mode 100644 Mobile-Robot-Foundation/p10/.idea/workspace.xml delete mode 100644 Mobile-Robot-Foundation/p2/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p2/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p2/.idea/p2.iml delete mode 100644 Mobile-Robot-Foundation/p2/.idea/p2.py delete mode 100644 Mobile-Robot-Foundation/p2/.idea/workspace.xml delete mode 100644 Mobile-Robot-Foundation/p3/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p3/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p3/.idea/p3.iml delete mode 100644 Mobile-Robot-Foundation/p3/.idea/p3.py delete mode 100644 Mobile-Robot-Foundation/p3/.idea/workspace.xml delete mode 100644 Mobile-Robot-Foundation/p4/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p4/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p4/.idea/p4.iml delete mode 100644 Mobile-Robot-Foundation/p4/.idea/p4.py delete mode 100644 Mobile-Robot-Foundation/p4/.idea/workspace.xml delete mode 100644 Mobile-Robot-Foundation/p5/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p5/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p5/.idea/p5.iml delete mode 100644 Mobile-Robot-Foundation/p5/.idea/p5.py delete mode 100644 Mobile-Robot-Foundation/p5/.idea/workspace.xml delete mode 100644 Mobile-Robot-Foundation/p6/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p6/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p6/.idea/p6.iml delete mode 100644 Mobile-Robot-Foundation/p6/.idea/p6.py delete mode 100644 Mobile-Robot-Foundation/p6/.idea/workspace.xml delete mode 100644 Mobile-Robot-Foundation/p7/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p7/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p7/.idea/p7.iml delete mode 100644 Mobile-Robot-Foundation/p7/.idea/p7.py delete mode 100644 Mobile-Robot-Foundation/p7/.idea/workspace.xml delete mode 100644 Mobile-Robot-Foundation/p8/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p8/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p8/.idea/p8.iml delete mode 100644 Mobile-Robot-Foundation/p8/.idea/p8.py delete mode 100644 Mobile-Robot-Foundation/p8/.idea/workspace.xml delete mode 100644 Mobile-Robot-Foundation/p9/.idea/misc.xml delete mode 100644 Mobile-Robot-Foundation/p9/.idea/modules.xml delete mode 100644 Mobile-Robot-Foundation/p9/.idea/p9.iml delete mode 100644 Mobile-Robot-Foundation/p9/.idea/p9.py delete mode 100644 Mobile-Robot-Foundation/p9/.idea/workspace.xml diff --git a/Mobile-Robot-Foundation/p1/.idea/misc.xml b/Mobile-Robot-Foundation/p1/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p1/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p1/.idea/modules.xml b/Mobile-Robot-Foundation/p1/.idea/modules.xml deleted file mode 100644 index e1c3031..0000000 --- a/Mobile-Robot-Foundation/p1/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p1/.idea/p1.iml b/Mobile-Robot-Foundation/p1/.idea/p1.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p1/.idea/p1.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p1/.idea/p1.py b/Mobile-Robot-Foundation/p1/.idea/p1.py deleted file mode 100644 index 7d7622a..0000000 --- a/Mobile-Robot-Foundation/p1/.idea/p1.py +++ /dev/null @@ -1,8 +0,0 @@ -import numpy as np -n=10 -A=np.zeros(n) -# A=[] -for i in range(n): - a=1/n - A[i]=a -print(A) diff --git a/Mobile-Robot-Foundation/p1/.idea/workspace.xml b/Mobile-Robot-Foundation/p1/.idea/workspace.xml deleted file mode 100644 index ca21f17..0000000 --- a/Mobile-Robot-Foundation/p1/.idea/workspace.xml +++ /dev/null @@ -1,321 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509008167442 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p10/.idea/misc.xml b/Mobile-Robot-Foundation/p10/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p10/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p10/.idea/modules.xml b/Mobile-Robot-Foundation/p10/.idea/modules.xml deleted file mode 100644 index 1c35428..0000000 --- a/Mobile-Robot-Foundation/p10/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p10/.idea/p10.iml b/Mobile-Robot-Foundation/p10/.idea/p10.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p10/.idea/p10.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p10/.idea/p10.py b/Mobile-Robot-Foundation/p10/.idea/p10.py deleted file mode 100644 index 7d35ca4..0000000 --- a/Mobile-Robot-Foundation/p10/.idea/p10.py +++ /dev/null @@ -1,85 +0,0 @@ -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) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Mobile-Robot-Foundation/p10/.idea/workspace.xml b/Mobile-Robot-Foundation/p10/.idea/workspace.xml deleted file mode 100644 index 3fd8219..0000000 --- a/Mobile-Robot-Foundation/p10/.idea/workspace.xml +++ /dev/null @@ -1,187 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509325324839 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p2/.idea/misc.xml b/Mobile-Robot-Foundation/p2/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p2/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p2/.idea/modules.xml b/Mobile-Robot-Foundation/p2/.idea/modules.xml deleted file mode 100644 index b0262c3..0000000 --- a/Mobile-Robot-Foundation/p2/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p2/.idea/p2.iml b/Mobile-Robot-Foundation/p2/.idea/p2.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p2/.idea/p2.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p2/.idea/p2.py b/Mobile-Robot-Foundation/p2/.idea/p2.py deleted file mode 100644 index 3048740..0000000 --- a/Mobile-Robot-Foundation/p2/.idea/p2.py +++ /dev/null @@ -1,64 +0,0 @@ -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() diff --git a/Mobile-Robot-Foundation/p2/.idea/workspace.xml b/Mobile-Robot-Foundation/p2/.idea/workspace.xml deleted file mode 100644 index 42de739..0000000 --- a/Mobile-Robot-Foundation/p2/.idea/workspace.xml +++ /dev/null @@ -1,190 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509008747122 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p3/.idea/misc.xml b/Mobile-Robot-Foundation/p3/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p3/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p3/.idea/modules.xml b/Mobile-Robot-Foundation/p3/.idea/modules.xml deleted file mode 100644 index ac30163..0000000 --- a/Mobile-Robot-Foundation/p3/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p3/.idea/p3.iml b/Mobile-Robot-Foundation/p3/.idea/p3.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p3/.idea/p3.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p3/.idea/p3.py b/Mobile-Robot-Foundation/p3/.idea/p3.py deleted file mode 100644 index 3dfbd4e..0000000 --- a/Mobile-Robot-Foundation/p3/.idea/p3.py +++ /dev/null @@ -1,53 +0,0 @@ -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() diff --git a/Mobile-Robot-Foundation/p3/.idea/workspace.xml b/Mobile-Robot-Foundation/p3/.idea/workspace.xml deleted file mode 100644 index 27e091a..0000000 --- a/Mobile-Robot-Foundation/p3/.idea/workspace.xml +++ /dev/null @@ -1,185 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509107360330 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p4/.idea/misc.xml b/Mobile-Robot-Foundation/p4/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p4/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p4/.idea/modules.xml b/Mobile-Robot-Foundation/p4/.idea/modules.xml deleted file mode 100644 index a1ba0bc..0000000 --- a/Mobile-Robot-Foundation/p4/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p4/.idea/p4.iml b/Mobile-Robot-Foundation/p4/.idea/p4.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p4/.idea/p4.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p4/.idea/p4.py b/Mobile-Robot-Foundation/p4/.idea/p4.py deleted file mode 100644 index f0f7ef4..0000000 --- a/Mobile-Robot-Foundation/p4/.idea/p4.py +++ /dev/null @@ -1,7 +0,0 @@ -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) diff --git a/Mobile-Robot-Foundation/p4/.idea/workspace.xml b/Mobile-Robot-Foundation/p4/.idea/workspace.xml deleted file mode 100644 index 5435988..0000000 --- a/Mobile-Robot-Foundation/p4/.idea/workspace.xml +++ /dev/null @@ -1,163 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509116951671 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p5/.idea/misc.xml b/Mobile-Robot-Foundation/p5/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p5/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p5/.idea/modules.xml b/Mobile-Robot-Foundation/p5/.idea/modules.xml deleted file mode 100644 index 2f041d3..0000000 --- a/Mobile-Robot-Foundation/p5/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p5/.idea/p5.iml b/Mobile-Robot-Foundation/p5/.idea/p5.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p5/.idea/p5.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p5/.idea/p5.py b/Mobile-Robot-Foundation/p5/.idea/p5.py deleted file mode 100644 index 466dbc4..0000000 --- a/Mobile-Robot-Foundation/p5/.idea/p5.py +++ /dev/null @@ -1,12 +0,0 @@ -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) diff --git a/Mobile-Robot-Foundation/p5/.idea/workspace.xml b/Mobile-Robot-Foundation/p5/.idea/workspace.xml deleted file mode 100644 index e2a2590..0000000 --- a/Mobile-Robot-Foundation/p5/.idea/workspace.xml +++ /dev/null @@ -1,207 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509117756532 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p6/.idea/misc.xml b/Mobile-Robot-Foundation/p6/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p6/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p6/.idea/modules.xml b/Mobile-Robot-Foundation/p6/.idea/modules.xml deleted file mode 100644 index 9e174be..0000000 --- a/Mobile-Robot-Foundation/p6/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p6/.idea/p6.iml b/Mobile-Robot-Foundation/p6/.idea/p6.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p6/.idea/p6.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p6/.idea/p6.py b/Mobile-Robot-Foundation/p6/.idea/p6.py deleted file mode 100644 index b7a11e7..0000000 --- a/Mobile-Robot-Foundation/p6/.idea/p6.py +++ /dev/null @@ -1,27 +0,0 @@ -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) - - - diff --git a/Mobile-Robot-Foundation/p6/.idea/workspace.xml b/Mobile-Robot-Foundation/p6/.idea/workspace.xml deleted file mode 100644 index 6465e91..0000000 --- a/Mobile-Robot-Foundation/p6/.idea/workspace.xml +++ /dev/null @@ -1,147 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509155687819 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/misc.xml b/Mobile-Robot-Foundation/p7/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p7/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/modules.xml b/Mobile-Robot-Foundation/p7/.idea/modules.xml deleted file mode 100644 index d204a9d..0000000 --- a/Mobile-Robot-Foundation/p7/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/p7.iml b/Mobile-Robot-Foundation/p7/.idea/p7.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p7/.idea/p7.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/p7.py b/Mobile-Robot-Foundation/p7/.idea/p7.py deleted file mode 100644 index 4daff53..0000000 --- a/Mobile-Robot-Foundation/p7/.idea/p7.py +++ /dev/null @@ -1,35 +0,0 @@ -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() \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p7/.idea/workspace.xml b/Mobile-Robot-Foundation/p7/.idea/workspace.xml deleted file mode 100644 index c0aea40..0000000 --- a/Mobile-Robot-Foundation/p7/.idea/workspace.xml +++ /dev/null @@ -1,181 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509164964972 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p8/.idea/misc.xml b/Mobile-Robot-Foundation/p8/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p8/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p8/.idea/modules.xml b/Mobile-Robot-Foundation/p8/.idea/modules.xml deleted file mode 100644 index d657844..0000000 --- a/Mobile-Robot-Foundation/p8/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p8/.idea/p8.iml b/Mobile-Robot-Foundation/p8/.idea/p8.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p8/.idea/p8.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p8/.idea/p8.py b/Mobile-Robot-Foundation/p8/.idea/p8.py deleted file mode 100644 index 2de9a94..0000000 --- a/Mobile-Robot-Foundation/p8/.idea/p8.py +++ /dev/null @@ -1,67 +0,0 @@ -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') - - - - - - - - diff --git a/Mobile-Robot-Foundation/p8/.idea/workspace.xml b/Mobile-Robot-Foundation/p8/.idea/workspace.xml deleted file mode 100644 index 2164ba3..0000000 --- a/Mobile-Robot-Foundation/p8/.idea/workspace.xml +++ /dev/null @@ -1,297 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509192875588 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p9/.idea/misc.xml b/Mobile-Robot-Foundation/p9/.idea/misc.xml deleted file mode 100644 index b5eb451..0000000 --- a/Mobile-Robot-Foundation/p9/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p9/.idea/modules.xml b/Mobile-Robot-Foundation/p9/.idea/modules.xml deleted file mode 100644 index 055813e..0000000 --- a/Mobile-Robot-Foundation/p9/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p9/.idea/p9.iml b/Mobile-Robot-Foundation/p9/.idea/p9.iml deleted file mode 100644 index 6f63a63..0000000 --- a/Mobile-Robot-Foundation/p9/.idea/p9.iml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p9/.idea/p9.py b/Mobile-Robot-Foundation/p9/.idea/p9.py deleted file mode 100644 index d1a8c81..0000000 --- a/Mobile-Robot-Foundation/p9/.idea/p9.py +++ /dev/null @@ -1,52 +0,0 @@ -from matplotlib import pyplot as plt -import numpy as np -import math - -#二维数组的索引函数，功能：在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 move(p,u,pMoveCorrect): - nCol = np.size(p, 1) # 列数,此处与'matlab'对应的代码有所不同 - nRow = np.size(p, 0) # 行数,此处与'matlab'对应的代码有所不同 - q = np.zeros((nRow, nCol)) - for i in range(nRow): - for j in range(nCol): - q[i][j] = pMoveCorrect * p[(i-u[0]) % nRow][(j-u[1]) % nCol] + (1 - pMoveCorrect) * p[i][j] - 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对应的代码有所不同 -stop = np.array([0, 0]) -right = np.array([0, 1]) -left = np.array([0,-1]) -down = np.array([1, 0]) -up = np.array([-1,0]) -pMoveCorrect = 0.8 -pStart = 0.7 -p=np.ones((nRow,nCol)) -p = (1 - pStart) / (nRow * nCol - 1) * p -p[2][1]=pStart -print('The Prior:') -print(p) -motions = right -q = move(p,motions,pMoveCorrect) -print('The probability after moving:') -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) - - diff --git a/Mobile-Robot-Foundation/p9/.idea/workspace.xml b/Mobile-Robot-Foundation/p9/.idea/workspace.xml deleted file mode 100644 index 4b1bb85..0000000 --- a/Mobile-Robot-Foundation/p9/.idea/workspace.xml +++ /dev/null @@ -1,179 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - true - DEFINITION_ORDER - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1509199150190 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file From 4acea6950a26d4f490fb20fecc107cf69917ca14 Mon Sep 17 00:00:00 2001 From: liyaochong <823098528@qq.com> Date: Wed, 29 Nov 2017 13:03:39 +0800 Subject: [PATCH 3/3] code(p1-p10) by python --- Mobile-Robot-Foundation/p1/p1.py | 8 +++ Mobile-Robot-Foundation/p10/p10.py | 85 ++++++++++++++++++++++++++++++ Mobile-Robot-Foundation/p2/p2.py | 64 ++++++++++++++++++++++ Mobile-Robot-Foundation/p3/p3.py | 53 +++++++++++++++++++ Mobile-Robot-Foundation/p4/p4.py | 7 +++ Mobile-Robot-Foundation/p5/p5.py | 12 +++++ Mobile-Robot-Foundation/p6/p6.py | 27 ++++++++++ Mobile-Robot-Foundation/p7/p7.py | 35 ++++++++++++ Mobile-Robot-Foundation/p8/p8.py | 67 +++++++++++++++++++++++ Mobile-Robot-Foundation/p9/p9.py | 52 ++++++++++++++++++ 10 files changed, 410 insertions(+) create mode 100644 Mobile-Robot-Foundation/p1/p1.py create mode 100644 Mobile-Robot-Foundation/p10/p10.py create mode 100644 Mobile-Robot-Foundation/p2/p2.py create mode 100644 Mobile-Robot-Foundation/p3/p3.py create mode 100644 Mobile-Robot-Foundation/p4/p4.py create mode 100644 Mobile-Robot-Foundation/p5/p5.py create mode 100644 Mobile-Robot-Foundation/p6/p6.py create mode 100644 Mobile-Robot-Foundation/p7/p7.py create mode 100644 Mobile-Robot-Foundation/p8/p8.py create mode 100644 Mobile-Robot-Foundation/p9/p9.py diff --git a/Mobile-Robot-Foundation/p1/p1.py b/Mobile-Robot-Foundation/p1/p1.py new file mode 100644 index 0000000..7d7622a --- /dev/null +++ b/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) diff --git a/Mobile-Robot-Foundation/p10/p10.py b/Mobile-Robot-Foundation/p10/p10.py new file mode 100644 index 0000000..7d35ca4 --- /dev/null +++ b/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) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Mobile-Robot-Foundation/p2/p2.py b/Mobile-Robot-Foundation/p2/p2.py new file mode 100644 index 0000000..3048740 --- /dev/null +++ b/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() diff --git a/Mobile-Robot-Foundation/p3/p3.py b/Mobile-Robot-Foundation/p3/p3.py new file mode 100644 index 0000000..3dfbd4e --- /dev/null +++ b/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() diff --git a/Mobile-Robot-Foundation/p4/p4.py b/Mobile-Robot-Foundation/p4/p4.py new file mode 100644 index 0000000..f0f7ef4 --- /dev/null +++ b/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) diff --git a/Mobile-Robot-Foundation/p5/p5.py b/Mobile-Robot-Foundation/p5/p5.py new file mode 100644 index 0000000..466dbc4 --- /dev/null +++ b/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) diff --git a/Mobile-Robot-Foundation/p6/p6.py b/Mobile-Robot-Foundation/p6/p6.py new file mode 100644 index 0000000..b7a11e7 --- /dev/null +++ b/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) + + + diff --git a/Mobile-Robot-Foundation/p7/p7.py b/Mobile-Robot-Foundation/p7/p7.py new file mode 100644 index 0000000..4daff53 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/Mobile-Robot-Foundation/p8/p8.py b/Mobile-Robot-Foundation/p8/p8.py new file mode 100644 index 0000000..2de9a94 --- /dev/null +++ b/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') + + + + + + + + diff --git a/Mobile-Robot-Foundation/p9/p9.py b/Mobile-Robot-Foundation/p9/p9.py new file mode 100644 index 0000000..d1a8c81 --- /dev/null +++ b/Mobile-Robot-Foundation/p9/p9.py @@ -0,0 +1,52 @@ +from matplotlib import pyplot as plt +import numpy as np +import math + +#二维数组的索引函数，功能：在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 move(p,u,pMoveCorrect): + nCol = np.size(p, 1) # 列数,此处与'matlab'对应的代码有所不同 + nRow = np.size(p, 0) # 行数,此处与'matlab'对应的代码有所不同 + q = np.zeros((nRow, nCol)) + for i in range(nRow): + for j in range(nCol): + q[i][j] = pMoveCorrect * p[(i-u[0]) % nRow][(j-u[1]) % nCol] + (1 - pMoveCorrect) * p[i][j] + 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对应的代码有所不同 +stop = np.array([0, 0]) +right = np.array([0, 1]) +left = np.array([0,-1]) +down = np.array([1, 0]) +up = np.array([-1,0]) +pMoveCorrect = 0.8 +pStart = 0.7 +p=np.ones((nRow,nCol)) +p = (1 - pStart) / (nRow * nCol - 1) * p +p[2][1]=pStart +print('The Prior:') +print(p) +motions = right +q = move(p,motions,pMoveCorrect) +print('The probability after moving:') +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) + +