diff --git a/Codes.python/p16/p16.py b/Codes.python/p16/p16.py new file mode 100644 index 0000000..df534df --- /dev/null +++ b/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() \ No newline at end of file diff --git a/Codes.python/p17/p17.py b/Codes.python/p17/p17.py new file mode 100644 index 0000000..360464e --- /dev/null +++ b/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() diff --git a/Codes.python/p18/p18.py b/Codes.python/p18/p18.py new file mode 100644 index 0000000..3c8fcaa --- /dev/null +++ b/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() \ No newline at end of file diff --git a/Codes.python/p19/p19.py b/Codes.python/p19/p19.py new file mode 100644 index 0000000..97ac638 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/Codes.python/p20/p20.py b/Codes.python/p20/p20.py new file mode 100644 index 0000000..45044f8 --- /dev/null +++ b/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() diff --git a/README.md b/README.md index c0b2b44..d840311 100644 --- a/README.md +++ b/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