用python改了p16-p20

Codes.python/p16/p16.py

@@ -0,0 +1,60 @@
+#-*- coding: utf-8 -*-
+
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy.linalg as sci
+import os
+
+def getSigmaEllipse(x, c, nsig, n):
+    if len(x) < 1 or len(c) < 1 or nsig == None or n == None:
+        inc = np.pi / 30
+    else:
+        inc = np.pi * 2 / n
+    r = sci.sqrtm(c)
+    larr = list(np.arange(0, 2 * np.pi + inc, inc))
+    larr.append(0)
+    phi = np.round(larr, decimals=4)
+    #17/12/8 fix bug:np.array([[np.cos(phi)],[np.sin(phi)]]).shape为(2, 1, 102), 进行dot运算时会出现dim错误
+    #故应该用reshape将数组转换为维度为(2, 102)的数组再进行运算
+    tmp = np.reshape(np.array([[np.cos(phi)],[np.sin(phi)]]), (2, 102))
+    a = nsig * np.dot(r,tmp)
+    a[0] = a[0] + x[0]
+    a[1] = a[1] + x[1]
+    return a
+
+
+
+#作图
+def drawimg(x1, y1, x2, y2):
+    plt.plot(x1, y1, 'r-', linewidth = 6)
+    plt.plot(x2, y2, 'xg', linewidth = 6)
+    plt.xlabel('x')
+    plt.ylabel('y')
+    plt.title('sigma_x={:.1f}, sigma_y={:.1f}, rho={:.1f}'.format(sigmax, sigmay, rho))
+    plt.xlim(-5.5, 5.5)
+    plt.ylim(-5.5, 5.5)
+    plt.grid(True)
+    xhis = np.arange(-5, 5.5, 0.5)
+    yhis = np.arange(-5, 6, 1)
+    axis = plt.gca()
+    axis.set_xticks(xhis)
+    axis.set_yticks(yhis)
+    #sava image
+    dirpath = os.path.dirname(__file__)
+    plt.savefig(dirpath + '/sigma.png')
+    plt.show()
+
+mu = [0, 0]
+sigmax = 5
+sigmay = 7
+rho = 0.2
+
+def main():
+    sigmaxy = sigmax * sigmay * rho
+    Sigma = np.array([[sigmax ** 2, sigmaxy], [sigmaxy, sigmay ** 2]])
+    ellipse = getSigmaEllipse(mu, Sigma, 1, 100)
+    drawimg(ellipse[0], ellipse[1], mu[0], mu[1])
+
+
+if __name__ == '__main__':
+    main()

Codes.python/p17/p17.py

@@ -0,0 +1,26 @@
+# -*- coding: utf-8 -*-
+
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy.linalg as sci
+import os
+
+
+
+def gaussian1d(mu, sigma, x):
+    p = 1 / (sigma * np.sqrt(2 * np.pi)) * np.exp(-(x - mu) ** 2 / (2 * sigma ** 2))
+    return p
+x = np.arange(-2, 12.01, 0.01)
+mu = 5
+sigma = 0.5
+p = gaussian1d(mu, sigma, x)
+
+#draw
+plt.plot(x, p, 'b-', linewidth = 4)
+plt.plot([mu, mu], [min(p), max(p)], 'r-', linewidth = 4)
+plt.xlabel('x')
+plt.ylabel('P(x)')
+plt.title('mu = {:.1f}, sigma = {:.1f}'.format(mu, sigma))
+dirpath = os.path.dirname(__file__)
+plt.savefig(dirpath + '/guassian1d.png')
+plt.show()

Codes.python/p18/p18.py

@@ -0,0 +1,99 @@
+# -*- coding: utf-8 -*-
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy.linalg as sci
+import os
+from mpl_toolkits.mplot3d import Axes3D
+import time
+
+#warning: 此程序在进行矩阵运算的时候会花费接近十分钟的时间
+
+def guassian2d(mu, Sigma, x, y):
+    xLen = len(x)
+    yLen = len(y)
+
+    if xLen != yLen:
+        print("The size of x and y should be the same!")
+    if len(mu) != 2:
+        print("The size of mu should be (2 * 1)!")
+    if Sigma[0].all() != Sigma[1].all():
+        print("'The covariance matrix should be symmetric!")
+    if Sigma[0].any() != 2:
+        print("The size of covariance matrix is invalid!")
+    #numpy中用np.finfo(float).eps表示matlab中的eps
+    if sci.det(Sigma) < np.finfo(float).eps:
+        print("Covariance must be positive semidefinite!")
+    pMat = np.zeros((xLen, yLen))
+
+    xMat, yMat = np.meshgrid(x, y)
+    #xMat = xMat.T
+    #yMat = yMat.T
+    i = 0
+    j = 0
+    for i in range(0, xLen):
+        for j in range(0, yLen):
+            xMinMu =  np.array([[[xMat[i, j] - mu[0]], [yMat[i, j] - mu[1]]]])
+            xMinMu = np.reshape(xMinMu, (1, 2))
+            p = np.exp(-0.5 * np.dot(np.dot(xMinMu, sci.inv(Sigma)), xMinMu.T))
+            p = p * sci.det(2 * np.pi * Sigma) ** (-0.5)
+            pMat[i, j] = p
+    return pMat, xMat, yMat 
+
+
+
+def draw3D():
+    fig = plt.figure() 
+    ax = fig.add_subplot(111, projection='3d')
+    p = ax.plot_surface(xMat, yMat, pMat)
+    ax.set_xlabel('x')
+    ax.set_ylabel('y')
+    ax.set_zlabel('Probability')
+    ax.set_xlim(min(x), max(x))
+    ax.set_ylim(min(y), max(y))
+    plt.savefig(dirpath + '\gaussian2d.png')
+
+def convert3Dview():
+    viewfig = plt.figure()
+    axv = viewfig.add_subplot(222, projection='3d')
+    axv.plot_surface(xMat, yMat, pMat)
+    axv.view_init(0, 90)
+    axv.set_xlim(min(x), max(x))
+    axv.set_ylim(min(y), max(y))
+    plt.savefig(dirpath + '\gaussian2dxy.png')
+
+def drawcontour():
+    plt.figure()
+    plt.plot(mu[0], mu[1], 'xg', linewidth = 1)
+    CS = plt.contour(xMat, yMat, pMat)
+    plt.clabel(CS, inline = 1, fontsize = 10)
+    plt.xlabel('x')
+    plt.ylabel('y')
+    plt.xlim(min(x), max(x))
+    plt.ylim(min(y), max(y))
+    plt.savefig(dirpath + '\gaussian2dcontour.png')
+
+mu = [6, 5]
+sigmax = 2
+sigmay = 1
+rho = -0.2
+sigmaxy = sigmax * sigmay * rho
+Sigma = np.array([[sigmax ** 2, sigmaxy], [sigmaxy, sigmay ** 2]])
+x = np.arange(0, 12.01, 0.01)
+y = x
+pMat, xMat, yMat = guassian2d(mu, Sigma, x, y)
+dirpath = os.path.dirname(__file__)
+
+
+def main():
+    starttime = time.time()
+    lasttime = time.time()
+    print('The guassian2d function with numpy to compute the {} size of matrix spend {}'.format(len(pMat) * len(pMat), lasttime - starttime))
+    print('The integration of p with respect to x and y is {:.4f}'.format(np.trapz(y, np.trapz(pMat, x))))
+    draw3D()
+    convert3Dview()
+    drawcontour()
+    plt.show()
+
+
+if __name__ == '__main__':
+    main()

Codes.python/p19/p19.py

@@ -0,0 +1,63 @@
+# -*- coding: utf-8 -*-
+
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy.linalg as sci
+import os
+
+
+
+def gaussian1d(mu, sigma, x):
+    p = 1 / (sigma * np.sqrt(2 * np.pi)) * np.exp(-(x - mu) ** 2 / (2 * sigma ** 2))
+    return p
+
+def convolute1d(fx, fy, x):
+    fxy = np.convolve(fx, fy, 'same')
+    fxy = fxy / np.trapz(fxy, x)
+    return fxy
+
+def product1d(fx, fy, x):
+    fxLen = len(fx)
+    fyLen = len(fy)
+    xLen = len(x)
+    if fxLen != fyLen:
+        print("Probability vectors must be of the same length")
+    if xLen != fxLen:
+        print("X rang and probability vectors must be of the same length")
+    fxy = fx * fy
+    fxy = fxy / np.trapz(fxy, x)
+    return fxy
+
+x = np.arange(-10, 10.01, 0.01)
+prior = gaussian1d(2, 1, x)
+motionModel = gaussian1d(1, 2, x)
+sensorModel = gaussian1d(5, 1, x)
+
+probtAfterMove = convolute1d(motionModel, prior, x)
+probtAfterSense = product1d(sensorModel, probtAfterMove, x)
+
+
+plt.subplot(2, 1, 1)
+plt.plot(x, prior, 'b-', linewidth = 2, label = 'prior')
+plt.plot(x, motionModel, 'g-', linewidth = 2, label = 'motionModel')
+plt.plot(x, probtAfterSense, 'r-', linewidth = 2, label = 'probtAfterSense')
+plt.legend(loc = 'upper right')
+plt.grid(True)
+plt.title('Bayes filtering (Prediction)')
+plt.ylabel('Probability')
+plt.ylim(0, 0.5)
+
+plt.subplot(2, 1, 2)
+plt.plot(x, probtAfterMove, 'r-', linewidth = 2, label = 'probtAfterMove')
+plt.plot(x, sensorModel, 'c-', linewidth = 2, label = 'sensorModel')
+plt.plot(x, probtAfterSense, 'k-', linewidth = 2, label = 'probtAfterSense')
+plt.legend(loc = 'upper left')
+plt.grid(True)
+plt.title('Update')
+plt.ylabel('Probability')
+plt.ylim(0, 0.5)
+
+dirpath = os.path.dirname(__file__)
+plt.savefig(dirpath + '\\bayesfiltering.png')
+
+plt.show()

Codes.python/p20/p20.py

@@ -0,0 +1,60 @@
+# -*- coding: utf-8 -*-
+
+import numpy as np
+import matplotlib.pyplot as plt
+import scipy.linalg as sci
+import os
+from p19 import gaussian1d,convolute1d, product1d
+
+
+def convolute1dParam(muPrior, sdPrior, muMotion, sdMotion):
+    muAfterMove = muPrior + muMotion
+    sdAfterMove = np.sqrt(sdPrior ** 2 + sdMotion ** 2)
+    return muAfterMove, sdAfterMove
+
+def product1dParam(muAfterMove, sdAfterMove, muSensor, sdSensor):
+    muPost = (muAfterMove * sdSensor ** 2 + muSensor * sdAfterMove ** 2) / (sdAfterMove ** 2 + sdSensor ** 2)
+    sdPost = (sdSensor ** 2 * sdAfterMove ** 2) / (sdAfterMove ** 2 + sdSensor ** 2)
+    return muPost, sdPost
+
+x = np.arange(-100, 100.001, 0.001)
+prior = gaussian1d(2, 1, x)
+motionModel = gaussian1d(1, 2, x)
+sensorModel = gaussian1d(5, 1, x)
+
+probAfterMove = convolute1d(motionModel, prior, x)
+probAfterSense = product1d(sensorModel, probAfterMove, x)
+
+muAfterMove, sdAfterMove = convolute1dParam(2, 1, 1, 2)
+muAfterSense, sdAfterSense = product1dParam(muAfterMove, sdAfterMove, 5, 1)
+
+probAfterMoveParam = gaussian1d(muAfterMove, sdAfterMove, x)
+probAfterSenseParam = gaussian1d(muAfterSense, sdAfterSense, x)
+
+probAfterSenseParamSeq = product1d(sensorModel, probAfterMoveParam, x)
+
+plt.subplot(2, 1, 1)
+plt.grid(True)
+plt.plot(x, probAfterMove, 'r-', linewidth = 6, label = 'probAfterMove')
+plt.plot(x, probAfterMoveParam, 'g:', linewidth = 3, label = 'probAfterMoveParam')
+plt.legend(loc = 'upper right')
+plt.title('Bayes filtering (Prediction)')
+plt.ylabel('Probability')
+plt.ylim(0, 0.2)
+plt.xlim(-5, 10)
+
+plt.subplot(2, 1, 2)
+plt.grid(True)
+plt.plot(x, probAfterSense, 'k-', linewidth = 6, label = 'probAfterSense')
+plt.plot(x, probAfterSenseParam, 'c-', linewidth = 3, label = 'probAfterSenseParam')
+plt.plot(x, probAfterSenseParamSeq, 'y-', linewidth = 2, label = 'probAfterSenseParamSeq')
+plt.legend(loc = 'upper right')
+plt.title('Title')
+plt.ylabel('Probability')
+plt.ylim(0, 0.2)
+plt.xlim(-5, 10)
+
+dirpath = os.path.dirname(__file__)
+plt.savefig(dirpath + '\\bayesfiltewithparams.png')
+
+plt.show()

README.md

@@ -2,4 +2,4 @@ The codes and slides for the first year graduate student course, the Fundamental
 
 ## 学生更新记录：
 
-- 霍煜豪 邮箱：591144810@qq.com 学号：201730310153。刚开始学python，改的比较慢到目前才改了16个，而且改的比较烂。。
+- 许瑞青 邮箱：2876030495@qq.com 学号：201730310141。我用python改了p16-p20