├── .gitattributes ├── Lesson1-Localization └── Problem Set │ └── Localization Program.py ├── Lessson2-Kalman Filters ├── Kalman Matrices.py └── Problem Set │ └── Programming Exercise.py ├── Lessson3-Particle Filters ├── Moving Robot+ADD Noise+.py ├── Problem Set │ ├── Circular Motion.py │ ├── Final Quiz.py │ └── Sensing.py └── Robot Particles+Important Weight+New Particle.py ├── Lessson4-Search ├── First Search Program+ExpansionGrid+Print Path.py ├── Implement Astar.py ├── Left Turn Policy.py ├── Optimum Policy.py ├── Problem Set │ └── Stochastic Motion.py └── Value Program.py ├── Lessson5-PID ├── P controller.py ├── PD controller.py ├── PID controller.py ├── Parameter Optimization.py ├── Path Smoothing.py └── Problem Set │ ├── Constrained Smoothing.py │ ├── Cyclic Smoothing.py │ └── Racetrack Control.py ├── Lessson6-SLAM ├── Confident Measurements.py ├── Expand.py ├── Fun with Parameters.py ├── Implementing SLAM.py ├── Omega and Xi.py ├── Problem Set │ └── Onlion Slam.py └── Segmented CTE.py ├── Practice Exam ├── Question 1-Question 11.jpg └── Warehouse Robot.py ├── Project Runaway RObot ├── Adding Noise.py ├── Chasing with a Plan.py ├── Noiseless Prediction.py ├── The Chase Begins.py ├── The Final Hunt.py ├── kalman.py ├── kalman.pyc ├── matrix.py ├── matrix.pyc ├── robot.py └── robot.pyc └── README.md /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /Lesson1-Localization/Problem Set/Localization Program.py: -------------------------------------------------------------------------------- 1 | # The function localize takes the following arguments: 2 | # 3 | # colors: 4 | # 2D list, each entry either 'R' (for red cell) or 'G' (for green cell) 5 | # 6 | # measurements: 7 | # list of measurements taken by the robot, each entry either 'R' or 'G' 8 | # 9 | # motions: 10 | # list of actions taken by the robot, each entry of the form [dy,dx], 11 | # where dx refers to the change in the x-direction (positive meaning 12 | # movement to the right) and dy refers to the change in the y-direction 13 | # (positive meaning movement downward) 14 | # NOTE: the *first* coordinate is change in y; the *second* coordinate is 15 | # change in x 16 | # 17 | # sensor_right: 18 | # float between 0 and 1, giving the probability that any given 19 | # measurement is correct; the probability that the measurement is 20 | # incorrect is 1-sensor_right 21 | # 22 | # p_move: 23 | # float between 0 and 1, giving the probability that any given movement 24 | # command takes place; the probability that the movement command fails 25 | # (and the robot remains still) is 1-p_move; the robot will NOT overshoot 26 | # its destination in this exercise 27 | # 28 | # The function should RETURN (not just show or print) a 2D list (of the same 29 | # dimensions as colors) that gives the probabilities that the robot occupies 30 | # each cell in the world. 31 | # 32 | # Compute the probabilities by assuming the robot initially has a uniform 33 | # probability of being in any cell. 34 | # 35 | # Also assume that at each step, the robot: 36 | # 1) first makes a movement, 37 | # 2) then takes a measurement. 38 | # 39 | # Motion: 40 | # [0,0] - stay 41 | # [0,1] - right 42 | # [0,-1] - left 43 | # [1,0] - down 44 | # [-1,0] - up 45 | def sense(p, colors, measurement, sensor_right): 46 | q=[[0.0 for i in range(len(colors[0]))] for j in range(len(colors))] 47 | for i in range(len(colors)): 48 | for j in range(len(colors[i])): 49 | hit = (measurement == colors[i][j]) 50 | q[i][j] = p[i][j] * (hit * sensor_right + (1-hit) * (1-sensor_right)) 51 | s = sum([sum(e) for e in q]) 52 | for i in range(len(colors)): 53 | q[i] = [e/s for e in q[i]] 54 | return q 55 | 56 | def move(p, colors, measurement, motion, p_move): 57 | q=[[0.0 for i in range(len(colors[0]))] for j in range(len(colors))] 58 | for i in range(len(colors)): 59 | for j in range(len(colors[i])): 60 | q[i][j] += p[i][j]*(1.0-p_move) 61 | q[(i+motion[0])%len(colors)][(j+motion[1])%len(colors[i])] += p[i][j]*p_move 62 | s = sum([sum(e) for e in q]) 63 | for i in range(len(colors)): 64 | q[i] = [e/s for e in q[i]] 65 | return q 66 | 67 | def localize(colors,measurements,motions,sensor_right,p_move): 68 | # initializes p to a uniform distribution over a grid of the same dimensions as colors 69 | pinit = 1.0 / float(len(colors)) / float(len(colors[0])) 70 | p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))] 71 | for i in range(len(measurements)): 72 | p = move(p, colors, measurements[i], motions[i], p_move) 73 | p = sense(p, colors, measurements[i], sensor_right) 74 | return p 75 | 76 | def show(p): 77 | rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p] 78 | print ('[' + ',\n '.join(rows) + ']') 79 | 80 | ############################################################# 81 | # For the following test case, your output should be 82 | # [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465], 83 | # [0.00715, 0.01017, 0.08696, 0.07988, 0.00935], 84 | # [0.00739, 0.00894, 0.11272, 0.35350, 0.04065], 85 | # [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]] 86 | # (within a tolerance of +/- 0.001 for each entry) 87 | 88 | colors = [['R','G','G','R','R'], 89 | ['R','R','G','R','R'], 90 | ['R','R','G','G','R'], 91 | ['R','R','R','R','R']] 92 | measurements = ['G','G','G','G','G'] 93 | motions = [[0,0],[0,1],[1,0],[1,0],[0,1]] 94 | p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8) 95 | show(p) # displays your answer 96 | -------------------------------------------------------------------------------- /Lessson2-Kalman Filters/Kalman Matrices.py: -------------------------------------------------------------------------------- 1 | # Write a function 'kalman_filter' that implements a multi- 2 | # dimensional Kalman Filter for the example given 3 | 4 | from math import * 5 | 6 | 7 | class matrix: 8 | 9 | # implements basic operations of a matrix class 10 | 11 | def __init__(self, value): 12 | self.value = value 13 | self.dimx = len(value) 14 | self.dimy = len(value[0]) 15 | if value == [[]]: 16 | self.dimx = 0 17 | 18 | def zero(self, dimx, dimy): 19 | # check if valid dimensions 20 | if dimx < 1 or dimy < 1: 21 | raise ValueError, "Invalid size of matrix" 22 | else: 23 | self.dimx = dimx 24 | self.dimy = dimy 25 | self.value = [[0 for row in range(dimy)] for col in range(dimx)] 26 | 27 | def identity(self, dim): 28 | # check if valid dimension 29 | if dim < 1: 30 | raise ValueError, "Invalid size of matrix" 31 | else: 32 | self.dimx = dim 33 | self.dimy = dim 34 | self.value = [[0 for row in range(dim)] for col in range(dim)] 35 | for i in range(dim): 36 | self.value[i][i] = 1 37 | 38 | def show(self): 39 | for i in range(self.dimx): 40 | print(self.value[i]) 41 | print(' ') 42 | 43 | def __add__(self, other): 44 | # check if correct dimensions 45 | if self.dimx != other.dimx or self.dimy != other.dimy: 46 | raise ValueError, "Matrices must be of equal dimensions to add" 47 | else: 48 | # add if correct dimensions 49 | res = matrix([[]]) 50 | res.zero(self.dimx, self.dimy) 51 | for i in range(self.dimx): 52 | for j in range(self.dimy): 53 | res.value[i][j] = self.value[i][j] + other.value[i][j] 54 | return res 55 | 56 | def __sub__(self, other): 57 | # check if correct dimensions 58 | if self.dimx != other.dimx or self.dimy != other.dimy: 59 | raise ValueError, "Matrices must be of equal dimensions to subtract" 60 | else: 61 | # subtract if correct dimensions 62 | res = matrix([[]]) 63 | res.zero(self.dimx, self.dimy) 64 | for i in range(self.dimx): 65 | for j in range(self.dimy): 66 | res.value[i][j] = self.value[i][j] - other.value[i][j] 67 | return res 68 | 69 | def __mul__(self, other): 70 | # check if correct dimensions 71 | if self.dimy != other.dimx: 72 | raise ValueError, "Matrices must be m*n and n*p to multiply" 73 | else: 74 | # multiply if correct dimensions 75 | res = matrix([[]]) 76 | res.zero(self.dimx, other.dimy) 77 | for i in range(self.dimx): 78 | for j in range(other.dimy): 79 | for k in range(self.dimy): 80 | res.value[i][j] += self.value[i][k] * other.value[k][j] 81 | return res 82 | 83 | def transpose(self): 84 | # compute transpose 85 | res = matrix([[]]) 86 | res.zero(self.dimy, self.dimx) 87 | for i in range(self.dimx): 88 | for j in range(self.dimy): 89 | res.value[j][i] = self.value[i][j] 90 | return res 91 | 92 | # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions 93 | 94 | def Cholesky(self, ztol=1.0e-5): 95 | # Computes the upper triangular Cholesky factorization of 96 | # a positive definite matrix. 97 | res = matrix([[]]) 98 | res.zero(self.dimx, self.dimx) 99 | 100 | for i in range(self.dimx): 101 | S = sum([(res.value[k][i])**2 for k in range(i)]) 102 | d = self.value[i][i] - S 103 | if abs(d) < ztol: 104 | res.value[i][i] = 0.0 105 | else: 106 | if d < 0.0: 107 | raise ValueError, "Matrix not positive-definite" 108 | res.value[i][i] = sqrt(d) 109 | for j in range(i+1, self.dimx): 110 | S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)]) 111 | if abs(S) < ztol: 112 | S = 0.0 113 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 114 | return res 115 | 116 | def CholeskyInverse(self): 117 | # Computes inverse of matrix given its Cholesky upper Triangular 118 | # decomposition of matrix. 119 | res = matrix([[]]) 120 | res.zero(self.dimx, self.dimx) 121 | 122 | # Backward step for inverse. 123 | for j in reversed(range(self.dimx)): 124 | tjj = self.value[j][j] 125 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 126 | res.value[j][j] = 1.0/tjj**2 - S/tjj 127 | for i in reversed(range(j)): 128 | res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i] 129 | return res 130 | 131 | def inverse(self): 132 | aux = self.Cholesky() 133 | res = aux.CholeskyInverse() 134 | return res 135 | 136 | def __repr__(self): 137 | return repr(self.value) 138 | 139 | 140 | ######################################## 141 | 142 | # Implement the filter function below 143 | 144 | def kalman_filter(x, P): 145 | for n in range(len(measurements)): 146 | 147 | # measurement update 148 | Z = matrix([[measurements[n]]]) 149 | y = Z-(H*x) 150 | S = H*P*H.transpose() + R 151 | K = P*H.transpose()*S.inverse() 152 | x = x+(K*y) 153 | P = (I - (K*H))*P 154 | # prediction 155 | x = (F*x)+u 156 | P = F*P*F.transpose() 157 | return x,P 158 | 159 | ############################################ 160 | ### use the code below to test your filter! 161 | ############################################ 162 | 163 | measurements = [1, 2, 3] 164 | 165 | x = matrix([[0.], [0.]]) # initial state (location and velocity) 166 | P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty 167 | u = matrix([[0.], [0.]]) # external motion 168 | F = matrix([[1., 1.], [0, 1.]]) # next state function 169 | H = matrix([[1., 0.]]) # measurement function 170 | R = matrix([[1.]]) # measurement uncertainty 171 | I = matrix([[1., 0.], [0., 1.]]) # identity matrix 172 | 173 | print(kalman_filter(x, P)) 174 | # output should be: 175 | # x: [[3.9996664447958645], [0.9999998335552873]] 176 | # P: [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]] 177 | -------------------------------------------------------------------------------- /Lessson2-Kalman Filters/Problem Set/Programming Exercise.py: -------------------------------------------------------------------------------- 1 | # Fill in the matrices P, F, H, R and I at the bottom 2 | # 3 | # This question requires NO CODING, just fill in the 4 | # matrices where indicated. Please do not delete or modify 5 | # any provided code OR comments. Good luck! 6 | 7 | from math import * 8 | 9 | class matrix: 10 | 11 | # implements basic operations of a matrix class 12 | 13 | def __init__(self, value): 14 | self.value = value 15 | self.dimx = len(value) 16 | self.dimy = len(value[0]) 17 | if value == [[]]: 18 | self.dimx = 0 19 | 20 | def zero(self, dimx, dimy): 21 | # check if valid dimensions 22 | if dimx < 1 or dimy < 1: 23 | raise ValueError, "Invalid size of matrix" 24 | else: 25 | self.dimx = dimx 26 | self.dimy = dimy 27 | self.value = [[0 for row in range(dimy)] for col in range(dimx)] 28 | 29 | def identity(self, dim): 30 | # check if valid dimension 31 | if dim < 1: 32 | raise ValueError, "Invalid size of matrix" 33 | else: 34 | self.dimx = dim 35 | self.dimy = dim 36 | self.value = [[0 for row in range(dim)] for col in range(dim)] 37 | for i in range(dim): 38 | self.value[i][i] = 1 39 | 40 | def show(self): 41 | for i in range(self.dimx): 42 | print self.value[i] 43 | print ' ' 44 | 45 | def __add__(self, other): 46 | # check if correct dimensions 47 | if self.dimx != other.dimx or self.dimy != other.dimy: 48 | raise ValueError, "Matrices must be of equal dimensions to add" 49 | else: 50 | # add if correct dimensions 51 | res = matrix([[]]) 52 | res.zero(self.dimx, self.dimy) 53 | for i in range(self.dimx): 54 | for j in range(self.dimy): 55 | res.value[i][j] = self.value[i][j] + other.value[i][j] 56 | return res 57 | 58 | def __sub__(self, other): 59 | # check if correct dimensions 60 | if self.dimx != other.dimx or self.dimy != other.dimy: 61 | raise ValueError, "Matrices must be of equal dimensions to subtract" 62 | else: 63 | # subtract if correct dimensions 64 | res = matrix([[]]) 65 | res.zero(self.dimx, self.dimy) 66 | for i in range(self.dimx): 67 | for j in range(self.dimy): 68 | res.value[i][j] = self.value[i][j] - other.value[i][j] 69 | return res 70 | 71 | def __mul__(self, other): 72 | # check if correct dimensions 73 | if self.dimy != other.dimx: 74 | raise ValueError, "Matrices must be m*n and n*p to multiply" 75 | else: 76 | # subtract if correct dimensions 77 | res = matrix([[]]) 78 | res.zero(self.dimx, other.dimy) 79 | for i in range(self.dimx): 80 | for j in range(other.dimy): 81 | for k in range(self.dimy): 82 | res.value[i][j] += self.value[i][k] * other.value[k][j] 83 | return res 84 | 85 | def transpose(self): 86 | # compute transpose 87 | res = matrix([[]]) 88 | res.zero(self.dimy, self.dimx) 89 | for i in range(self.dimx): 90 | for j in range(self.dimy): 91 | res.value[j][i] = self.value[i][j] 92 | return res 93 | 94 | # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions 95 | 96 | def Cholesky(self, ztol=1.0e-5): 97 | # Computes the upper triangular Cholesky factorization of 98 | # a positive definite matrix. 99 | res = matrix([[]]) 100 | res.zero(self.dimx, self.dimx) 101 | 102 | for i in range(self.dimx): 103 | S = sum([(res.value[k][i])**2 for k in range(i)]) 104 | d = self.value[i][i] - S 105 | if abs(d) < ztol: 106 | res.value[i][i] = 0.0 107 | else: 108 | if d < 0.0: 109 | raise ValueError, "Matrix not positive-definite" 110 | res.value[i][i] = sqrt(d) 111 | for j in range(i+1, self.dimx): 112 | S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)]) 113 | if abs(S) < ztol: 114 | S = 0.0 115 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 116 | return res 117 | 118 | def CholeskyInverse(self): 119 | # Computes inverse of matrix given its Cholesky upper Triangular 120 | # decomposition of matrix. 121 | res = matrix([[]]) 122 | res.zero(self.dimx, self.dimx) 123 | 124 | # Backward step for inverse. 125 | for j in reversed(range(self.dimx)): 126 | tjj = self.value[j][j] 127 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 128 | res.value[j][j] = 1.0/tjj**2 - S/tjj 129 | for i in reversed(range(j)): 130 | res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i] 131 | return res 132 | 133 | def inverse(self): 134 | aux = self.Cholesky() 135 | res = aux.CholeskyInverse() 136 | return res 137 | 138 | def __repr__(self): 139 | return repr(self.value) 140 | 141 | 142 | ######################################## 143 | 144 | def filter(x, P): 145 | for n in range(len(measurements)): 146 | 147 | # prediction 148 | x = (F * x) + u 149 | P = F * P * F.transpose() 150 | 151 | # measurement update 152 | Z = matrix([measurements[n]]) 153 | y = Z.transpose() - (H * x) 154 | S = H * P * H.transpose() + R 155 | K = P * H.transpose() * S.inverse() 156 | x = x + (K * y) 157 | P = (I - (K * H)) * P 158 | 159 | print 'x= ' 160 | x.show() 161 | print 'P= ' 162 | P.show() 163 | 164 | ######################################## 165 | 166 | print "### 4-dimensional example ###" 167 | 168 | measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]] 169 | initial_xy = [4., 12.] 170 | 171 | # measurements = [[1., 4.], [6., 0.], [11., -4.], [16., -8.]] 172 | # initial_xy = [-4., 8.] 173 | 174 | # measurements = [[1., 17.], [1., 15.], [1., 13.], [1., 11.]] 175 | # initial_xy = [1., 19.] 176 | 177 | dt = 0.1 178 | 179 | x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity) 180 | u = matrix([[0.], [0.], [0.], [0.]]) # external motion 181 | 182 | #### DO NOT MODIFY ANYTHING ABOVE HERE #### 183 | #### fill this in, remember to use the matrix() function!: #### 184 | #[x, y, x', y'] 185 | P = matrix([[0.,0.,0.,0.],[0.,0.,0.,0.],[0.,0.,1000.,0.],[0.,0.,0.,1000.]]) # initial uncertainty: 0 for positions x and y, 1000 for the two velocities 186 | F = matrix([[1,0,dt,0],[0,1,0,dt],[0,0,1,0],[0,0,0,1]]) # next state function: generalize the 2d version to 4d 187 | H = matrix([[1,0,0,0],[0,1,0,0]]) # measurement function: reflect the fact that we observe x and y but not the two velocities 188 | R = matrix([[0.1,0.],[0.,0.1]]) # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal 189 | I = matrix([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]) # 4d identity matrix 190 | 191 | ###### DO NOT MODIFY ANYTHING HERE ####### 192 | 193 | filter(x, P) 194 | -------------------------------------------------------------------------------- /Lessson3-Particle Filters/Moving Robot+ADD Noise+.py: -------------------------------------------------------------------------------- 1 | #firstly 2 | # Make a robot called myrobot that starts at 3 | # coordinates 30, 50 heading north (pi/2). 4 | # Have your robot turn clockwise by pi/2, move 5 | # 15 m, and sense. Then have it turn clockwise 6 | # by pi/2 again, move 10 m, and sense again. 7 | #secondly 8 | # Now add noise to your robot as follows: 9 | # forward_noise = 5.0, turn_noise = 0.1, 10 | # sense_noise = 5.0. 11 | # 12 | # Once again, your robot starts at 30, 50, 13 | # heading north (pi/2), then turns clockwise 14 | # by pi/2, moves 15 meters, senses, 15 | # then turns clockwise by pi/2 again, moves 16 | # 10 m, then senses again. 17 | 18 | # Your program should print out the result of 19 | # your two sense measurements. 20 | # 21 | # Don't modify the code below. Please enter 22 | # your code at the bottom. 23 | 24 | from math import * 25 | import random 26 | 27 | 28 | 29 | landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]] 30 | world_size = 100.0 31 | 32 | 33 | class robot: 34 | def __init__(self): 35 | self.x = random.random() * world_size 36 | self.y = random.random() * world_size 37 | self.orientation = random.random() * 2.0 * pi 38 | #self.forward_noise = 0.0; 39 | #self.turn_noise = 0.0; 40 | #self.sense_noise = 0.0; 41 | self.forward_noise = 5.0; 42 | self.turn_noise = 0.1; 43 | self.sense_noise = 5.0; 44 | 45 | def set(self, new_x, new_y, new_orientation): 46 | if new_x < 0 or new_x >= world_size: 47 | raise ValueError, 'X coordinate out of bound' 48 | if new_y < 0 or new_y >= world_size: 49 | raise ValueError, 'Y coordinate out of bound' 50 | if new_orientation < 0 or new_orientation >= 2 * pi: 51 | raise ValueError, 'Orientation must be in [0..2pi]' 52 | self.x = float(new_x) 53 | self.y = float(new_y) 54 | self.orientation = float(new_orientation) 55 | 56 | 57 | def set_noise(self, new_f_noise, new_t_noise, new_s_noise): 58 | # makes it possible to change the noise parameters 59 | # this is often useful in particle filters 60 | self.forward_noise = float(new_f_noise); 61 | self.turn_noise = float(new_t_noise); 62 | self.sense_noise = float(new_s_noise); 63 | 64 | 65 | def sense(self): 66 | Z = [] 67 | for i in range(len(landmarks)): 68 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 69 | dist += random.gauss(0.0, self.sense_noise) 70 | Z.append(dist) 71 | return Z 72 | 73 | 74 | def move(self, turn, forward): 75 | if forward < 0: 76 | raise ValueError, 'Robot cant move backwards' 77 | 78 | # turn, and add randomness to the turning command 79 | orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise) 80 | orientation %= 2 * pi 81 | 82 | # move, and add randomness to the motion command 83 | dist = float(forward) + random.gauss(0.0, self.forward_noise) 84 | x = self.x + (cos(orientation) * dist) 85 | y = self.y + (sin(orientation) * dist) 86 | x %= world_size # cyclic truncate 87 | y %= world_size 88 | 89 | # set particle 90 | res = robot() 91 | res.set(x, y, orientation) 92 | res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise) 93 | return res 94 | 95 | def Gaussian(self, mu, sigma, x): 96 | 97 | # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma 98 | return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) 99 | 100 | 101 | def measurement_prob(self, measurement): 102 | 103 | # calculates how likely a measurement should be 104 | 105 | prob = 1.0; 106 | for i in range(len(landmarks)): 107 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 108 | prob *= self.Gaussian(dist, self.sense_noise, measurement[i]) 109 | return prob 110 | 111 | 112 | 113 | def __repr__(self): 114 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation)) 115 | 116 | 117 | 118 | def eval(r, p): 119 | sum = 0.0; 120 | for i in range(len(p)): # calculate mean error 121 | dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0) 122 | dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0) 123 | err = sqrt(dx * dx + dy * dy) 124 | sum += err 125 | return sum / float(len(p)) 126 | 127 | 128 | 129 | #### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW #### 130 | 131 | myrobot = robot() 132 | myrobot.set(30., 50., pi/2) 133 | print (myrobot.sense()) 134 | myrobot = myrobot.move(-pi/2, 15) 135 | print (myrobot.sense()) 136 | myrobot = myrobot.move(-pi/2, 10) 137 | print (myrobot.sense()) -------------------------------------------------------------------------------- /Lessson3-Particle Filters/Problem Set/Circular Motion.py: -------------------------------------------------------------------------------- 1 | # ----------------- 2 | # USER INSTRUCTIONS 3 | # 4 | # Write a function in the class robot called move() 5 | # 6 | # that takes self and a motion vector (this 7 | # motion vector contains a steering* angle and a 8 | # distance) as input and returns an instance of the class 9 | # robot with the appropriate x, y, and orientation 10 | # for the given motion. 11 | # 12 | # *steering is defined in the video 13 | # which accompanies this problem. 14 | # 15 | # For now, please do NOT add noise to your move function. 16 | # 17 | # Please do not modify anything except where indicated 18 | # below. 19 | # 20 | # There are test cases which you are free to use at the 21 | # bottom. If you uncomment them for testing, make sure you 22 | # re-comment them before you submit. 23 | 24 | from math import * 25 | import random 26 | # -------- 27 | # 28 | # the "world" has 4 landmarks. 29 | # the robot's initial coordinates are somewhere in the square 30 | # represented by the landmarks. 31 | # 32 | # NOTE: Landmark coordinates are given in (y, x) form and NOT 33 | # in the traditional (x, y) format! 34 | 35 | landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks 36 | world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds" 37 | max_steering_angle = pi/4 # You don't need to use this value, but it is good to keep in mind the limitations of a real car. 38 | 39 | # ------------------------------------------------ 40 | # 41 | # this is the robot class 42 | # 43 | 44 | class robot: 45 | 46 | # -------- 47 | 48 | # init: 49 | # creates robot and initializes location/orientation 50 | # 51 | 52 | def __init__(self, length = 10.0): 53 | self.x = random.random() * world_size # initial x position 54 | self.y = random.random() * world_size # initial y position 55 | self.orientation = random.random() * 2.0 * pi # initial orientation 56 | self.length = length # length of robot 57 | self.bearing_noise = 0.0 # initialize bearing noise to zero 58 | self.steering_noise = 0.0 # initialize steering noise to zero 59 | self.distance_noise = 0.0 # initialize distance noise to zero 60 | 61 | def __repr__(self): 62 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation)) 63 | # -------- 64 | # set: 65 | # sets a robot coordinate 66 | # 67 | 68 | def set(self, new_x, new_y, new_orientation): 69 | 70 | if new_orientation < 0 or new_orientation >= 2 * pi: 71 | raise ValueError, 'Orientation must be in [0..2pi]' 72 | self.x = float(new_x) 73 | self.y = float(new_y) 74 | self.orientation = float(new_orientation) 75 | 76 | 77 | # -------- 78 | # set_noise: 79 | # sets the noise parameters 80 | # 81 | 82 | def set_noise(self, new_b_noise, new_s_noise, new_d_noise): 83 | # makes it possible to change the noise parameters 84 | # this is often useful in particle filters 85 | self.bearing_noise = float(new_b_noise) 86 | self.steering_noise = float(new_s_noise) 87 | self.distance_noise = float(new_d_noise) 88 | 89 | ############# ONLY ADD/MODIFY CODE BELOW HERE ################### 90 | 91 | # -------- 92 | # move: 93 | # move along a section of a circular path according to motion 94 | # 95 | 96 | def move(self, motion): # Do not change the name of this function 97 | beta = motion[1]/self.length*tan(motion[0]) 98 | if beta*beta > 0.000001: 99 | R = motion[1]/beta 100 | self.x -= sin(self.orientation)*R-sin(self.orientation+beta)*R 101 | self.y += cos(self.orientation)*R-cos(self.orientation+beta)*R 102 | self.orientation += beta 103 | else: 104 | self.x += motion[1]*cos(motion[0]) 105 | self.y += motion[1]*sin(motion[0]) 106 | self.orientation += beta 107 | 108 | # set particle 109 | result = robot() 110 | result.set(self.x, self.y, self.orientation) 111 | result.set_noise(self.bearing_noise, self.steering_noise, self.distance_noise) 112 | result.length = self.length 113 | return result # make sure your move function returns an instance 114 | # of the robot class with the correct coordinates. 115 | 116 | ############## ONLY ADD/MODIFY CODE ABOVE HERE #################### 117 | 118 | 119 | ## IMPORTANT: You may uncomment the test cases below to test your code. 120 | ## But when you submit this code, your test cases MUST be commented 121 | ## out. Our testing program provides its own code for testing your 122 | ## move function with randomized motion data. 123 | 124 | ## -------- 125 | ## TEST CASE: 126 | ## 127 | ## 1) The following code should print: 128 | ## Robot: [x=0.0 y=0.0 orient=0.0] 129 | ## Robot: [x=10.0 y=0.0 orient=0.0] 130 | ## Robot: [x=19.861 y=1.4333 orient=0.2886] 131 | ## Robot: [x=39.034 y=7.1270 orient=0.2886] 132 | ## 133 | ## 134 | # length = 20. 135 | # bearing_noise = 0.0 136 | # steering_noise = 0.0 137 | # distance_noise = 0.0 138 | 139 | # myrobot = robot(length) 140 | # myrobot.set(0.0, 0.0, 0.0) 141 | # myrobot.set_noise(bearing_noise, steering_noise, distance_noise) 142 | 143 | # motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]] 144 | 145 | # T = len(motions) 146 | 147 | # print 'Robot: ', myrobot 148 | # for t in range(T): 149 | # myrobot = myrobot.move(motions[t]) 150 | # print 'Robot: ', myrobot 151 | ## 152 | ## 153 | 154 | ## IMPORTANT: You may uncomment the test cases below to test your code. 155 | ## But when you submit this code, your test cases MUST be commented 156 | ## out. Our testing program provides its own code for testing your 157 | ## move function with randomized motion data. 158 | 159 | 160 | ## 2) The following code should print: 161 | ## Robot: [x=0.0 y=0.0 orient=0.0] 162 | ## Robot: [x=9.9828 y=0.5063 orient=0.1013] 163 | ## Robot: [x=19.863 y=2.0201 orient=0.2027] 164 | ## Robot: [x=29.539 y=4.5259 orient=0.3040] 165 | ## Robot: [x=38.913 y=7.9979 orient=0.4054] 166 | ## Robot: [x=47.887 y=12.400 orient=0.5067] 167 | ## Robot: [x=56.369 y=17.688 orient=0.6081] 168 | ## Robot: [x=64.273 y=23.807 orient=0.7094] 169 | ## Robot: [x=71.517 y=30.695 orient=0.8108] 170 | ## Robot: [x=78.027 y=38.280 orient=0.9121] 171 | ## Robot: [x=83.736 y=46.485 orient=1.0135] 172 | ## 173 | ## 174 | length = 20. 175 | bearing_noise = 0.0 176 | steering_noise = 0.0 177 | distance_noise = 0.0 178 | 179 | myrobot = robot(length) 180 | myrobot.set(0.0, 0.0, 0.0) 181 | myrobot.set_noise(bearing_noise, steering_noise, distance_noise) 182 | 183 | motions = [[0.2, 10.] for row in range(10)] 184 | 185 | T = len(motions) 186 | 187 | print 'Robot: ', myrobot 188 | for t in range(T): 189 | myrobot = myrobot.move(motions[t]) 190 | print 'Robot: ', myrobot 191 | 192 | ## IMPORTANT: You may uncomment the test cases below to test your code. 193 | ## But when you submit this code, your test cases MUST be commented 194 | ## out. Our testing program provides its own code for testing your 195 | ## move function with randomized motion data. 196 | 197 | 198 | -------------------------------------------------------------------------------- /Lessson3-Particle Filters/Problem Set/Final Quiz.py: -------------------------------------------------------------------------------- 1 | # -------------- 2 | # USER INSTRUCTIONS 3 | # 4 | # Now you will put everything together. 5 | # 6 | # First make sure that your sense and move functions 7 | # work as expected for the test cases provided at the 8 | # bottom of the previous two programming assignments. 9 | # Once you are satisfied, copy your sense and move 10 | # definitions into the robot class on this page, BUT 11 | # now include noise. 12 | # 13 | # A good way to include noise in the sense step is to 14 | # add Gaussian noise, centered at zero with variance 15 | # of self.bearing_noise to each bearing. You can do this 16 | # with the command random.gauss(0, self.bearing_noise) 17 | # 18 | # In the move step, you should make sure that your 19 | # actual steering angle is chosen from a Gaussian 20 | # distribution of steering angles. This distribution 21 | # should be centered at the intended steering angle 22 | # with variance of self.steering_noise. 23 | # 24 | # Feel free to use the included set_noise function. 25 | # 26 | # Please do not modify anything except where indicated 27 | # below. 28 | 29 | from math import * 30 | import random 31 | 32 | # -------- 33 | # 34 | # some top level parameters 35 | # 36 | 37 | max_steering_angle = pi / 4.0 # You do not need to use this value, but keep in mind the limitations of a real car. 38 | bearing_noise = 0.1 # Noise parameter: should be included in sense function. 39 | steering_noise = 0.1 # Noise parameter: should be included in move function. 40 | distance_noise = 5.0 # Noise parameter: should be included in move function. 41 | 42 | tolerance_xy = 15.0 # Tolerance for localization in the x and y directions. 43 | tolerance_orientation = 0.25 # Tolerance for orientation. 44 | 45 | 46 | # -------- 47 | # 48 | # the "world" has 4 landmarks. 49 | # the robot's initial coordinates are somewhere in the square 50 | # represented by the landmarks. 51 | # 52 | # NOTE: Landmark coordinates are given in (y, x) form and NOT 53 | # in the traditional (x, y) format! 54 | 55 | landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) format. 56 | world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds" 57 | 58 | # ------------------------------------------------ 59 | # 60 | # this is the robot class 61 | # 62 | 63 | class robot: 64 | 65 | # -------- 66 | # init: 67 | # creates robot and initializes location/orientation 68 | # 69 | 70 | def __init__(self, length = 20.0): 71 | self.x = random.random() * world_size # initial x position 72 | self.y = random.random() * world_size # initial y position 73 | self.orientation = random.random() * 2.0 * pi # initial orientation 74 | self.length = length # length of robot 75 | self.bearing_noise = 0.0 # initialize bearing noise to zero 76 | self.steering_noise = 0.0 # initialize steering noise to zero 77 | self.distance_noise = 0.0 # initialize distance noise to zero 78 | 79 | # -------- 80 | # set: 81 | # sets a robot coordinate 82 | # 83 | 84 | def set(self, new_x, new_y, new_orientation): 85 | 86 | if new_orientation < 0 or new_orientation >= 2 * pi: 87 | raise ValueError, 'Orientation must be in [0..2pi]' 88 | self.x = float(new_x) 89 | self.y = float(new_y) 90 | self.orientation = float(new_orientation) 91 | 92 | # -------- 93 | # set_noise: 94 | # sets the noise parameters 95 | # 96 | def set_noise(self, new_b_noise, new_s_noise, new_d_noise): 97 | # makes it possible to change the noise parameters 98 | # this is often useful in particle filters 99 | self.bearing_noise = float(new_b_noise) 100 | self.steering_noise = float(new_s_noise) 101 | self.distance_noise = float(new_d_noise) 102 | 103 | # -------- 104 | # measurement_prob 105 | # computes the probability of a measurement 106 | # 107 | 108 | def measurement_prob(self, measurements): 109 | 110 | # calculate the correct measurement 111 | predicted_measurements = self.sense(0) # Our sense function took 0 as an argument to switch off noise. 112 | 113 | 114 | # compute errors 115 | error = 1.0 116 | for i in range(len(measurements)): 117 | error_bearing = abs(measurements[i] - predicted_measurements[i]) 118 | error_bearing = (error_bearing + pi) % (2.0 * pi) - pi # truncate 119 | 120 | 121 | # update Gaussian 122 | error *= (exp(- (error_bearing ** 2) / (self.bearing_noise ** 2) / 2.0) / 123 | sqrt(2.0 * pi * (self.bearing_noise ** 2))) 124 | 125 | return error 126 | 127 | def __repr__(self): #allows us to print robot attributes. 128 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), 129 | str(self.orientation)) 130 | 131 | ############# ONLY ADD/MODIFY CODE BELOW HERE ################### 132 | 133 | def move(self, motion, tolerance=0.001): # Do not change the name of this function 134 | 135 | steering = motion[0] 136 | distance = motion[1] 137 | 138 | if abs(steering) > max_steering_angle: 139 | raise ValueError, 'Exceeding max steering angle' 140 | 141 | if distance < 0.0: 142 | raise ValueError, 'Moving backwards is not valid' 143 | 144 | #make a new copy 145 | res = robot() 146 | res.length = self.length 147 | res.bearing_noise = self.bearing_noise 148 | res.steering_noise = self.steering_noise 149 | res.distance_noise = self.distance_noise 150 | 151 | #apply noise 152 | steering2 = random.gauss(steering, self.steering_noise) 153 | distance2 = random.gauss(distance,self.distance_noise) 154 | 155 | #Execute motion 156 | turn = distance2/res.length*tan(steering2) 157 | if abs(turn) < tolerance: 158 | #approximate by straight line motion 159 | res.x = self.x + (distance2 * cos(self.orientation)) 160 | res.y = self.y + (distance2 * sin(self.orientation)) 161 | res.orientation = (self.orientation + turn)%(2.0 * pi) 162 | else: 163 | #approximate bicycle model for motion 164 | radius = distance2 / turn 165 | cx = self.x - (sin(self.orientation) * radius) 166 | cy = self.y + (cos(self.orientation) * radius) 167 | res.orientation = (self.orientation + turn)%(2.0 * pi) 168 | res.x = cx + (sin(self.orientation) * radius) 169 | res.y = cy - (cos(self.orientation) * radius) 170 | return res 171 | 172 | # copy your code from the previous exercise 173 | # and modify it so that it simulates motion noise 174 | # according to the noise parameters 175 | # self.steering_noise 176 | # self.distance_noise 177 | 178 | def sense(self, add_noise = 1): #do not change the name of this function 179 | Z = [] 180 | for e in landmarks: 181 | bearing = abs(atan2(abs(e[0]-self.y),e[1]-self.x) - max(0,int(e[0] w[index]: 310 | beta -= w[index] 311 | index = (index + 1) % N 312 | p3.append(p[index]) 313 | p = p3 314 | 315 | return get_position(p) 316 | 317 | ## IMPORTANT: You may uncomment the test cases below to test your code. 318 | ## But when you submit this code, your test cases MUST be commented 319 | ## out. 320 | ## 321 | ## You can test whether your particle filter works using the 322 | ## function check_output (see test case 2). We will be using a similar 323 | ## function. Note: Even for a well-implemented particle filter this 324 | ## function occasionally returns False. This is because a particle 325 | ## filter is a randomized algorithm. We will be testing your code 326 | ## multiple times. Make sure check_output returns True at least 80% 327 | ## of the time. 328 | 329 | 330 | 331 | ## -------- 332 | ## TEST CASES: 333 | ## 334 | ##1) Calling the particle_filter function with the following 335 | ## motions and measurements should return a [x,y,orientation] 336 | ## vector near [x=93.476 y=75.186 orient=5.2664], that is, the 337 | ## robot's true location. 338 | ## 339 | # motions = [[2. * pi / 10, 20.] for row in range(8)] 340 | # measurements = [[4.746936, 3.859782, 3.045217, 2.045506], 341 | # [3.510067, 2.916300, 2.146394, 1.598332], 342 | # [2.972469, 2.407489, 1.588474, 1.611094], 343 | # [1.906178, 1.193329, 0.619356, 0.807930], 344 | # [1.352825, 0.662233, 0.144927, 0.799090], 345 | # [0.856150, 0.214590, 5.651497, 1.062401], 346 | # [0.194460, 5.660382, 4.761072, 2.471682], 347 | # [5.717342, 4.736780, 3.909599, 2.342536]] 348 | 349 | # print particle_filter(motions, measurements) 350 | 351 | ## 2) You can generate your own test cases by generating 352 | ## measurements using the generate_ground_truth function. 353 | ## It will print the robot's last location when calling it. 354 | ## 355 | ## 356 | number_of_iterations = 6 357 | motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)] 358 | 359 | x = generate_ground_truth(motions) 360 | final_robot = x[0] 361 | measurements = x[1] 362 | estimated_position = particle_filter(motions, measurements) 363 | print_measurements(measurements) 364 | print 'Ground truth: ', final_robot 365 | print 'Particle filter: ', estimated_position 366 | print 'Code check: ', check_output(final_robot, estimated_position) 367 | 368 | 369 | 370 | -------------------------------------------------------------------------------- /Lessson3-Particle Filters/Problem Set/Sensing.py: -------------------------------------------------------------------------------- 1 | # -------------- 2 | # USER INSTRUCTIONS 3 | # 4 | # Write a function in the class robot called sense() 5 | # that takes self as input 6 | # and returns a list, Z, of the four bearings* to the 4 7 | # different landmarks. you will have to use the robot's 8 | # x and y position, as well as its orientation, to 9 | # compute this. 10 | # 11 | # *bearing is defined in the video 12 | # which accompanies this problem. 13 | # 14 | # For now, please do NOT add noise to your sense function. 15 | # 16 | # Please do not modify anything except where indicated 17 | # below. 18 | # 19 | # There are test cases provided at the bottom which you are 20 | # free to use. If you uncomment any of these cases for testing 21 | # make sure that you re-comment it before you submit. 22 | 23 | from math import * 24 | import random 25 | 26 | # -------- 27 | # 28 | # the "world" has 4 landmarks. 29 | # the robot's initial coordinates are somewhere in the square 30 | # represented by the landmarks. 31 | # 32 | # NOTE: Landmark coordinates are given in (y, x) form and NOT 33 | # in the traditional (x, y) format! 34 | 35 | landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) form. 36 | world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds" 37 | 38 | # ------------------------------------------------ 39 | # 40 | # this is the robot class 41 | # 42 | 43 | class robot: 44 | 45 | # -------- 46 | # init: 47 | # creates robot and initializes location/orientation 48 | # 49 | 50 | def __init__(self, length = 10.0): 51 | self.x = random.random() * world_size # initial x position 52 | self.y = random.random() * world_size # initial y position 53 | self.orientation = random.random() * 2.0 * pi # initial orientation 54 | self.length = length # length of robot 55 | self.bearing_noise = 0.0 # initialize bearing noise to zero 56 | self.steering_noise = 0.0 # initialize steering noise to zero 57 | self.distance_noise = 0.0 # initialize distance noise to zero 58 | 59 | 60 | 61 | def __repr__(self): 62 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), 63 | str(self.orientation)) 64 | 65 | 66 | # -------- 67 | # set: 68 | # sets a robot coordinate 69 | # 70 | 71 | def set(self, new_x, new_y, new_orientation): 72 | if new_orientation < 0 or new_orientation >= 2 * pi: 73 | raise ValueError, 'Orientation must be in [0..2pi]' 74 | self.x = float(new_x) 75 | self.y = float(new_y) 76 | self.orientation = float(new_orientation) 77 | 78 | # -------- 79 | # set_noise: 80 | # sets the noise parameters 81 | # 82 | 83 | def set_noise(self, new_b_noise, new_s_noise, new_d_noise): 84 | # makes it possible to change the noise parameters 85 | # this is often useful in particle filters 86 | self.bearing_noise = float(new_b_noise) 87 | self.steering_noise = float(new_s_noise) 88 | self.distance_noise = float(new_d_noise) 89 | 90 | ############# ONLY ADD/MODIFY CODE BELOW HERE ################### 91 | 92 | # -------- 93 | # sense: 94 | # obtains bearings from positions 95 | # 96 | 97 | def sense(self): #do not change the name of this function 98 | Z = [] 99 | for e in landmarks: 100 | Z.append(abs(atan2(abs(e[0]-self.y),e[1]-self.x) - max(0,int(e[0]= world_size: 38 | raise ValueError, 'X coordinate out of bound' 39 | if new_y < 0 or new_y >= world_size: 40 | raise ValueError, 'Y coordinate out of bound' 41 | if new_orientation < 0 or new_orientation >= 2 * pi: 42 | raise ValueError, 'Orientation must be in [0..2pi]' 43 | self.x = float(new_x) 44 | self.y = float(new_y) 45 | self.orientation = float(new_orientation) 46 | 47 | 48 | def set_noise(self, new_f_noise, new_t_noise, new_s_noise): 49 | # makes it possible to change the noise parameters 50 | # this is often useful in particle filters 51 | self.forward_noise = float(new_f_noise); 52 | self.turn_noise = float(new_t_noise); 53 | self.sense_noise = float(new_s_noise); 54 | 55 | 56 | def sense(self): 57 | Z = [] 58 | for i in range(len(landmarks)): 59 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 60 | dist += random.gauss(0.0, self.sense_noise) 61 | Z.append(dist) 62 | return Z 63 | 64 | 65 | def move(self, turn, forward): 66 | if forward < 0: 67 | raise ValueError, 'Robot cant move backwards' 68 | 69 | # turn, and add randomness to the turning command 70 | orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise) 71 | orientation %= 2 * pi 72 | 73 | # move, and add randomness to the motion command 74 | dist = float(forward) + random.gauss(0.0, self.forward_noise) 75 | x = self.x + (cos(orientation) * dist) 76 | y = self.y + (sin(orientation) * dist) 77 | x %= world_size # cyclic truncate 78 | y %= world_size 79 | 80 | # set particle 81 | res = robot() 82 | res.set(x, y, orientation) 83 | res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise) 84 | return res 85 | 86 | def Gaussian(self, mu, sigma, x): 87 | 88 | # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma 89 | return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) 90 | 91 | 92 | def measurement_prob(self, measurement): 93 | 94 | # calculates how likely a measurement should be 95 | 96 | prob = 1.0; 97 | for i in range(len(landmarks)): 98 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 99 | prob *= self.Gaussian(dist, self.sense_noise, measurement[i]) 100 | return prob 101 | 102 | def __repr__(self): 103 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation)) 104 | 105 | def eval(r, p): 106 | sum = 0.0; 107 | for i in range(len(p)): # calculate mean error 108 | dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0) 109 | dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0) 110 | err = sqrt(dx * dx + dy * dy) 111 | sum += err 112 | return sum / float(len(p)) 113 | 114 | #myrobot = robot() 115 | #myrobot.set_noise(5.0, 0.1, 5.0) 116 | #myrobot.set(30.0, 50.0, pi/2) 117 | #myrobot = myrobot.move(-pi/2, 15.0) 118 | #print myrobot.sense() 119 | #myrobot = myrobot.move(-pi/2, 10.0) 120 | #print myrobot.sense() 121 | #### DON'T MODIFY ANYTHING ABOVE HERE! ENTER/MODIFY CODE BELOW #### 122 | myrobot = robot() 123 | myrobot = myrobot.move(0.1, 5.0) 124 | Z = myrobot.sense() 125 | N = 1000 126 | T = 10 #Leave this as 10 for grading purposes. 127 | 128 | p = [] 129 | for i in range(N): 130 | r = robot() 131 | r.set_noise(0.05, 0.05, 5.0) 132 | p.append(r) 133 | 134 | for t in range(T): 135 | myrobot = myrobot.move(0.1, 5.0) 136 | Z = myrobot.sense() 137 | 138 | p2 = [] 139 | for i in range(N): 140 | p2.append(p[i].move(0.1, 5.0)) 141 | p = p2 142 | 143 | w = [] 144 | for i in range(N): 145 | w.append(p[i].measurement_prob(Z)) 146 | 147 | p3 = [] 148 | index = int(random.random() * N) 149 | beta = 0.0 150 | mw = max(w) 151 | for i in range(N): 152 | beta += random.random() * 2.0 * mw 153 | while beta > w[index]: 154 | beta -= w[index] 155 | index = (index + 1) % N 156 | p3.append(p[index]) 157 | p = p3 158 | #enter code here, make sure that you output 10 print statements. 159 | print (eval(myrobot, p)) -------------------------------------------------------------------------------- /Lessson4-Search/First Search Program+ExpansionGrid+Print Path.py: -------------------------------------------------------------------------------- 1 | # ---------- 2 | # User Instructions: 3 | # 4 | # Define a function, search() that returns a list 5 | # in the form of [optimal path length, row, col]. For 6 | # the grid shown below, your function should output 7 | # [11, 4, 5]. 8 | # 9 | # If there is no valid path from the start point 10 | # to the goal, your function should return the string 11 | # 'fail' 12 | # ---------- 13 | 14 | # Grid format: 15 | # 0 = Navigable space 16 | # 1 = Occupied space 17 | 18 | grid = [[0, 0, 1, 0, 0, 0], 19 | [0, 0, 1, 0, 0, 0], 20 | [0, 0, 0, 0, 1, 0], 21 | [0, 0, 1, 1, 1, 0], 22 | [0, 0, 0, 0, 1, 0]] 23 | init = [0, 0] 24 | goal = [len(grid)-1, len(grid[0])-1] 25 | cost = 1 26 | 27 | delta = [[-1, 0], # go up 28 | [ 0,-1], # go left 29 | [ 1, 0], # go down 30 | [ 0, 1]] # go right 31 | 32 | delta_name = ['^', '<', 'v', '>'] 33 | 34 | def search(grid,init,goal,cost): 35 | all_list = [] 36 | drop_list = [] 37 | every_list = [init] 38 | n = 0 39 | nn = 1 40 | grid1 = [[-1 if q==1 else q for q in e] for e in grid] 41 | while 1: 42 | every_list1 = [] 43 | for i in range(len(every_list)): 44 | for e in [[every_list[i][0]-1,every_list[i][1]],[every_list[i][0],every_list[i][1]-1],[every_list[i][0]+1,every_list[i][1]],[every_list[i][0],every_list[i][1]+1]]: 45 | if 0<=e[0]<5 and 0<=e[1]<6 and grid[e[0]][e[1]] == 0: 46 | if e not in drop_list and e not in every_list and e not in every_list1: 47 | every_list1.append(e) 48 | grid1[e[0]][e[1]] = nn 49 | nn += 1 50 | drop_list.extend(every_list) 51 | all_list.append(every_list) 52 | every_list = every_list1 53 | n += 1 54 | if goal in every_list: 55 | grid2 = [[-1 if (q==0 or q > grid1[goal[0]][goal[1]]) else q for q in e] for e in grid1] 56 | grid2[init[0]][init[1]] = 0 57 | move = goal 58 | grid[goal[0]][goal[1]] = '*' 59 | for i in range(len(all_list)): 60 | nnn = len(all_list) - i -1 61 | for e in all_list[nnn]: 62 | ee_list = [[e[0]-1,e[1]],[e[0],e[1]-1],[e[0]+1,e[1]],[e[0],e[1]+1]] 63 | for j in range(4): 64 | if ee_list[j] == move: 65 | grid[e[0]][e[1]] = delta_name[j] 66 | move = [e[0],e[1]] 67 | #return ([n, goal[0], goal[1]]) 68 | #return grid2 69 | return grid 70 | if every_list == []: 71 | grid2 = [[-1 if q==0 else q for q in e] for e in grid1] 72 | grid2[init[0]][init[1]] = 0 73 | #return 'fail' 74 | return grid2 75 | print (search(grid,init,goal,cost)) 76 | -------------------------------------------------------------------------------- /Lessson4-Search/Implement Astar.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions: 3 | # 4 | # Modify the the search function so that it becomes 5 | # an A* search algorithm as defined in the previous 6 | # lectures. 7 | # 8 | # Your function should return the expanded grid 9 | # which shows, for each element, the count when 10 | # it was expanded or -1 if the element was never expanded. 11 | # 12 | # If there is no path from init to goal, 13 | # the function should return the string 'fail' 14 | # ---------- 15 | 16 | grid = [[0, 1, 0, 0, 0, 0], 17 | [0, 1, 0, 0, 0, 0], 18 | [0, 1, 0, 0, 0, 0], 19 | [0, 1, 0, 0, 0, 0], 20 | [0, 0, 0, 0, 1, 0]] 21 | heuristic = [[9, 8, 7, 6, 5, 4], 22 | [8, 7, 6, 5, 4, 3], 23 | [7, 6, 5, 4, 3, 2], 24 | [6, 5, 4, 3, 2, 1], 25 | [5, 4, 3, 2, 1, 0]] 26 | 27 | init = [0, 0] 28 | goal = [len(grid)-1, len(grid[0])-1] 29 | cost = 1 30 | 31 | delta = [[-1, 0 ], # go up 32 | [ 0, -1], # go left 33 | [ 1, 0 ], # go down 34 | [ 0, 1 ]] # go right 35 | 36 | delta_name = ['^', '<', 'v', '>'] 37 | 38 | def search(grid,init,goal,cost,heuristic): 39 | # ---------------------------------------- 40 | # modify the code below 41 | # ---------------------------------------- 42 | closed = [[0 for col in range(len(grid[0]))] for row in range(len(grid))] 43 | closed[init[0]][init[1]] = 1 44 | 45 | expand = [[-1 for col in range(len(grid[0]))] for row in range(len(grid))] 46 | action = [[-1 for col in range(len(grid[0]))] for row in range(len(grid))] 47 | 48 | x = init[0] 49 | y = init[1] 50 | g = 0 51 | h = heuristic[x][y] 52 | f = g + h 53 | 54 | open = [[f, g, h, x, y]] 55 | 56 | found = False # flag that is set when search is complete 57 | resign = False # flag set if we can't find expand 58 | count = 0 59 | 60 | while not found and not resign: 61 | if len(open) == 0: 62 | resign = True 63 | return "Fail" 64 | else: 65 | open.sort() 66 | open.reverse() 67 | next = open.pop() 68 | x = next[3] 69 | y = next[4] 70 | g = next[1] 71 | expand[x][y] = count 72 | count += 1 73 | 74 | if x == goal[0] and y == goal[1]: 75 | found = True 76 | else: 77 | for i in range(len(delta)): 78 | x2 = x + delta[i][0] 79 | y2 = y + delta[i][1] 80 | if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]): 81 | if closed[x2][y2] == 0 and grid[x2][y2] == 0: 82 | g2 = g + cost 83 | h2 = heuristic[x2][y2] 84 | f2 = g2 + h2 85 | open.append([f2, g2, h2, x2, y2]) 86 | closed[x2][y2] = 1 87 | 88 | return expand 89 | print (search(grid,init,goal,cost,heuristic)) -------------------------------------------------------------------------------- /Lessson4-Search/Left Turn Policy.py: -------------------------------------------------------------------------------- 1 | # ---------- 2 | # User Instructions: 3 | # 4 | # Implement the function optimum_policy2D below. 5 | # 6 | # You are given a car in grid with initial state 7 | # init. Your task is to compute and return the car's 8 | # optimal path to the position specified in goal; 9 | # the costs for each motion are as defined in cost. 10 | # 11 | # There are four motion directions: up, left, down, and right. 12 | # Increasing the index in this array corresponds to making a 13 | # a left turn, and decreasing the index corresponds to making a 14 | # right turn. 15 | 16 | forward = [[-1, 0], # go up 17 | [ 0, -1], # go left 18 | [ 1, 0], # go down 19 | [ 0, 1]] # go right 20 | forward_name = ['up', 'left', 'down', 'right'] 21 | 22 | # action has 3 values: right turn, no turn, left turn 23 | action = [-1, 0, 1] 24 | action_name = ['R', '#', 'L'] 25 | 26 | # EXAMPLE INPUTS: 27 | # grid format: 28 | # 0 = navigable space 29 | # 1 = unnavigable space 30 | grid = [[1, 1, 1, 0, 0, 0], 31 | [1, 1, 1, 0, 0, 0], 32 | [0, 0, 0, 0, 0, 0], 33 | [1, 1, 1, 0, 1, 1], 34 | [1, 1, 1, 0, 1, 1]] 35 | 36 | init = [4, 3, 0] # given in the form [row,col,direction] 37 | # direction = 0: up 38 | # 1: left 39 | # 2: down 40 | # 3: right 41 | 42 | goal = [2, 0] # given in the form [row,col] 43 | 44 | cost = [2, 1, 20] # cost has 3 values, corresponding to making 45 | # a right turn, no turn, and a left turn 46 | 47 | # EXAMPLE OUTPUT: 48 | # calling optimum_policy2D with the given parameters should return 49 | # [[' ', ' ', ' ', 'R', '#', 'R'], 50 | # [' ', ' ', ' ', '#', ' ', '#'], 51 | # ['*', '#', '#', '#', '#', 'R'], 52 | # [' ', ' ', ' ', '#', ' ', ' '], 53 | # [' ', ' ', ' ', '#', ' ', ' ']] 54 | # ---------- 55 | def value(mean, init, cost): 56 | valus = [init[2]] + [forward.index([mean[i+1][j]-mean[i][j] for j in range(2)]) for i in range(len(mean)-1)] 57 | valus = [(valus[i+1]-valus[i])%4 for i in range(len(valus)-1)] 58 | valus = [cost[2] if e==1 else e for e in valus] 59 | valus = [cost[1] if e==0 else e for e in valus] 60 | valus = [cost[0] if e==3 else e for e in valus] 61 | return sum(valus) 62 | 63 | 64 | # ---------------------------------------- 65 | # modify code below 66 | # ---------------------------------------- 67 | #forward[(init[3]+i)%4] 68 | def optimum_policy2D(grid,init,goal,cost): 69 | means = [] #quan bu lu jing 70 | mean = [] #dan ci lu jing 71 | mean_1 = [] #fen cha ji ben lu jing 72 | step = [[init[0], init[1]]] #xia yi bu ji he 73 | last_step = [] #shang bu 74 | values = [] #ji suan xiao hao zhi 75 | min_value = 10000 76 | q = 0 #mei ci fen cha you ji gu 77 | while step != []: 78 | k = len(step) 79 | if value(mean, init, cost) < min_value: #xiao hao bu chao guo xian you fang an 80 | for i in range(len(forward)): 81 | if 0<=step[-1][0]+forward[i][0] 1: 93 | last_step = step[-1] 94 | step.pop() 95 | mean.append(last_step) 96 | aaa = [[e for e in mean], q] 97 | mean_1.append(aaa) 98 | #cha lu ji suan shang bu 99 | if q == 1: 100 | last_step = step[-1] 101 | step.pop() 102 | mean.append(last_step) 103 | #cha lu bu tong, huan cha lu ji xu 104 | if q == 0: 105 | for i in range(len(mean_1)): 106 | if mean_1[len(mean_1)-1-i][-1] > 1: 107 | mean_1[len(mean_1)-1-i][-1] -= 1 108 | mean_1 = mean_1[:len(mean_1)-i] 109 | aaa = [e for e in mean_1[-1][0]] 110 | mean = aaa 111 | last_step = mean[-1] 112 | step.pop() 113 | break 114 | if len(step) == k+q: 115 | step.pop() 116 | 117 | #shi fou dao da mu di di 118 | if goal in step: 119 | means.append(mean+[goal]) 120 | if q>1: 121 | mean_1.pop() 122 | for i in range(len(mean_1)): 123 | if mean_1[len(mean_1)-1-i][-1] > 1: 124 | mean_1[len(mean_1)-1-i][-1] -= 1 125 | mean_1 = mean_1[:len(mean_1)-i] 126 | aaa = [e for e in mean_1[-1][0]] 127 | mean = aaa 128 | step.remove(goal) 129 | last_step = mean[-1] 130 | break 131 | if len(step) == k+q: 132 | step.pop() 133 | 134 | 135 | #tian jia mei ge fang fa de xiao hao zhi 136 | if len(means) != len(values): 137 | values.append(value(means[-1], init, cost)) 138 | min_value = min(values) 139 | else: 140 | for i in range(len(mean_1)): 141 | if mean_1[len(mean_1)-1-i][-1] > 1: 142 | mean_1[len(mean_1)-1-i][-1] -= 1 143 | mean_1 = mean_1[:len(mean_1)-i] 144 | aaa = [e for e in mean_1[-1][0]] 145 | mean = aaa 146 | last_step = mean[-1] 147 | step.pop() 148 | break 149 | if len(step) == k: 150 | step.pop() 151 | 152 | return means[values.index(min_value)],min_value,len(means) 153 | 154 | print (optimum_policy2D(grid,init,goal,cost)) 155 | -------------------------------------------------------------------------------- /Lessson4-Search/Optimum Policy.py: -------------------------------------------------------------------------------- 1 | # ---------- 2 | # User Instructions: 3 | # 4 | # Write a function optimum_policy that returns 5 | # a grid which shows the optimum policy for robot 6 | # motion. This means there should be an optimum 7 | # direction associated with each navigable cell from 8 | # which the goal can be reached. 9 | # 10 | # Unnavigable cells as well as cells from which 11 | # the goal cannot be reached should have a string 12 | # containing a single space (' '), as shown in the 13 | # previous video. The goal cell should have '*'. 14 | # ---------- 15 | 16 | grid = [[0, 1, 0, 0, 0, 0], 17 | [0, 1, 0, 0, 0, 0], 18 | [0, 1, 0, 0, 0, 0], 19 | [0, 1, 0, 0, 0, 0], 20 | [0, 0, 0, 0, 1, 0]] 21 | init = [0, 0] 22 | goal = [len(grid)-1, len(grid[0])-1] 23 | cost = 1 # the cost associated with moving from a cell to an adjacent one 24 | 25 | delta = [[-1, 0 ], # go up 26 | [ 0, -1], # go left 27 | [ 1, 0 ], # go down 28 | [ 0, 1 ]] # go right 29 | 30 | delta_name = ['^', '<', 'v', '>'] 31 | 32 | def optimum_policy(grid,goal,cost): 33 | # ---------------------------------------- 34 | # modify code below 35 | # ---------------------------------------- 36 | value = [[99 for row in range(len(grid[0]))] for col in range(len(grid))] 37 | policy = [[99 for row in range(len(grid[0]))] for col in range(len(grid))] 38 | change = True 39 | 40 | while change: 41 | change = False 42 | 43 | for x in range(len(grid)): 44 | for y in range(len(grid[0])): 45 | if goal[0] == x and goal[1] == y: 46 | if value[x][y] > 0: 47 | value[x][y] = 0 48 | policy[x][y] = '*' 49 | change = True 50 | 51 | elif grid[x][y] == 0: 52 | for a in range(len(delta)): 53 | x2 = x + delta[a][0] 54 | y2 = y + delta[a][1] 55 | 56 | if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]) and grid[x2][y2] == 0: 57 | v2 = value[x2][y2] + cost 58 | 59 | if v2 < value[x][y]: 60 | change = True 61 | value[x][y] = v2 62 | policy[x][y] = delta_name[a] 63 | return policy 64 | print (optimum_policy(grid,goal,cost)) -------------------------------------------------------------------------------- /Lessson4-Search/Problem Set/Stochastic Motion.py: -------------------------------------------------------------------------------- 1 | # -------------- 2 | # USER INSTRUCTIONS 3 | # 4 | # Write a function called stochastic_value that 5 | # returns two grids. The first grid, value, should 6 | # contain the computed value of each cell as shown 7 | # in the video. The second grid, policy, should 8 | # contain the optimum policy for each cell. 9 | # 10 | # -------------- 11 | # GRADING NOTES 12 | # 13 | # We will be calling your stochastic_value function 14 | # with several different grids and different values 15 | # of success_prob, collision_cost, and cost_step. 16 | # In order to be marked correct, your function must 17 | # RETURN (it does not have to print) two grids, 18 | # value and policy. 19 | # 20 | # When grading your value grid, we will compare the 21 | # value of each cell with the true value according 22 | # to this model. If your answer for each cell 23 | # is sufficiently close to the correct answer 24 | # (within 0.001), you will be marked as correct. 25 | 26 | delta = [[-1, 0 ], # go up 27 | [ 0, -1], # go left 28 | [ 1, 0 ], # go down 29 | [ 0, 1 ]] # go right 30 | 31 | delta_name = ['^', '<', 'v', '>'] # Use these when creating your policy grid. 32 | 33 | # --------------------------------------------- 34 | # Modify the function stochastic_value below 35 | # --------------------------------------------- 36 | 37 | def stochastic_value(grid,goal,cost_step,collision_cost,success_prob): 38 | failure_prob = (1.0 - success_prob)/2.0 # Probability(stepping left) = prob(stepping right) = failure_prob 39 | value = [[collision_cost if grid[row][col]==1 else 0 for col in range(len(grid[0]))] for row in range(len(grid))] 40 | policy = [[' ' for col in range(len(grid[0]))] for row in range(len(grid))] 41 | 42 | ii = 1 43 | value_sum2 = 0 44 | while 1: 45 | value_sum1 = 0 46 | for i in range(len(grid)): 47 | for j in range(len(grid[0])): 48 | aaa = [] 49 | for e in delta: 50 | if 0<=i+e[0]0 else 110 53 | for k in range(len(aaa)): 54 | nnn = delta.index([aaa[k][1][0]-i,aaa[k][1][1]-j]) 55 | bbb = [[i+delta[(nnn-1)%4][0], j+delta[(nnn-1)%4][1]], [i+delta[nnn][0], j+delta[nnn][1]], [i+delta[(nnn+1)%4][0], j+delta[(nnn+1)%4][1]]] 56 | bbb = [value[q[0]][q[1]] if 0<=q[0]', 'v', 'v', '*'] 107 | # ['>', '>', '^', '<'] 108 | # ['>', '^', '^', '<'] 109 | # ['^', ' ', ' ', '^'] 110 | -------------------------------------------------------------------------------- /Lessson4-Search/Value Program.py: -------------------------------------------------------------------------------- 1 | # ---------- 2 | # User Instructions: 3 | # 4 | # Create a function compute_value which returns 5 | # a grid of values. The value of a cell is the minimum 6 | # number of moves required to get from the cell to the goal. 7 | # 8 | # If a cell is a wall or it is impossible to reach the goal from a cell, 9 | # assign that cell a value of 99. 10 | # ---------- 11 | 12 | grid = [[0, 0, 1, 0, 0, 0], 13 | [0, 0, 1, 0, 0, 0], 14 | [0, 0, 0, 0, 1, 0], 15 | [0, 0, 1, 1, 1, 0], 16 | [0, 0, 0, 0, 1, 0]] 17 | goal = [len(grid)-1, len(grid[0])-1] 18 | cost = 1 # the cost associated with moving from a cell to an adjacent one 19 | 20 | delta = [[-1, 0 ], # go up 21 | [ 0, -1], # go left 22 | [ 1, 0 ], # go down 23 | [ 0, 1 ]] # go right 24 | 25 | delta_name = ['^', '<', 'v', '>'] 26 | 27 | def compute_value(grid,goal,cost): 28 | 29 | # make sure your function returns a grid of values as 30 | # demonstrated in the previous video. 31 | value = [[99 if grid[i][j]==1 else 100 for j in range(len(grid[i]))] for i in range(len(grid))] 32 | value[goal[0]][goal[1]] = 0 33 | n = 0 34 | while 1: 35 | for i in range(len(value)): 36 | for j in range(len(value[i])): 37 | if value[i][j] == n: 38 | for k in range(len(delta)): 39 | if 0<=i+delta[k][0] max_steering_angle: 74 | steering = max_steering_angle 75 | if steering < -max_steering_angle: 76 | steering = -max_steering_angle 77 | if distance < 0.0: 78 | distance = 0.0 79 | 80 | # make a new copy 81 | res = robot() 82 | res.length = self.length 83 | res.steering_noise = self.steering_noise 84 | res.distance_noise = self.distance_noise 85 | res.steering_drift = self.steering_drift 86 | 87 | # apply noise 88 | steering2 = random.gauss(steering, self.steering_noise) 89 | distance2 = random.gauss(distance, self.distance_noise) 90 | 91 | # apply steering drift 92 | steering2 += self.steering_drift 93 | 94 | # Execute motion 95 | turn = np.tan(steering2) * distance2 / self.length 96 | 97 | if abs(turn) < tolerance: 98 | # approximate by straight line motion 99 | res.x = self.x + distance2 * np.cos(self.orientation) 100 | res.y = self.y + distance2 * np.sin(self.orientation) 101 | res.orientation = (self.orientation + turn) % (2.0 * np.pi) 102 | else: 103 | # approximate bicycle model for motion 104 | radius = distance2 / turn 105 | cx = self.x - (np.sin(self.orientation) * radius) 106 | cy = self.y + (np.cos(self.orientation) * radius) 107 | res.orientation = (self.orientation + turn) % (2.0 * np.pi) 108 | res.x = cx + (np.sin(res.orientation) * radius) 109 | res.y = cy - (np.cos(res.orientation) * radius) 110 | 111 | return res 112 | 113 | def __repr__(self): 114 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 115 | 116 | ############## ADD / MODIFY CODE BELOW #################### 117 | # ------------------------------------------------------------------------ 118 | # 119 | # run - does a single control run 120 | 121 | def run(param): 122 | myrobot = robot() 123 | myrobot.set(0.0, 1.0, 0.0) 124 | speed = 1.0 # motion distance is equalt to speed (we assume time = 1) 125 | N = 100 126 | 127 | for i in range(N): 128 | crosstrack_error = myrobot.y 129 | steering = -param * crosstrack_error 130 | myrobot = myrobot.move(steering, speed) 131 | 132 | print( myrobot, steering) 133 | 134 | run(0.1) -------------------------------------------------------------------------------- /Lessson5-PID/PD controller.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions 3 | # 4 | # Implement a PD controller by running 100 iterations 5 | # of robot motion. The steering angle should be set 6 | # by the parameter tau so that: 7 | # 8 | # steering = -tau_p * CTE - tau_d * diff_CTE 9 | # where differential crosstrack error (diff_CTE) 10 | # is given by CTE(t) - CTE(t-1) 11 | # 12 | # Your code should print output that looks like 13 | # the output shown in the video. 14 | # 15 | # Only modify code at the bottom! 16 | # ------------ 17 | 18 | import random 19 | import numpy as np 20 | import matplotlib.pyplot as plt 21 | 22 | # ------------------------------------------------ 23 | # 24 | # this is the Robot class 25 | # 26 | 27 | class Robot(object): 28 | def __init__(self, length=20.0): 29 | """ 30 | Creates robot and initializes location/orientation to 0, 0, 0. 31 | """ 32 | self.x = 0.0 33 | self.y = 0.0 34 | self.orientation = 0.0 35 | self.length = length 36 | self.steering_noise = 0.0 37 | self.distance_noise = 0.0 38 | self.steering_drift = 0.0 39 | 40 | def set(self, x, y, orientation): 41 | """ 42 | Sets a robot coordinate. 43 | """ 44 | self.x = x 45 | self.y = y 46 | self.orientation = orientation % (2.0 * np.pi) 47 | 48 | def set_noise(self, steering_noise, distance_noise): 49 | """ 50 | Sets the noise parameters. 51 | """ 52 | # makes it possible to change the noise parameters 53 | # this is often useful in particle filters 54 | self.steering_noise = steering_noise 55 | self.distance_noise = distance_noise 56 | 57 | def set_steering_drift(self, drift): 58 | """ 59 | Sets the systematical steering drift parameter 60 | """ 61 | self.steering_drift = drift 62 | 63 | def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0): 64 | """ 65 | steering = front wheel steering angle, limited by max_steering_angle 66 | distance = total distance driven, most be non-negative 67 | """ 68 | if steering > max_steering_angle: 69 | steering = max_steering_angle 70 | if steering < -max_steering_angle: 71 | steering = -max_steering_angle 72 | if distance < 0.0: 73 | distance = 0.0 74 | 75 | # make a new copy 76 | # res = Robot() 77 | # res.length = self.length 78 | # res.steering_noise = self.steering_noise 79 | # res.distance_noise = self.distance_noise 80 | # res.steering_drift = self.steering_drift 81 | 82 | # apply noise 83 | steering2 = random.gauss(steering, self.steering_noise) 84 | distance2 = random.gauss(distance, self.distance_noise) 85 | 86 | # apply steering drift 87 | steering2 += self.steering_drift 88 | 89 | # Execute motion 90 | turn = np.tan(steering2) * distance2 / self.length 91 | 92 | if abs(turn) < tolerance: 93 | # approximate by straight line motion 94 | self.x += distance2 * np.cos(self.orientation) 95 | self.y += distance2 * np.sin(self.orientation) 96 | self.orientation = (self.orientation + turn) % (2.0 * np.pi) 97 | else: 98 | # approximate bicycle model for motion 99 | radius = distance2 / turn 100 | cx = self.x - (np.sin(self.orientation) * radius) 101 | cy = self.y + (np.cos(self.orientation) * radius) 102 | self.orientation = (self.orientation + turn) % (2.0 * np.pi) 103 | self.x = cx + (np.sin(self.orientation) * radius) 104 | self.y = cy - (np.cos(self.orientation) * radius) 105 | 106 | def __repr__(self): 107 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 108 | 109 | ############## ADD / MODIFY CODE BELOW #################### 110 | # ------------------------------------------------------------------------ 111 | # 112 | # run - does a single control run 113 | 114 | # previous P controller 115 | def run_p(robot, tau, n=100, speed=1.0): 116 | x_trajectory = [] 117 | y_trajectory = [] 118 | for i in range(n): 119 | cte = robot.y 120 | steer = -tau * cte 121 | robot.move(steer, speed) 122 | x_trajectory.append(robot.x) 123 | y_trajectory.append(robot.y) 124 | return x_trajectory, y_trajectory 125 | 126 | robot = Robot() 127 | robot.set(0, 1, 0) 128 | 129 | def run(robot, tau_p, tau_d, n=100, speed=1.0): 130 | x_trajectory = [] 131 | y_trajectory = [] 132 | cte_pre = 0 133 | for i in range(n): 134 | cte = robot.y 135 | diff_cte = cte - cte_pre 136 | steer = -tau_p * cte - tau_d * diff_cte 137 | robot.move(steer, speed) 138 | x_trajectory.append(robot.x) 139 | y_trajectory.append(robot.y) 140 | cte_pre = cte 141 | return x_trajectory, y_trajectory 142 | 143 | x_trajectory, y_trajectory = run(robot, 0.2, 3.0) 144 | n = len(x_trajectory) 145 | 146 | plt.plot(x_trajectory, y_trajectory, 'g', label='PD controller') 147 | plt.plot(x_trajectory, np.zeros(n), 'r', label='reference') 148 | plt.legend() 149 | plt.show() 150 | -------------------------------------------------------------------------------- /Lessson5-PID/PID controller.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions 3 | # 4 | # Implement a P controller by running 100 iterations 5 | # of robot motion. The steering angle should be set 6 | # by the parameter tau so that: 7 | # 8 | # steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE 9 | # 10 | # where the integrated crosstrack error (int_CTE) is 11 | # the sum of all the previous crosstrack errors. 12 | # This term works to cancel out steering drift. 13 | # 14 | # Your code should print a list that looks just like 15 | # the list shown in the video. 16 | # 17 | # Only modify code at the bottom! 18 | # ------------ 19 | 20 | import random 21 | import numpy as np 22 | import matplotlib.pyplot as plt 23 | 24 | # ------------------------------------------------ 25 | # 26 | # this is the Robot class 27 | # 28 | 29 | class Robot(object): 30 | def __init__(self, length=20.0): 31 | """ 32 | Creates robot and initializes location/orientation to 0, 0, 0. 33 | """ 34 | self.x = 0.0 35 | self.y = 0.0 36 | self.orientation = 0.0 37 | self.length = length 38 | self.steering_noise = 0.0 39 | self.distance_noise = 0.0 40 | self.steering_drift = 0.0 41 | 42 | def set(self, x, y, orientation): 43 | """ 44 | Sets a robot coordinate. 45 | """ 46 | self.x = x 47 | self.y = y 48 | self.orientation = orientation % (2.0 * np.pi) 49 | 50 | def set_noise(self, steering_noise, distance_noise): 51 | """ 52 | Sets the noise parameters. 53 | """ 54 | # makes it possible to change the noise parameters 55 | # this is often useful in particle filters 56 | self.steering_noise = steering_noise 57 | self.distance_noise = distance_noise 58 | 59 | def set_steering_drift(self, drift): 60 | """ 61 | Sets the systematical steering drift parameter 62 | """ 63 | self.steering_drift = drift 64 | 65 | def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0): 66 | """ 67 | steering = front wheel steering angle, limited by max_steering_angle 68 | distance = total distance driven, most be non-negative 69 | """ 70 | if steering > max_steering_angle: 71 | steering = max_steering_angle 72 | if steering < -max_steering_angle: 73 | steering = -max_steering_angle 74 | if distance < 0.0: 75 | distance = 0.0 76 | 77 | # make a new copy 78 | # res = Robot() 79 | # res.length = self.length 80 | # res.steering_noise = self.steering_noise 81 | # res.distance_noise = self.distance_noise 82 | # res.steering_drift = self.steering_drift 83 | 84 | # apply noise 85 | steering2 = random.gauss(steering, self.steering_noise) 86 | distance2 = random.gauss(distance, self.distance_noise) 87 | 88 | # apply steering drift 89 | steering2 += self.steering_drift 90 | 91 | # Execute motion 92 | turn = np.tan(steering2) * distance2 / self.length 93 | 94 | if abs(turn) < tolerance: 95 | # approximate by straight line motion 96 | self.x += distance2 * np.cos(self.orientation) 97 | self.y += distance2 * np.sin(self.orientation) 98 | self.orientation = (self.orientation + turn) % (2.0 * np.pi) 99 | else: 100 | # approximate bicycle model for motion 101 | radius = distance2 / turn 102 | cx = self.x - (np.sin(self.orientation) * radius) 103 | cy = self.y + (np.cos(self.orientation) * radius) 104 | self.orientation = (self.orientation + turn) % (2.0 * np.pi) 105 | self.x = cx + (np.sin(self.orientation) * radius) 106 | self.y = cy - (np.cos(self.orientation) * radius) 107 | 108 | def __repr__(self): 109 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 110 | 111 | ############## ADD / MODIFY CODE BELOW #################### 112 | # ------------------------------------------------------------------------ 113 | # 114 | # run - does a single control run 115 | 116 | robot = Robot() 117 | robot.set(0, 1, 0) 118 | 119 | 120 | def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0): 121 | x_trajectory = [] 122 | y_trajectory = [] 123 | cte_pre = 0 124 | cte_sum = 0 125 | for i in range(n): 126 | cte = robot.y 127 | diff_cte = cte - cte_pre 128 | int_cte = cte_sum + cte 129 | steering = -tau_p * cte - tau_d * diff_cte - tau_i * int_cte 130 | robot.move(steering, speed) 131 | x_trajectory.append(robot.x) 132 | y_trajectory.append(robot.y) 133 | cte_pre = cte 134 | cte_sum = int_cte 135 | return x_trajectory, y_trajectory 136 | 137 | 138 | x_trajectory, y_trajectory = run(robot, 0.2, 3.0, 0.004) 139 | n = len(x_trajectory) 140 | 141 | plt.plot(x_trajectory, y_trajectory, 'g', label='PID controller') 142 | plt.plot(x_trajectory, np.zeros(n), 'r', label='reference') 143 | plt.legend() 144 | plt.show() 145 | -------------------------------------------------------------------------------- /Lessson5-PID/Parameter Optimization.py: -------------------------------------------------------------------------------- 1 | # ---------------- 2 | # User Instructions 3 | # 4 | # Implement twiddle as shown in the previous two videos. 5 | # Your accumulated error should be very small! 6 | # 7 | # Your twiddle function should RETURN the accumulated 8 | # error. Try adjusting the parameters p and dp to make 9 | # this error as small as possible. 10 | # 11 | # Try to get your error below 1.0e-10 with as few iterations 12 | # as possible (too many iterations will cause a timeout). 13 | # No cheating! 14 | # ------------ 15 | 16 | import random 17 | import numpy as np 18 | import matplotlib.pyplot as plt 19 | 20 | # ------------------------------------------------ 21 | # 22 | # this is the Robot class 23 | # 24 | 25 | class Robot(object): 26 | def __init__(self, length=20.0): 27 | """ 28 | Creates robot and initializes location/orientation to 0, 0, 0. 29 | """ 30 | self.x = 0.0 31 | self.y = 0.0 32 | self.orientation = 0.0 33 | self.length = length 34 | self.steering_noise = 0.0 35 | self.distance_noise = 0.0 36 | self.steering_drift = 0.0 37 | 38 | def set(self, x, y, orientation): 39 | """ 40 | Sets a robot coordinate. 41 | """ 42 | self.x = x 43 | self.y = y 44 | self.orientation = orientation % (2.0 * np.pi) 45 | 46 | def set_noise(self, steering_noise, distance_noise): 47 | """ 48 | Sets the noise parameters. 49 | """ 50 | # makes it possible to change the noise parameters 51 | # this is often useful in particle filters 52 | self.steering_noise = steering_noise 53 | self.distance_noise = distance_noise 54 | 55 | def set_steering_drift(self, drift): 56 | """ 57 | Sets the systematical steering drift parameter 58 | """ 59 | self.steering_drift = drift 60 | 61 | def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0): 62 | """ 63 | steering = front wheel steering angle, limited by max_steering_angle 64 | distance = total distance driven, most be non-negative 65 | """ 66 | if steering > max_steering_angle: 67 | steering = max_steering_angle 68 | if steering < -max_steering_angle: 69 | steering = -max_steering_angle 70 | if distance < 0.0: 71 | distance = 0.0 72 | 73 | # make a new copy 74 | # res = Robot() 75 | # res.length = self.length 76 | # res.steering_noise = self.steering_noise 77 | # res.distance_noise = self.distance_noise 78 | # res.steering_drift = self.steering_drift 79 | 80 | # apply noise 81 | steering2 = random.gauss(steering, self.steering_noise) 82 | distance2 = random.gauss(distance, self.distance_noise) 83 | 84 | # apply steering drift 85 | steering2 += self.steering_drift 86 | 87 | # Execute motion 88 | turn = np.tan(steering2) * distance2 / self.length 89 | 90 | if abs(turn) < tolerance: 91 | # approximate by straight line motion 92 | self.x += distance2 * np.cos(self.orientation) 93 | self.y += distance2 * np.sin(self.orientation) 94 | self.orientation = (self.orientation + turn) % (2.0 * np.pi) 95 | else: 96 | # approximate bicycle model for motion 97 | radius = distance2 / turn 98 | cx = self.x - (np.sin(self.orientation) * radius) 99 | cy = self.y + (np.cos(self.orientation) * radius) 100 | self.orientation = (self.orientation + turn) % (2.0 * np.pi) 101 | self.x = cx + (np.sin(self.orientation) * radius) 102 | self.y = cy - (np.cos(self.orientation) * radius) 103 | 104 | def __repr__(self): 105 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 106 | 107 | ############## ADD / MODIFY CODE BELOW #################### 108 | # ------------------------------------------------------------------------ 109 | # 110 | # run - does a single control run 111 | 112 | 113 | def make_robot(): 114 | """ 115 | Resets the robot back to the initial position and drift. 116 | You'll want to call this after you call `run`. 117 | """ 118 | robot = Robot() 119 | robot.set(0, 1, 0) 120 | robot.set_steering_drift(10 / 180 * np.pi) 121 | return robot 122 | 123 | 124 | # NOTE: We use params instead of tau_p, tau_d, tau_i 125 | def run(robot, params, n=100, speed=1.0): 126 | x_trajectory = [] 127 | y_trajectory = [] 128 | err = 0 129 | # TODO: your code here 130 | prev_cte = robot.y 131 | int_cte = 0 132 | for i in range(2 * n): 133 | cte = robot.y 134 | diff_cte = (cte - prev_cte) / speed 135 | int_cte += cte 136 | prev_cte = cte 137 | steer = -params[0] * cte - params[1] * diff_cte - params[2] * int_cte 138 | robot.move(steer, speed) 139 | x_trajectory.append(robot.x) 140 | y_trajectory.append(robot.y) 141 | if i >= n: 142 | err += cte ** 2 143 | return x_trajectory, y_trajectory, err / n 144 | 145 | 146 | # Make this tolerance bigger if you are timing out! 147 | def twiddle(tol=0.0001): 148 | # TODO: Add code here 149 | # Don't forget to call `make_robot` before you call `run`! 150 | p = [0, 0, 0] 151 | dp = [1, 1, 1] 152 | robot = make_robot() 153 | x_trajectory, y_trajectory, best_err = run(robot, p) 154 | 155 | it = 0 156 | while sum(dp) > tol: 157 | print("Iteration {}, best error = {}".format(it, best_err)) 158 | for i in range(len(p)): 159 | p[i] += dp[i] 160 | robot = make_robot() 161 | x_trajectory, y_trajectory, err = run(robot, p) 162 | 163 | if err < best_err: 164 | best_err = err 165 | dp[i] *= 1.1 166 | else: 167 | p[i] -= 2 * dp[i] 168 | robot = make_robot() 169 | x_trajectory, y_trajectory, err = run(robot, p) 170 | 171 | if err < best_err: 172 | best_err = err 173 | dp[i] *= 1.1 174 | else: 175 | p[i] += dp[i] 176 | dp[i] *= 0.9 177 | it += 1 178 | print (p) 179 | return p 180 | 181 | 182 | params = twiddle() 183 | robot = make_robot() 184 | x_trajectory, y_trajectory, err = run(robot, params) 185 | n = len(x_trajectory) 186 | 187 | plt.plot(x_trajectory, y_trajectory, 'g', label='twiddle PID controller') 188 | plt.plot(x_trajectory, np.zeros(n), 'r', label='reference') 189 | plt.legend() 190 | plt.show() 191 | 192 | ## [0.49021069899169206, 5.104201288723152, 1.215780871989683e-12] -------------------------------------------------------------------------------- /Lessson5-PID/Path Smoothing.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions 3 | # 4 | # Define a function smooth that takes a path as its input 5 | # (with optional parameters for weight_data, weight_smooth, 6 | # and tolerance) and returns a smooth path. The first and 7 | # last points should remain unchanged. 8 | # 9 | # Smoothing should be implemented by iteratively updating 10 | # each entry in newpath until some desired level of accuracy 11 | # is reached. The update should be done according to the 12 | # gradient descent equations given in the instructor's note 13 | # below (the equations given in the video are not quite 14 | # correct). 15 | # ----------- 16 | 17 | from copy import deepcopy 18 | 19 | # thank you to EnTerr for posting this on our discussion forum 20 | def printpaths(path,newpath): 21 | for old,new in zip(path,newpath): 22 | print '['+ ', '.join('%.3f'%x for x in old) + \ 23 | '] -> ['+ ', '.join('%.3f'%x for x in new) +']' 24 | 25 | # Don't modify path inside your function. 26 | path = [[0, 0], 27 | [0, 1], 28 | [0, 2], 29 | [1, 2], 30 | [2, 2], 31 | [3, 2], 32 | [4, 2], 33 | [4, 3], 34 | [4, 4]] 35 | 36 | def smooth(path, weight_data = 0.5, weight_smooth = 0.1, tolerance = 0.000001): 37 | 38 | # Make a deep copy of path into newpath 39 | newpath = deepcopy(path) 40 | 41 | ii = 1 42 | newpath_sum = 10000 43 | while ii: 44 | newpath1 = deepcopy(newpath) 45 | for i in range(1,len(newpath)-1): 46 | for j in range(len(newpath[i])): 47 | newpath[i][j] += weight_data * (path[i][j] - newpath[i][j]) + weight_smooth * (newpath[i-1][j] + newpath[i+1][j] - 2.0 * newpath[i][j]) 48 | 49 | newpath_sum = sum([sum([abs(newpath[i][j]-newpath1[i][j]) for j in range(len(newpath[i]))]) for i in range(len(newpath))]) 50 | ii += 1 51 | if newpath_sum < tolerance: 52 | print (ii) 53 | break 54 | 55 | return newpath # Leave this line for the grader! 56 | 57 | printpaths(path,smooth(path)) 58 | -------------------------------------------------------------------------------- /Lessson5-PID/Problem Set/Constrained Smoothing.py: -------------------------------------------------------------------------------- 1 | # ------------- 2 | # User Instructions 3 | # 4 | # Now you will be incorporating fixed points into 5 | # your smoother. 6 | # 7 | # You will need to use the equations from gradient 8 | # descent AND the new equations presented in the 9 | # previous lecture to implement smoothing with 10 | # fixed points. 11 | # 12 | # Your function should return the newpath that it 13 | # calculates. 14 | # 15 | # Feel free to use the provided solution_check function 16 | # to test your code. You can find it at the bottom. 17 | # 18 | 19 | ######################## ENTER CODE BELOW HERE ######################### 20 | 21 | def smooth(path, fix, weight_data = 0.0, weight_smooth = 0.1, tolerance = 0.00001): 22 | 23 | # Make a deep copy of path into newpath 24 | newpath = [[path[i][j] for j in range(len(path[i]))] for i in range(len(path))] 25 | 26 | ii = 1 27 | newpath_sum = 10000 28 | while ii: 29 | newpath1 = [[newpath[i][j] for j in range(len(path[i]))] for i in range(len(path))] 30 | for i in range(len(newpath)): 31 | if fix[i]==0: 32 | for j in range(len(newpath[i])): 33 | newpath[i][j] += weight_smooth * (newpath[(i-1)%len(path)][j] + newpath[(i+1)%len(path)][j] - \ 34 | 2.0 * newpath[i][j]) + \ 35 | (weight_smooth / 2.0) * (2.0 * newpath[(i-1)%len(path)][j] - \ 36 | newpath[(i-2)%len(path)][j] - newpath[i][j]) + \ 37 | (weight_smooth / 2.0) * (2.0 * newpath[(i+1)%len(path)][j] - \ 38 | newpath[(i+2)%len(path)][j] - newpath[i][j]) 39 | 40 | newpath_sum = sum([sum([abs(newpath[i][j]-newpath1[i][j]) for j in range(len(newpath[i]))]) for i in range(len(newpath))]) 41 | ii += 1 42 | if newpath_sum < tolerance: 43 | print (ii) 44 | break 45 | 46 | return newpath 47 | 48 | # -------------- 49 | # Testing Instructions 50 | # 51 | # To test your code, call the solution_check function with the argument smooth: 52 | # solution_check(smooth) 53 | # 54 | 55 | def solution_check(smooth, eps = 0.0001): 56 | 57 | def test_case_str(path, fix): 58 | assert( len(path) == len(fix) ) 59 | 60 | if len(path) == 0: 61 | return '[]' 62 | if len(path) == 1: 63 | s = '[' + str(path[0]) + ']' 64 | if fix[0]: s += ' #fix' 65 | return s 66 | 67 | s = '[' + str(path[0]) + ',' 68 | if fix[0]: s += ' #fix' 69 | for pt,f in zip(path[1:-1],fix[1:-1]): 70 | s += '\n ' + str(pt) + ',' 71 | if f: s += ' #fix' 72 | s += '\n ' + str(path[-1]) + ']' 73 | if fix[-1]: s += ' #fix' 74 | return s 75 | 76 | testpaths = [[[0, 0],[1, 0],[2, 0],[3, 0],[4, 0],[5, 0],[6, 0],[6, 1],[6, 2],[6, 3],[5, 3],[4, 3],[3, 3],[2, 3],[1, 3],[0, 3],[0, 2],[0, 1]], 77 | [[0, 0],[2, 0],[4, 0],[4, 2],[4, 4],[2, 4],[0, 4],[0, 2]]] 78 | testfixpts = [[1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0], 79 | [1, 0, 1, 0, 1, 0, 1, 0]] 80 | pseudo_answers = [[[0, 0],[0.7938620981547201, -0.8311168821106101],[1.8579052986461084, -1.3834788165869276],[3.053905318597796, -1.5745863173084],[4.23141390533387, -1.3784271816058231],[5.250184859723701, -0.8264215958231558],[6, 0],[6.415150091996651, 0.9836951698796843],[6.41942442687092, 2.019512290770163],[6, 3],[5.206131365604606, 3.831104483245191],[4.142082497497067, 4.383455704596517],[2.9460804122779813, 4.5745592975708105],[1.768574219397359, 4.378404668718541],[0.7498089205417316, 3.826409771585794],[0, 3],[-0.4151464728194156, 2.016311854977891],[-0.4194207879552198, 0.9804948340550833]], 81 | [[0, 0],[2.0116767115496095, -0.7015439080661671],[4, 0],[4.701543905420104, 2.0116768147460418],[4, 4],[1.9883231877640861, 4.701543807525115],[0, 4],[-0.7015438099112995, 1.9883232808252207]]] 82 | true_answers = [[[0, 0],[0.7826068175979299, -0.6922616156406778],[1.826083356960912, -1.107599209206985],[2.999995745732953, -1.2460426422963626],[4.173909508264126, -1.1076018591282746],[5.217389489606966, -0.6922642758483151],[6, 0],[6.391305105067843, 0.969228211275216],[6.391305001845138, 2.0307762911524616],[6, 3],[5.217390488523538, 3.6922567975830876],[4.17391158149052, 4.107590195596796],[2.9999982969959467, 4.246032043344827],[1.8260854997325473, 4.107592961155283],[0.7826078838205919, 3.692259569132191],[0, 3],[-0.3913036785959153, 2.030774470796648], [-0.3913035729270973, 0.9692264531461132]], 83 | [[0, 0],[1.9999953708444873, -0.6666702980585777],[4, 0],[4.666670298058577, 2.000005101453379],[4, 4],[1.9999948985466212, 4.6666612524128],[0, 4],[-0.6666612524127998, 2.000003692691148]]] 84 | newpaths = map(lambda p: smooth(*p), zip(testpaths,testfixpts)) 85 | 86 | correct = True 87 | 88 | for path,fix,p_answer,t_answer,newpath in zip(testpaths,testfixpts, 89 | pseudo_answers,true_answers, 90 | newpaths): 91 | if type(newpath) != list: 92 | print "Error: smooth did not return a list for the path:" 93 | print test_case_str(path,fix) + '\n' 94 | correct = False 95 | continue 96 | if len(newpath) != len(path): 97 | print "Error: smooth did not return a list of the correct length for the path:" 98 | print test_case_str(path,fix) + '\n' 99 | correct = False 100 | continue 101 | 102 | good_pairs = True 103 | for newpt,pt in zip(newpath,path): 104 | if len(newpt) != len(pt): 105 | good_pairs = False 106 | break 107 | if not good_pairs: 108 | print "Error: smooth did not return a list of coordinate pairs for the path:" 109 | print test_case_str(path,fix) + '\n' 110 | correct = False 111 | continue 112 | assert( good_pairs ) 113 | 114 | # check whether to check against true or pseudo answers 115 | answer = None 116 | if abs(newpath[1][0] - t_answer[1][0]) <= eps: 117 | answer = t_answer 118 | elif abs(newpath[1][0] - p_answer[1][0]) <= eps: 119 | answer = p_answer 120 | else: 121 | print 'smooth returned an incorrect answer for the path:' 122 | print test_case_str(path,fix) + '\n' 123 | continue 124 | assert( answer is not None ) 125 | 126 | entries_match = True 127 | for p,q in zip(newpath,answer): 128 | for pi,qi in zip(p,q): 129 | if abs(pi - qi) > eps: 130 | entries_match = False 131 | break 132 | if not entries_match: break 133 | if not entries_match: 134 | print 'smooth returned an incorrect answer for the path:' 135 | print test_case_str(path,fix) + '\n' 136 | continue 137 | 138 | assert( entries_match ) 139 | if answer == t_answer: 140 | print 'smooth returned the correct answer for the path:' 141 | print test_case_str(path,fix) + '\n' 142 | elif answer == p_answer: 143 | print 'smooth returned a correct* answer for the path:' 144 | print test_case_str(path,fix) 145 | print '''*However, your answer uses the "nonsimultaneous" update method, which 146 | is not technically correct. You should modify your code so that newpath[i][j] is only 147 | updated once per iteration, or else the intermediate updates made to newpath[i][j] 148 | will affect the final answer.\n''' 149 | 150 | solution_check(smooth) 151 | -------------------------------------------------------------------------------- /Lessson5-PID/Problem Set/Cyclic Smoothing.py: -------------------------------------------------------------------------------- 1 | # ------------- 2 | # User Instructions 3 | # 4 | # Here you will be implementing a cyclic smoothing 5 | # algorithm. This algorithm should not fix the end 6 | # points (as you did in the unit quizzes). You 7 | # should use the gradient descent equations that 8 | # you used previously. 9 | # 10 | # Your function should return the newpath that it 11 | # calculates. 12 | # 13 | # Feel free to use the provided solution_check function 14 | # to test your code. You can find it at the bottom. 15 | # 16 | # -------------- 17 | # Testing Instructions 18 | # 19 | # To test your code, call the solution_check function with 20 | # two arguments. The first argument should be the result of your 21 | # smooth function. The second should be the corresponding answer. 22 | # For example, calling 23 | # 24 | # solution_check(smooth(testpath1), answer1) 25 | # 26 | # should return True if your answer is correct and False if 27 | # it is not. 28 | 29 | from math import * 30 | 31 | # Do not modify path inside your function. 32 | path=[[0, 0], 33 | [1, 0], 34 | [2, 0], 35 | [3, 0], 36 | [4, 0], 37 | [5, 0], 38 | [6, 0], 39 | [6, 1], 40 | [6, 2], 41 | [6, 3], 42 | [5, 3], 43 | [4, 3], 44 | [3, 3], 45 | [2, 3], 46 | [1, 3], 47 | [0, 3], 48 | [0, 2], 49 | [0, 1]] 50 | 51 | ############# ONLY ENTER CODE BELOW THIS LINE ########## 52 | 53 | # ------------------------------------------------ 54 | # smooth coordinates 55 | # If your code is timing out, make the tolerance parameter 56 | # larger to decrease run time. 57 | # 58 | 59 | def smooth(path, weight_data = 0.1, weight_smooth = 0.1, tolerance = 0.00001): 60 | 61 | # Make a deep copy of path into newpath 62 | newpath = [[path[i][j] for j in range(len(path[i]))] for i in range(len(path))] 63 | 64 | ii = 1 65 | newpath_sum = 10000 66 | while ii: 67 | newpath1 = [[newpath[i][j] for j in range(len(path[i]))] for i in range(len(path))] 68 | for i in range(len(newpath)): 69 | for j in range(len(newpath[i])): 70 | newpath[i][j] += weight_data * (path[i][j] - newpath[i][j]) + weight_smooth * (newpath[(i-1)%len(path)][j] + newpath[(i+1)%len(path)][j] - 2.0 * newpath[i][j]) 71 | 72 | newpath_sum = sum([sum([abs(newpath[i][j]-newpath1[i][j]) for j in range(len(newpath[i]))]) for i in range(len(newpath))]) 73 | ii += 1 74 | if newpath_sum < tolerance: 75 | print (ii) 76 | break 77 | 78 | return newpath 79 | 80 | # thank you - EnTerr - for posting this on our discussion forum 81 | 82 | #newpath = smooth(path) 83 | #for i in range(len(path)): 84 | # print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']' 85 | 86 | 87 | ##### TESTING ###### 88 | 89 | # -------------------------------------------------- 90 | # check if two numbers are 'close enough,'used in 91 | # solution_check function. 92 | # 93 | def close_enough(user_answer, true_answer, epsilon = 0.001): 94 | if abs(user_answer - true_answer) > epsilon: 95 | return False 96 | return True 97 | 98 | # -------------------------------------------------- 99 | # check your solution against our reference solution for 100 | # a variety of test cases (given below) 101 | # 102 | def solution_check(newpath, answer): 103 | if type(newpath) != type(answer): 104 | print "Error. You do not return a list." 105 | return False 106 | if len(newpath) != len(answer): 107 | print 'Error. Your newpath is not the correct length.' 108 | return False 109 | if len(newpath[0]) != len(answer[0]): 110 | print 'Error. Your entries do not contain an (x, y) coordinate pair.' 111 | return False 112 | for i in range(len(newpath)): 113 | for j in range(len(newpath[0])): 114 | if not close_enough(newpath[i][j], answer[i][j]): 115 | print 'Error, at least one of your entries is not correct.' 116 | return False 117 | print "Test case correct!" 118 | return True 119 | 120 | # -------------- 121 | # Testing Instructions 122 | # 123 | # To test your code, call the solution_check function with 124 | # two arguments. The first argument should be the result of your 125 | # smooth function. The second should be the corresponding answer. 126 | # For example, calling 127 | # 128 | # solution_check(smooth(testpath1), answer1) 129 | # 130 | # should return True if your answer is correct and False if 131 | # it is not. 132 | 133 | testpath1 = [[0, 0], 134 | [1, 0], 135 | [2, 0], 136 | [3, 0], 137 | [4, 0], 138 | [5, 0], 139 | [6, 0], 140 | [6, 1], 141 | [6, 2], 142 | [6, 3], 143 | [5, 3], 144 | [4, 3], 145 | [3, 3], 146 | [2, 3], 147 | [1, 3], 148 | [0, 3], 149 | [0, 2], 150 | [0, 1]] 151 | 152 | answer1 = [[0.4705860385182691, 0.4235279620576893], 153 | [1.1764695730296597, 0.16470408411716733], 154 | [2.058823799247812, 0.07058633859438503], 155 | [3.000001503542886, 0.04705708651959327], 156 | [3.9411790099468273, 0.07058689299792453], 157 | [4.8235326678889345, 0.16470511854183797], 158 | [5.529415336860586, 0.4235293374365447], 159 | [5.76470933698621, 1.1058829941330384], 160 | [5.764708805535902, 1.8941189433780983], 161 | [5.5294138118186265, 2.5764724018811056], 162 | [4.823530348360371, 2.835296251305122], 163 | [3.941176199414957, 2.929413985845729], 164 | [2.9999985709076413, 2.952943245204772], 165 | [2.0588211310939526, 2.9294134622132018], 166 | [1.1764675231284938, 2.8352952720424938], 167 | [0.4705848811030855, 2.5764710948028178], 168 | [0.23529088056307781, 1.8941174802285707], 169 | [0.23529138316655338, 1.1058815684272394]] 170 | 171 | testpath2 = [[1, 0], # Move in the shape of a plus sign 172 | [2, 0], 173 | [2, 1], 174 | [3, 1], 175 | [3, 2], 176 | [2, 2], 177 | [2, 3], 178 | [1, 3], 179 | [1, 2], 180 | [0, 2], 181 | [0, 1], 182 | [1, 1]] 183 | 184 | answer2 = [[1.2222234770374059, 0.4444422843711052], 185 | [1.7777807251383388, 0.4444432993123497], 186 | [2.111114925633848, 0.8888894279539462], 187 | [2.5555592020540376, 1.2222246475393077], 188 | [2.5555580686154244, 1.7777817817879298], 189 | [2.111111849558437, 2.1111159707965514], 190 | [1.7777765871460525, 2.55556033483712], 191 | [1.2222194640861452, 2.5555593592828543], 192 | [0.8888853322565222, 2.111113321684573], 193 | [0.44444105139827167, 1.777778212019149], 194 | [0.44444210978390364, 1.2222211690821811], 195 | [0.8888882042812255, 0.8888870211766268]] 196 | 197 | print (solution_check(smooth(testpath1), answer1)) 198 | print (solution_check(smooth(testpath2), answer2)) -------------------------------------------------------------------------------- /Lessson5-PID/Problem Set/Racetrack Control.py: -------------------------------------------------------------------------------- 1 | # -------------- 2 | # User Instructions 3 | # 4 | # Define a function cte in the robot class that will 5 | # compute the crosstrack error for a robot on a 6 | # racetrack with a shape as described in the video. 7 | # 8 | # You will need to base your error calculation on 9 | # the robot's location on the track. Remember that 10 | # the robot will be traveling to the right on the 11 | # upper straight segment and to the left on the lower 12 | # straight segment. 13 | # 14 | # -------------- 15 | # Grading Notes 16 | # 17 | # We will be testing your cte function directly by 18 | # calling it with different robot locations and making 19 | # sure that it returns the correct crosstrack error. 20 | 21 | from math import * 22 | import random 23 | 24 | 25 | # ------------------------------------------------ 26 | # 27 | # this is the robot class 28 | # 29 | 30 | class robot: 31 | 32 | # -------- 33 | # init: 34 | # creates robot and initializes location/orientation to 0, 0, 0 35 | # 36 | 37 | def __init__(self, length = 20.0): 38 | self.x = 0.0 39 | self.y = 0.0 40 | self.orientation = 0.0 41 | self.length = length 42 | self.steering_noise = 0.0 43 | self.distance_noise = 0.0 44 | self.steering_drift = 0.0 45 | 46 | # -------- 47 | # set: 48 | # sets a robot coordinate 49 | # 50 | 51 | def set(self, new_x, new_y, new_orientation): 52 | 53 | self.x = float(new_x) 54 | self.y = float(new_y) 55 | self.orientation = float(new_orientation) % (2.0 * pi) 56 | 57 | 58 | # -------- 59 | # set_noise: 60 | # sets the noise parameters 61 | # 62 | 63 | def set_noise(self, new_s_noise, new_d_noise): 64 | # makes it possible to change the noise parameters 65 | # this is often useful in particle filters 66 | self.steering_noise = float(new_s_noise) 67 | self.distance_noise = float(new_d_noise) 68 | 69 | # -------- 70 | # set_steering_drift: 71 | # sets the systematical steering drift parameter 72 | # 73 | 74 | def set_steering_drift(self, drift): 75 | self.steering_drift = drift 76 | 77 | # -------- 78 | # move: 79 | # steering = front wheel steering angle, limited by max_steering_angle 80 | # distance = total distance driven, most be non-negative 81 | 82 | def move(self, steering, distance, 83 | tolerance = 0.001, max_steering_angle = pi / 4.0): 84 | 85 | if steering > max_steering_angle: 86 | steering = max_steering_angle 87 | if steering < -max_steering_angle: 88 | steering = -max_steering_angle 89 | if distance < 0.0: 90 | distance = 0.0 91 | 92 | 93 | # make a new copy 94 | res = robot() 95 | res.length = self.length 96 | res.steering_noise = self.steering_noise 97 | res.distance_noise = self.distance_noise 98 | res.steering_drift = self.steering_drift 99 | 100 | # apply noise 101 | steering2 = random.gauss(steering, self.steering_noise) 102 | distance2 = random.gauss(distance, self.distance_noise) 103 | 104 | # apply steering drift 105 | steering2 += self.steering_drift 106 | 107 | # Execute motion 108 | turn = tan(steering2) * distance2 / res.length 109 | 110 | if abs(turn) < tolerance: 111 | 112 | # approximate by straight line motion 113 | 114 | res.x = self.x + (distance2 * cos(self.orientation)) 115 | res.y = self.y + (distance2 * sin(self.orientation)) 116 | res.orientation = (self.orientation + turn) % (2.0 * pi) 117 | 118 | else: 119 | 120 | # approximate bicycle model for motion 121 | 122 | radius = distance2 / turn 123 | cx = self.x - (sin(self.orientation) * radius) 124 | cy = self.y + (cos(self.orientation) * radius) 125 | res.orientation = (self.orientation + turn) % (2.0 * pi) 126 | res.x = cx + (sin(res.orientation) * radius) 127 | res.y = cy - (cos(res.orientation) * radius) 128 | 129 | return res 130 | 131 | 132 | 133 | 134 | def __repr__(self): 135 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 136 | 137 | 138 | ############## ONLY ADD / MODIFY CODE BELOW THIS LINE #################### 139 | 140 | def cte(self, radius): 141 | if self.x 3* radius: 144 | cte = sqrt((self.x-3.0*radius)**2+(self.y-radius)**2)-radius 145 | elif self.y>radius: 146 | cte = self.y - 2.0*radius 147 | else: 148 | cte = -self.y 149 | return cte 150 | # def cte(self, radius): 151 | # if self.x <0: 152 | # self.x = 0 153 | # elif self.x>100: 154 | # self.x = 100 155 | 156 | # if self.x< radius and self.orientation <= 0.5*pi: 157 | # cte = self.y-(sqrt(radius**2-(self.x-radius)**2)+radius) 158 | # elif radius <= self.x <= 3*radius and (self.orientation<0.5*pi or self.orientation>1.5*pi ): 159 | # cte = self.y - radius*2 160 | # elif self.x>3*radius and self.orientation >= 1.5*pi: 161 | # cte = self.y-(sqrt(radius**2-(self.x-3*radius)**2)+radius) 162 | # elif self.x>3*radius and self.orientation <= 1.5*pi: 163 | # cte = self.y-(sqrt(radius**2-(self.x-3*radius)**2)-radius) 164 | # elif radius <= self.x <= 3*radius and 0.5*pi < self.orientation < 1.5*pi : 165 | # cte = -self.y 166 | # elif self.x< radius and self.orientation > 0.5*pi: 167 | # cte = -self.y+(sqrt(radius**2-(self.x-radius)**2)-radius) 168 | # return cte 169 | ############## ONLY ADD / MODIFY CODE ABOVE THIS LINE #################### 170 | 171 | 172 | 173 | 174 | # ------------------------------------------------------------------------ 175 | # 176 | # run - does a single control run. 177 | 178 | 179 | def run(params, radius, printflag = False): 180 | myrobot = robot() 181 | myrobot.set(0.0, radius, pi / 2.0) 182 | speed = 1.0 # motion distance is equal to speed (we assume time = 1) 183 | err = 0.0 184 | int_crosstrack_error = 0.0 185 | N = 200 186 | 187 | crosstrack_error = myrobot.cte(radius) # You need to define the cte function! 188 | 189 | for i in range(N*2): 190 | diff_crosstrack_error = - crosstrack_error 191 | crosstrack_error = myrobot.cte(radius) 192 | diff_crosstrack_error += crosstrack_error 193 | int_crosstrack_error += crosstrack_error 194 | steer = - params[0] * crosstrack_error \ 195 | - params[1] * diff_crosstrack_error \ 196 | - params[2] * int_crosstrack_error 197 | myrobot = myrobot.move(steer, speed) 198 | if i >= N: 199 | err += crosstrack_error ** 2 200 | if printflag: 201 | print myrobot 202 | return err / float(N) 203 | 204 | radius = 25.0 205 | params = [10.0, 15.0, 0] 206 | err = run(params, radius, True) 207 | print '\nFinal parameters: ', params, '\n ->', err 208 | -------------------------------------------------------------------------------- /Lessson6-SLAM/Confident Measurements.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions 3 | # 4 | # Modify the previous code to adjust for a highly 5 | # confident last measurement. Do this by adding a 6 | # factor of 5 into your Omega and Xi matrices 7 | # as described in the video. 8 | 9 | 10 | 11 | from math import * 12 | import random 13 | 14 | 15 | #=============================================================== 16 | # 17 | # SLAM in a rectolinear world (we avoid non-linearities) 18 | # 19 | # 20 | #=============================================================== 21 | 22 | 23 | # ------------------------------------------------ 24 | # 25 | # this is the matrix class 26 | # we use it because it makes it easier to collect constraints in GraphSLAM 27 | # and to calculate solutions (albeit inefficiently) 28 | # 29 | 30 | class matrix: 31 | 32 | # implements basic operations of a matrix class 33 | 34 | # ------------ 35 | # 36 | # initialization - can be called with an initial matrix 37 | # 38 | 39 | def __init__(self, value = [[]]): 40 | self.value = value 41 | self.dimx = len(value) 42 | self.dimy = len(value[0]) 43 | if value == [[]]: 44 | self.dimx = 0 45 | 46 | # ------------ 47 | # 48 | # makes matrix of a certain size and sets each element to zero 49 | # 50 | 51 | def zero(self, dimx, dimy = 0): 52 | if dimy == 0: 53 | dimy = dimx 54 | # check if valid dimensions 55 | if dimx < 1 or dimy < 1: 56 | raise ValueError, "Invalid size of matrix" 57 | else: 58 | self.dimx = dimx 59 | self.dimy = dimy 60 | self.value = [[0.0 for row in range(dimy)] for col in range(dimx)] 61 | 62 | # ------------ 63 | # 64 | # makes matrix of a certain (square) size and turns matrix into identity matrix 65 | # 66 | 67 | 68 | def identity(self, dim): 69 | # check if valid dimension 70 | if dim < 1: 71 | raise ValueError, "Invalid size of matrix" 72 | else: 73 | self.dimx = dim 74 | self.dimy = dim 75 | self.value = [[0.0 for row in range(dim)] for col in range(dim)] 76 | for i in range(dim): 77 | self.value[i][i] = 1.0 78 | # ------------ 79 | # 80 | # prints out values of matrix 81 | # 82 | 83 | 84 | def show(self, txt = ''): 85 | for i in range(len(self.value)): 86 | print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']' 87 | print ' ' 88 | 89 | # ------------ 90 | # 91 | # defines elmement-wise matrix addition. Both matrices must be of equal dimensions 92 | # 93 | 94 | 95 | def __add__(self, other): 96 | # check if correct dimensions 97 | if self.dimx != other.dimx or self.dimx != other.dimx: 98 | raise ValueError, "Matrices must be of equal dimension to add" 99 | else: 100 | # add if correct dimensions 101 | res = matrix() 102 | res.zero(self.dimx, self.dimy) 103 | for i in range(self.dimx): 104 | for j in range(self.dimy): 105 | res.value[i][j] = self.value[i][j] + other.value[i][j] 106 | return res 107 | 108 | # ------------ 109 | # 110 | # defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions 111 | # 112 | 113 | def __sub__(self, other): 114 | # check if correct dimensions 115 | if self.dimx != other.dimx or self.dimx != other.dimx: 116 | raise ValueError, "Matrices must be of equal dimension to subtract" 117 | else: 118 | # subtract if correct dimensions 119 | res = matrix() 120 | res.zero(self.dimx, self.dimy) 121 | for i in range(self.dimx): 122 | for j in range(self.dimy): 123 | res.value[i][j] = self.value[i][j] - other.value[i][j] 124 | return res 125 | 126 | # ------------ 127 | # 128 | # defines multiplication. Both matrices must be of fitting dimensions 129 | # 130 | 131 | 132 | def __mul__(self, other): 133 | # check if correct dimensions 134 | if self.dimy != other.dimx: 135 | raise ValueError, "Matrices must be m*n and n*p to multiply" 136 | else: 137 | # multiply if correct dimensions 138 | res = matrix() 139 | res.zero(self.dimx, other.dimy) 140 | for i in range(self.dimx): 141 | for j in range(other.dimy): 142 | for k in range(self.dimy): 143 | res.value[i][j] += self.value[i][k] * other.value[k][j] 144 | return res 145 | 146 | 147 | # ------------ 148 | # 149 | # returns a matrix transpose 150 | # 151 | 152 | 153 | def transpose(self): 154 | # compute transpose 155 | res = matrix() 156 | res.zero(self.dimy, self.dimx) 157 | for i in range(self.dimx): 158 | for j in range(self.dimy): 159 | res.value[j][i] = self.value[i][j] 160 | return res 161 | 162 | # ------------ 163 | # 164 | # creates a new matrix from the existing matrix elements. 165 | # 166 | # Example: 167 | # l = matrix([[ 1, 2, 3, 4, 5], 168 | # [ 6, 7, 8, 9, 10], 169 | # [11, 12, 13, 14, 15]]) 170 | # 171 | # l.take([0, 2], [0, 2, 3]) 172 | # 173 | # results in: 174 | # 175 | # [[1, 3, 4], 176 | # [11, 13, 14]] 177 | # 178 | # 179 | # take is used to remove rows and columns from existing matrices 180 | # list1/list2 define a sequence of rows/columns that shall be taken 181 | # is no list2 is provided, then list2 is set to list1 (good for symmetric matrices) 182 | # 183 | 184 | def take(self, list1, list2 = []): 185 | if list2 == []: 186 | list2 = list1 187 | if len(list1) > self.dimx or len(list2) > self.dimy: 188 | raise ValueError, "list invalid in take()" 189 | 190 | res = matrix() 191 | res.zero(len(list1), len(list2)) 192 | for i in range(len(list1)): 193 | for j in range(len(list2)): 194 | res.value[i][j] = self.value[list1[i]][list2[j]] 195 | return res 196 | 197 | # ------------ 198 | # 199 | # creates a new matrix from the existing matrix elements. 200 | # 201 | # Example: 202 | # l = matrix([[1, 2, 3], 203 | # [4, 5, 6]]) 204 | # 205 | # l.expand(3, 5, [0, 2], [0, 2, 3]) 206 | # 207 | # results in: 208 | # 209 | # [[1, 0, 2, 3, 0], 210 | # [0, 0, 0, 0, 0], 211 | # [4, 0, 5, 6, 0]] 212 | # 213 | # expand is used to introduce new rows and columns into an existing matrix 214 | # list1/list2 are the new indexes of row/columns in which the matrix 215 | # elements are being mapped. Elements for rows and columns 216 | # that are not listed in list1/list2 217 | # will be initialized by 0.0. 218 | # 219 | 220 | def expand(self, dimx, dimy, list1, list2 = []): 221 | if list2 == []: 222 | list2 = list1 223 | if len(list1) > self.dimx or len(list2) > self.dimy: 224 | raise ValueError, "list invalid in expand()" 225 | 226 | res = matrix() 227 | res.zero(dimx, dimy) 228 | for i in range(len(list1)): 229 | for j in range(len(list2)): 230 | res.value[list1[i]][list2[j]] = self.value[i][j] 231 | return res 232 | 233 | # ------------ 234 | # 235 | # Computes the upper triangular Cholesky factorization of 236 | # a positive definite matrix. 237 | # This code is based on http://adorio-research.org/wordpress/?p=4560 238 | 239 | def Cholesky(self, ztol= 1.0e-5): 240 | 241 | 242 | res = matrix() 243 | res.zero(self.dimx, self.dimx) 244 | 245 | for i in range(self.dimx): 246 | S = sum([(res.value[k][i])**2 for k in range(i)]) 247 | d = self.value[i][i] - S 248 | if abs(d) < ztol: 249 | res.value[i][i] = 0.0 250 | else: 251 | if d < 0.0: 252 | raise ValueError, "Matrix not positive-definite" 253 | res.value[i][i] = sqrt(d) 254 | for j in range(i+1, self.dimx): 255 | S = sum([res.value[k][i] * res.value[k][j] for k in range(i)]) 256 | if abs(S) < ztol: 257 | S = 0.0 258 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 259 | return res 260 | 261 | # ------------ 262 | # 263 | # Computes inverse of matrix given its Cholesky upper Triangular 264 | # decomposition of matrix. 265 | # This code is based on http://adorio-research.org/wordpress/?p=4560 266 | 267 | def CholeskyInverse(self): 268 | # Computes inverse of matrix given its Cholesky upper Triangular 269 | # decomposition of matrix. 270 | # This code is based on http://adorio-research.org/wordpress/?p=4560 271 | 272 | res = matrix() 273 | res.zero(self.dimx, self.dimx) 274 | 275 | # Backward step for inverse. 276 | for j in reversed(range(self.dimx)): 277 | tjj = self.value[j][j] 278 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 279 | res.value[j][j] = 1.0/ tjj**2 - S/ tjj 280 | for i in reversed(range(j)): 281 | res.value[j][i] = res.value[i][j] = \ 282 | -sum([self.value[i][k]*res.value[k][j] for k in \ 283 | range(i+1,self.dimx)])/self.value[i][i] 284 | return res 285 | 286 | # ------------ 287 | # 288 | # comutes and returns the inverse of a square matrix 289 | # 290 | 291 | 292 | def inverse(self): 293 | aux = self.Cholesky() 294 | res = aux.CholeskyInverse() 295 | return res 296 | 297 | # ------------ 298 | # 299 | # prints matrix (needs work!) 300 | # 301 | 302 | 303 | def __repr__(self): 304 | return repr(self.value) 305 | 306 | 307 | # ###################################################################### 308 | # ###################################################################### 309 | # ###################################################################### 310 | 311 | 312 | # Including the 5 times multiplier, your returned mu should now be: 313 | # 314 | # [[-3.0], 315 | # [2.179], 316 | # [5.714], 317 | # [6.821]] 318 | 319 | 320 | 321 | ############## MODIFY CODE BELOW ################## 322 | 323 | def doit(initial_pos, move1, move2, Z0, Z1, Z2): 324 | Omega = matrix([[1.0, 0.0, 0.0], 325 | [0.0, 0.0, 0.0], 326 | [0.0, 0.0, 0.0]]) 327 | Xi = matrix([[initial_pos], 328 | [0.0], 329 | [0.0]]) 330 | 331 | Omega += matrix([[1.0, -1.0, 0.0], 332 | [-1.0, 1.0, 0.0], 333 | [0.0, 0.0, 0.0]]) 334 | Xi += matrix([[-move1], 335 | [move1], 336 | [0.0]]) 337 | 338 | Omega += matrix([[0.0, 0.0, 0.0], 339 | [0.0, 1.0, -1.0], 340 | [0.0, -1.0, 1.0]]) 341 | Xi += matrix([[0.0], 342 | [-move2], 343 | [move2]]) 344 | 345 | Omega = Omega.expand(4, 4, [0, 1, 2], [0, 1, 2]) 346 | Xi = Xi.expand(4, 1, [0, 1, 2], [0]) 347 | 348 | Omega += matrix([[1.0, 0.0, 0.0, -1.0], 349 | [0.0, 0.0, 0.0, 0.0], 350 | [0.0, 0.0, 0.0, 0.0], 351 | [-1.0, 0.0, 0.0, 1.0]]) 352 | Xi += matrix([[-Z0], 353 | [0.0], 354 | [0.0], 355 | [Z0]]) 356 | 357 | Omega += matrix([[0.0, 0.0, 0.0, 0.0], 358 | [0.0, 1.0, 0.0, -1.0], 359 | [0.0, 0.0, 0.0, 0.0], 360 | [0.0, -1.0, 0.0, 1.0]]) 361 | Xi += matrix([[0.0], 362 | [-Z1], 363 | [0.0], 364 | [Z1]]) 365 | 366 | Omega += matrix([[0.0, 0.0, 0.0, 0.0], 367 | [0.0, 0.0, 0.0, 0.0], 368 | [0.0, 0.0, 5.0, -5.0], 369 | [0.0, 0.0, -5.0, 5.0]]) 370 | Xi += matrix([[0.0], 371 | [0.0], 372 | [-5*Z2], 373 | [5*Z2]]) 374 | 375 | Omega.show('Omega: ') 376 | Xi.show('Xi: ') 377 | mu = Omega.inverse() * Xi 378 | mu.show('Mu: ') 379 | 380 | return mu 381 | 382 | doit(-3, 5, 3, 10, 5, 1) 383 | 384 | 385 | -------------------------------------------------------------------------------- /Lessson6-SLAM/Expand.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions 3 | # 4 | # Modify your doit function to incorporate 3 5 | # distance measurements to a landmark(Z0, Z1, Z2). 6 | # You should use the provided expand function to 7 | # allow your Omega and Xi matrices to accomodate 8 | # the new information. 9 | # 10 | # Each landmark measurement should modify 4 11 | # values in your Omega matrix and 2 in your 12 | # Xi vector. 13 | # 14 | # Add your code at line 356. 15 | 16 | from math import * 17 | import random 18 | 19 | #=============================================================== 20 | # 21 | # SLAM in a rectolinear world (we avoid non-linearities) 22 | # 23 | # 24 | #=============================================================== 25 | 26 | 27 | # ------------------------------------------------ 28 | # 29 | # this is the matrix class 30 | # we use it because it makes it easier to collect constraints in GraphSLAM 31 | # and to calculate solutions (albeit inefficiently) 32 | # 33 | 34 | class matrix: 35 | 36 | # implements basic operations of a matrix class 37 | 38 | # ------------ 39 | # 40 | # initialization - can be called with an initial matrix 41 | # 42 | 43 | def __init__(self, value = [[]]): 44 | self.value = value 45 | self.dimx = len(value) 46 | self.dimy = len(value[0]) 47 | if value == [[]]: 48 | self.dimx = 0 49 | 50 | # ------------ 51 | # 52 | # makes matrix of a certain size and sets each element to zero 53 | # 54 | 55 | def zero(self, dimx, dimy = 0): 56 | if dimy == 0: 57 | dimy = dimx 58 | # check if valid dimensions 59 | if dimx < 1 or dimy < 1: 60 | raise ValueError, "Invalid size of matrix" 61 | else: 62 | self.dimx = dimx 63 | self.dimy = dimy 64 | self.value = [[0.0 for row in range(dimy)] for col in range(dimx)] 65 | 66 | # ------------ 67 | # 68 | # makes matrix of a certain (square) size and turns matrix into identity matrix 69 | # 70 | 71 | 72 | def identity(self, dim): 73 | # check if valid dimension 74 | if dim < 1: 75 | raise ValueError, "Invalid size of matrix" 76 | else: 77 | self.dimx = dim 78 | self.dimy = dim 79 | self.value = [[0.0 for row in range(dim)] for col in range(dim)] 80 | for i in range(dim): 81 | self.value[i][i] = 1.0 82 | # ------------ 83 | # 84 | # prints out values of matrix 85 | # 86 | 87 | 88 | def show(self, txt = ''): 89 | for i in range(len(self.value)): 90 | print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']' 91 | print ' ' 92 | 93 | # ------------ 94 | # 95 | # defines elmement-wise matrix addition. Both matrices must be of equal dimensions 96 | # 97 | 98 | 99 | def __add__(self, other): 100 | # check if correct dimensions 101 | if self.dimx != other.dimx or self.dimx != other.dimx: 102 | raise ValueError, "Matrices must be of equal dimension to add" 103 | else: 104 | # add if correct dimensions 105 | res = matrix() 106 | res.zero(self.dimx, self.dimy) 107 | for i in range(self.dimx): 108 | for j in range(self.dimy): 109 | res.value[i][j] = self.value[i][j] + other.value[i][j] 110 | return res 111 | 112 | # ------------ 113 | # 114 | # defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions 115 | # 116 | 117 | def __sub__(self, other): 118 | # check if correct dimensions 119 | if self.dimx != other.dimx or self.dimx != other.dimx: 120 | raise ValueError, "Matrices must be of equal dimension to subtract" 121 | else: 122 | # subtract if correct dimensions 123 | res = matrix() 124 | res.zero(self.dimx, self.dimy) 125 | for i in range(self.dimx): 126 | for j in range(self.dimy): 127 | res.value[i][j] = self.value[i][j] - other.value[i][j] 128 | return res 129 | 130 | # ------------ 131 | # 132 | # defines multiplication. Both matrices must be of fitting dimensions 133 | # 134 | 135 | 136 | def __mul__(self, other): 137 | # check if correct dimensions 138 | if self.dimy != other.dimx: 139 | raise ValueError, "Matrices must be m*n and n*p to multiply" 140 | else: 141 | # multiply if correct dimensions 142 | res = matrix() 143 | res.zero(self.dimx, other.dimy) 144 | for i in range(self.dimx): 145 | for j in range(other.dimy): 146 | for k in range(self.dimy): 147 | res.value[i][j] += self.value[i][k] * other.value[k][j] 148 | return res 149 | 150 | 151 | # ------------ 152 | # 153 | # returns a matrix transpose 154 | # 155 | 156 | 157 | def transpose(self): 158 | # compute transpose 159 | res = matrix() 160 | res.zero(self.dimy, self.dimx) 161 | for i in range(self.dimx): 162 | for j in range(self.dimy): 163 | res.value[j][i] = self.value[i][j] 164 | return res 165 | 166 | # ------------ 167 | # 168 | # creates a new matrix from the existing matrix elements. 169 | # 170 | # Example: 171 | # l = matrix([[ 1, 2, 3, 4, 5], 172 | # [ 6, 7, 8, 9, 10], 173 | # [11, 12, 13, 14, 15]]) 174 | # 175 | # l.take([0, 2], [0, 2, 3]) 176 | # 177 | # results in: 178 | # 179 | # [[1, 3, 4], 180 | # [11, 13, 14]] 181 | # 182 | # 183 | # take is used to remove rows and columns from existing matrices 184 | # list1/list2 define a sequence of rows/columns that shall be taken 185 | # is no list2 is provided, then list2 is set to list1 (good for symmetric matrices) 186 | # 187 | 188 | def take(self, list1, list2 = []): 189 | if list2 == []: 190 | list2 = list1 191 | if len(list1) > self.dimx or len(list2) > self.dimy: 192 | raise ValueError, "list invalid in take()" 193 | 194 | res = matrix() 195 | res.zero(len(list1), len(list2)) 196 | for i in range(len(list1)): 197 | for j in range(len(list2)): 198 | res.value[i][j] = self.value[list1[i]][list2[j]] 199 | return res 200 | 201 | # ------------ 202 | # 203 | # creates a new matrix from the existing matrix elements. 204 | # 205 | # Example: 206 | # l = matrix([[1, 2, 3], 207 | # [4, 5, 6]]) 208 | # 209 | # l.expand(3, 5, [0, 2], [0, 2, 3]) 210 | # 211 | # results in: 212 | # 213 | # [[1, 0, 2, 3, 0], 214 | # [0, 0, 0, 0, 0], 215 | # [4, 0, 5, 6, 0]] 216 | # 217 | # expand is used to introduce new rows and columns into an existing matrix 218 | # list1/list2 are the new indexes of row/columns in which the matrix 219 | # elements are being mapped. Elements for rows and columns 220 | # that are not listed in list1/list2 221 | # will be initialized by 0.0. 222 | # 223 | 224 | def expand(self, dimx, dimy, list1, list2 = []): 225 | if list2 == []: 226 | list2 = list1 227 | if len(list1) > self.dimx or len(list2) > self.dimy: 228 | raise ValueError, "list invalid in expand()" 229 | 230 | res = matrix() 231 | res.zero(dimx, dimy) 232 | for i in range(len(list1)): 233 | for j in range(len(list2)): 234 | res.value[list1[i]][list2[j]] = self.value[i][j] 235 | return res 236 | 237 | # ------------ 238 | # 239 | # Computes the upper triangular Cholesky factorization of 240 | # a positive definite matrix. 241 | # This code is based on http://adorio-research.org/wordpress/?p=4560 242 | 243 | def Cholesky(self, ztol= 1.0e-5): 244 | 245 | 246 | res = matrix() 247 | res.zero(self.dimx, self.dimx) 248 | 249 | for i in range(self.dimx): 250 | S = sum([(res.value[k][i])**2 for k in range(i)]) 251 | d = self.value[i][i] - S 252 | if abs(d) < ztol: 253 | res.value[i][i] = 0.0 254 | else: 255 | if d < 0.0: 256 | raise ValueError, "Matrix not positive-definite" 257 | res.value[i][i] = sqrt(d) 258 | for j in range(i+1, self.dimx): 259 | S = sum([res.value[k][i] * res.value[k][j] for k in range(i)]) 260 | if abs(S) < ztol: 261 | S = 0.0 262 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 263 | return res 264 | 265 | # ------------ 266 | # 267 | # Computes inverse of matrix given its Cholesky upper Triangular 268 | # decomposition of matrix. 269 | # This code is based on http://adorio-research.org/wordpress/?p=4560 270 | 271 | def CholeskyInverse(self): 272 | # Computes inverse of matrix given its Cholesky upper Triangular 273 | # decomposition of matrix. 274 | # This code is based on http://adorio-research.org/wordpress/?p=4560 275 | 276 | res = matrix() 277 | res.zero(self.dimx, self.dimx) 278 | 279 | # Backward step for inverse. 280 | for j in reversed(range(self.dimx)): 281 | tjj = self.value[j][j] 282 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 283 | res.value[j][j] = 1.0/ tjj**2 - S/ tjj 284 | for i in reversed(range(j)): 285 | res.value[j][i] = res.value[i][j] = \ 286 | -sum([self.value[i][k]*res.value[k][j] for k in \ 287 | range(i+1,self.dimx)])/self.value[i][i] 288 | return res 289 | 290 | # ------------ 291 | # 292 | # comutes and returns the inverse of a square matrix 293 | # 294 | 295 | 296 | def inverse(self): 297 | aux = self.Cholesky() 298 | res = aux.CholeskyInverse() 299 | return res 300 | 301 | # ------------ 302 | # 303 | # prints matrix (needs work!) 304 | # 305 | 306 | 307 | def __repr__(self): 308 | return repr(self.value) 309 | 310 | 311 | 312 | # ###################################################################### 313 | # ###################################################################### 314 | # ###################################################################### 315 | 316 | 317 | """ 318 | For the following example, you would call doit(-3, 5, 3, 10, 5, 2): 319 | 3 robot positions 320 | initially: -3 (measure landmark to be 10 away) 321 | moves by 5 (measure landmark to be 5 away) 322 | moves by 3 (measure landmark to be 2 away) 323 | 324 | 325 | 326 | which should return a mu of: 327 | [[-3.0], 328 | [2.0], 329 | [5.0], 330 | [7.0]] 331 | """ 332 | def doit(initial_pos, move1, move2, Z0, Z1, Z2): 333 | Omega = matrix([[1.0, 0.0, 0.0], 334 | [0.0, 0.0, 0.0], 335 | [0.0, 0.0, 0.0]]) 336 | Xi = matrix([[initial_pos], 337 | [0.0], 338 | [0.0]]) 339 | 340 | Omega += matrix([[1.0, -1.0, 0.0], 341 | [-1.0, 1.0, 0.0], 342 | [0.0, 0.0, 0.0]]) 343 | Xi += matrix([[-move1], 344 | [move1], 345 | [0.0]]) 346 | 347 | Omega += matrix([[0.0, 0.0, 0.0], 348 | [0.0, 1.0, -1.0], 349 | [0.0, -1.0, 1.0]]) 350 | Xi += matrix([[0.0], 351 | [-move2], 352 | [move2]]) 353 | 354 | Omega = Omega.expand(Omega.dimx+1, Omega.dimy+1, list1 = [i for i in range(Omega.dimx)], list2 = []) 355 | Xi = Xi.expand(Xi.dimx+1, Xi.dimy, list1 = [i for i in range(Xi.dimx)], list2 = [0]) 356 | Omega += matrix([[1.0, 0.0, 0.0, -1.0], 357 | [0.0, 1.0, 0.0, -1.0], 358 | [0.0, 0.0, 1.0, -1.0], 359 | [-1.0,-1.0,-1.0,3.0]]) 360 | Xi += matrix([[-Z0], 361 | [-Z1], 362 | [-Z2], 363 | [Z0+Z1+Z2]]) 364 | 365 | Omega.show('Omega: ') 366 | Xi.show('Xi: ') 367 | mu = Omega.inverse() * Xi 368 | mu.show('Mu: ') 369 | 370 | return mu 371 | 372 | doit(-3, 5, 3, 10, 5, 2) 373 | 374 | 375 | -------------------------------------------------------------------------------- /Lessson6-SLAM/Omega and Xi.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions 3 | # 4 | # Write a function, doit, that takes as its input an 5 | # initial robot position, move1, and move2. This 6 | # function should compute the Omega and Xi matrices 7 | # discussed in lecture and should RETURN the mu vector 8 | # (which is the product of Omega.inverse() and Xi). 9 | # 10 | # Please enter your code at the bottom. 11 | 12 | 13 | 14 | from math import * 15 | import random 16 | 17 | 18 | #=============================================================== 19 | # 20 | # SLAM in a rectolinear world (we avoid non-linearities) 21 | # 22 | # 23 | #=============================================================== 24 | 25 | 26 | # ------------------------------------------------ 27 | # 28 | # this is the matrix class 29 | # we use it because it makes it easier to collect constraints in GraphSLAM 30 | # and to calculate solutions (albeit inefficiently) 31 | # 32 | 33 | class matrix: 34 | 35 | # implements basic operations of a matrix class 36 | 37 | # ------------ 38 | # 39 | # initialization - can be called with an initial matrix 40 | # 41 | 42 | def __init__(self, value = [[]]): 43 | self.value = value 44 | self.dimx = len(value) 45 | self.dimy = len(value[0]) 46 | if value == [[]]: 47 | self.dimx = 0 48 | 49 | # ------------ 50 | # 51 | # makes matrix of a certain size and sets each element to zero 52 | # 53 | 54 | def zero(self, dimx, dimy = 0): 55 | if dimy == 0: 56 | dimy = dimx 57 | # check if valid dimensions 58 | if dimx < 1 or dimy < 1: 59 | raise ValueError, "Invalid size of matrix" 60 | else: 61 | self.dimx = dimx 62 | self.dimy = dimy 63 | self.value = [[0.0 for row in range(dimy)] for col in range(dimx)] 64 | 65 | # ------------ 66 | # 67 | # makes matrix of a certain (square) size and turns matrix into identity matrix 68 | # 69 | 70 | 71 | def identity(self, dim): 72 | # check if valid dimension 73 | if dim < 1: 74 | raise ValueError, "Invalid size of matrix" 75 | else: 76 | self.dimx = dim 77 | self.dimy = dim 78 | self.value = [[0.0 for row in range(dim)] for col in range(dim)] 79 | for i in range(dim): 80 | self.value[i][i] = 1.0 81 | # ------------ 82 | # 83 | # prints out values of matrix 84 | # 85 | 86 | 87 | def show(self, txt = ''): 88 | for i in range(len(self.value)): 89 | print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']' 90 | print ' ' 91 | 92 | # ------------ 93 | # 94 | # defines elmement-wise matrix addition. Both matrices must be of equal dimensions 95 | # 96 | 97 | 98 | def __add__(self, other): 99 | # check if correct dimensions 100 | if self.dimx != other.dimx or self.dimx != other.dimx: 101 | raise ValueError, "Matrices must be of equal dimension to add" 102 | else: 103 | # add if correct dimensions 104 | res = matrix() 105 | res.zero(self.dimx, self.dimy) 106 | for i in range(self.dimx): 107 | for j in range(self.dimy): 108 | res.value[i][j] = self.value[i][j] + other.value[i][j] 109 | return res 110 | 111 | # ------------ 112 | # 113 | # defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions 114 | # 115 | 116 | def __sub__(self, other): 117 | # check if correct dimensions 118 | if self.dimx != other.dimx or self.dimx != other.dimx: 119 | raise ValueError, "Matrices must be of equal dimension to subtract" 120 | else: 121 | # subtract if correct dimensions 122 | res = matrix() 123 | res.zero(self.dimx, self.dimy) 124 | for i in range(self.dimx): 125 | for j in range(self.dimy): 126 | res.value[i][j] = self.value[i][j] - other.value[i][j] 127 | return res 128 | 129 | # ------------ 130 | # 131 | # defines multiplication. Both matrices must be of fitting dimensions 132 | # 133 | 134 | 135 | def __mul__(self, other): 136 | # check if correct dimensions 137 | if self.dimy != other.dimx: 138 | raise ValueError, "Matrices must be m*n and n*p to multiply" 139 | else: 140 | # multiply if correct dimensions 141 | res = matrix() 142 | res.zero(self.dimx, other.dimy) 143 | for i in range(self.dimx): 144 | for j in range(other.dimy): 145 | for k in range(self.dimy): 146 | res.value[i][j] += self.value[i][k] * other.value[k][j] 147 | return res 148 | 149 | 150 | # ------------ 151 | # 152 | # returns a matrix transpose 153 | # 154 | 155 | 156 | def transpose(self): 157 | # compute transpose 158 | res = matrix() 159 | res.zero(self.dimy, self.dimx) 160 | for i in range(self.dimx): 161 | for j in range(self.dimy): 162 | res.value[j][i] = self.value[i][j] 163 | return res 164 | 165 | # ------------ 166 | # 167 | # creates a new matrix from the existing matrix elements. 168 | # 169 | # Example: 170 | # l = matrix([[ 1, 2, 3, 4, 5], 171 | # [ 6, 7, 8, 9, 10], 172 | # [11, 12, 13, 14, 15]]) 173 | # 174 | # l.take([0, 2], [0, 2, 3]) 175 | # 176 | # results in: 177 | # 178 | # [[1, 3, 4], 179 | # [11, 13, 14]] 180 | # 181 | # 182 | # take is used to remove rows and columns from existing matrices 183 | # list1/list2 define a sequence of rows/columns that shall be taken 184 | # is no list2 is provided, then list2 is set to list1 (good for symmetric matrices) 185 | # 186 | 187 | def take(self, list1, list2 = []): 188 | if list2 == []: 189 | list2 = list1 190 | if len(list1) > self.dimx or len(list2) > self.dimy: 191 | raise ValueError, "list invalid in take()" 192 | 193 | res = matrix() 194 | res.zero(len(list1), len(list2)) 195 | for i in range(len(list1)): 196 | for j in range(len(list2)): 197 | res.value[i][j] = self.value[list1[i]][list2[j]] 198 | return res 199 | 200 | # ------------ 201 | # 202 | # creates a new matrix from the existing matrix elements. 203 | # 204 | # Example: 205 | # l = matrix([[1, 2, 3], 206 | # [4, 5, 6]]) 207 | # 208 | # l.expand(3, 5, [0, 2], [0, 2, 3]) 209 | # 210 | # results in: 211 | # 212 | # [[1, 0, 2, 3, 0], 213 | # [0, 0, 0, 0, 0], 214 | # [4, 0, 5, 6, 0]] 215 | # 216 | # expand is used to introduce new rows and columns into an existing matrix 217 | # list1/list2 are the new indexes of row/columns in which the matrix 218 | # elements are being mapped. Elements for rows and columns 219 | # that are not listed in list1/list2 220 | # will be initialized by 0.0. 221 | # 222 | 223 | def expand(self, dimx, dimy, list1, list2 = []): 224 | if list2 == []: 225 | list2 = list1 226 | if len(list1) > self.dimx or len(list2) > self.dimy: 227 | raise ValueError, "list invalid in expand()" 228 | 229 | res = matrix() 230 | res.zero(dimx, dimy) 231 | for i in range(len(list1)): 232 | for j in range(len(list2)): 233 | res.value[list1[i]][list2[j]] = self.value[i][j] 234 | return res 235 | 236 | # ------------ 237 | # 238 | # Computes the upper triangular Cholesky factorization of 239 | # a positive definite matrix. 240 | # This code is based on http://adorio-research.org/wordpress/?p=4560 241 | 242 | def Cholesky(self, ztol= 1.0e-5): 243 | res = matrix() 244 | res.zero(self.dimx, self.dimx) 245 | 246 | for i in range(self.dimx): 247 | S = sum([(res.value[k][i])**2 for k in range(i)]) 248 | d = self.value[i][i] - S 249 | if abs(d) < ztol: 250 | res.value[i][i] = 0.0 251 | else: 252 | if d < 0.0: 253 | raise ValueError, "Matrix not positive-definite" 254 | res.value[i][i] = sqrt(d) 255 | for j in range(i+1, self.dimx): 256 | S = sum([res.value[k][i] * res.value[k][j] for k in range(i)]) 257 | if abs(S) < ztol: 258 | S = 0.0 259 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 260 | return res 261 | 262 | # ------------ 263 | # 264 | # Computes inverse of matrix given its Cholesky upper Triangular 265 | # decomposition of matrix. 266 | # This code is based on http://adorio-research.org/wordpress/?p=4560 267 | 268 | def CholeskyInverse(self): 269 | res = matrix() 270 | res.zero(self.dimx, self.dimx) 271 | 272 | # Backward step for inverse. 273 | for j in reversed(range(self.dimx)): 274 | tjj = self.value[j][j] 275 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 276 | res.value[j][j] = 1.0/ tjj**2 - S/ tjj 277 | for i in reversed(range(j)): 278 | res.value[j][i] = res.value[i][j] = \ 279 | -sum([self.value[i][k]*res.value[k][j] for k in \ 280 | range(i+1,self.dimx)])/self.value[i][i] 281 | return res 282 | 283 | # ------------ 284 | # 285 | # comutes and returns the inverse of a square matrix 286 | # 287 | 288 | 289 | def inverse(self): 290 | aux = self.Cholesky() 291 | print aux 292 | res = aux.CholeskyInverse() 293 | return res 294 | 295 | # ------------ 296 | # 297 | # prints matrix (needs work!) 298 | # 299 | 300 | 301 | def __repr__(self): 302 | return repr(self.value) 303 | 304 | 305 | 306 | # ###################################################################### 307 | # ###################################################################### 308 | # ###################################################################### 309 | 310 | 311 | """ 312 | For the following example, you would call doit(-3, 5, 3): 313 | 3 robot positions 314 | initially: -3 315 | moves by 5 316 | moves by 3 317 | 318 | 319 | 320 | which should return a mu of: 321 | [[-3.0], 322 | [2.0], 323 | [5.0]] 324 | """ 325 | def doit(initial_pos, move1, move2): 326 | Omega = matrix([[1,0,0],[0,0,0],[0,0,0]]) 327 | Xi = matrix([[-3,0,0]]) 328 | 329 | Omega += matrix([[1,-1,0],[-1,1,0],[0,0,0]]) 330 | Xi += matrix([[-5,5,0]]) 331 | 332 | Omega += matrix([[0,0,0],[0,1,-1],[0,-1,1]]) 333 | Xi += matrix([[0,-3,3]]) 334 | 335 | mu = Omega.inverse()*Xi.transpose() 336 | return mu 337 | 338 | print (doit(-3, 5, 3)) 339 | 340 | #奇异值分解 只能算对称正定矩阵 341 | -------------------------------------------------------------------------------- /Practice Exam/Question 1-Question 11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lijian-ml/CS373-Programming-a-Robotic-Car/0f55f6649b36d0306c14de4050c1ee90446993de/Practice Exam/Question 1-Question 11.jpg -------------------------------------------------------------------------------- /Practice Exam/Warehouse Robot.py: -------------------------------------------------------------------------------- 1 | # ------------------- 2 | # Background Information 3 | # 4 | # In this problem, you will build a planner that helps a robot 5 | # find the shortest way in a warehouse filled with boxes 6 | # that he has to pick up and deliver to a drop zone. 7 | # 8 | # For example: 9 | # 10 | # warehouse = [[ 1, 2, 3], 11 | # [ 0, 0, 0], 12 | # [ 0, 0, 0]] 13 | # dropzone = [2,0] 14 | # todo = [2, 1] 15 | # 16 | # The robot starts at the dropzone. 17 | # The dropzone can be in any free corner of the warehouse map. 18 | # todo is a list of boxes to be picked up and delivered to the dropzone. 19 | # 20 | # Robot can move diagonally, but the cost of a diagonal move is 1.5. 21 | # The cost of moving one step horizontally or vertically is 1. 22 | # So if the dropzone is at [2, 0], the cost to deliver box number 2 23 | # would be 5. 24 | 25 | # To pick up a box, the robot has to move into the same cell as the box. 26 | # When the robot picks up a box, that cell becomes passable (marked 0) 27 | # The robot can pick up only one box at a time and once picked up 28 | # it has to return the box to the dropzone by moving onto the dropzone cell. 29 | # Once the robot has stepped on the dropzone, the box is taken away, 30 | # and it is free to continue with its todo list. 31 | # Tasks must be executed in the order that they are given in the todo list. 32 | # You may assume that in all warehouse maps, all boxes are 33 | # reachable from beginning (the robot is not boxed in). 34 | 35 | # ------------------- 36 | # User Instructions 37 | # 38 | # Design a planner (any kind you like, so long as it works!) 39 | # in a function named plan() that takes as input three parameters: 40 | # warehouse, dropzone, and todo. See parameter info below. 41 | # 42 | # Your function should RETURN the final, accumulated cost to do 43 | # all tasks in the todo list in the given order, which should 44 | # match with our answer. You may include print statements to show 45 | # the optimum path, but that will have no effect on grading. 46 | # 47 | # Your solution must work for a variety of warehouse layouts and 48 | # any length of todo list. 49 | # 50 | # Add your code at line 76. 51 | # 52 | # -------------------- 53 | # Parameter Info 54 | # 55 | # warehouse - a grid of values, where 0 means that the cell is passable, 56 | # and a number 1 <= n <= 99 means that box n is located at that cell. 57 | # dropzone - determines the robot's start location and the place to return boxes 58 | # todo - list of tasks, containing box numbers that have to be picked up 59 | # 60 | # -------------------- 61 | # Testing 62 | # 63 | # You may use our test function below, solution_check(), 64 | # to test your code for a variety of input parameters. 65 | 66 | warehouse = [[ 1, 2, 3], 67 | [ 0, 0, 0], 68 | [ 0, 0, 0]] 69 | dropzone = [2,0] 70 | todo = [2, 1] 71 | 72 | # ------------------------------------------ 73 | # plan - Returns cost to take all boxes in the todo list to dropzone 74 | # 75 | # ---------------------------------------- 76 | # modify code below 77 | # ---------------------------------------- 78 | def plan(warehouse, dropzone, todo): 79 | state = [[1,0],[0,1],[-1,0],[0,-1]] 80 | state_decline = [[1,1],[-1,1],[-1,-1],[1,-1]] 81 | cost = 0 82 | 83 | for ii in range(len(todo)): 84 | warehouse_flag = [[0 for q in e] for e in warehouse] 85 | next_state = [dropzone] 86 | while 1: 87 | next_state1 = [] 88 | for i in range(len(next_state)): 89 | for j in range(4): 90 | first_d = next_state[i][0]+state[j][0] 91 | second_d = next_state[i][1]+state[j][1] 92 | first_d_decline = next_state[i][0]+state_decline[j][0] 93 | second_d_decline = next_state[i][1]+state_decline[j][1] 94 | if 0<= first_d_decline 1: 139 | print "Your code is too slow, try to optimize it! Running time was: ", runtime 140 | return False 141 | if correct_answers == len(answer_list): 142 | print "\nYou passed all test cases!" 143 | return True 144 | else: 145 | print "\nYou passed", correct_answers, "of", len(answer_list), "test cases. Try to get them all!" 146 | return False 147 | #Testing environment 148 | # Test Case 1 149 | warehouse1 = [[ 1, 2, 3], 150 | [ 0, 0, 0], 151 | [ 0, 0, 0]] 152 | dropzone1 = [2,0] 153 | todo1 = [2, 1] 154 | true_cost1 = 9 155 | # Test Case 2 156 | warehouse2 = [[ 1, 2, 3, 4], 157 | [ 0, 0, 0, 0], 158 | [ 5, 6, 7, 0], 159 | [ 'x', 0, 0, 8]] 160 | dropzone2 = [3,0] 161 | todo2 = [2, 5, 1] 162 | true_cost2 = 21 163 | 164 | # Test Case 3 165 | warehouse3 = [[ 1, 2, 3, 4, 5, 6, 7], 166 | [ 0, 0, 0, 0, 0, 0, 0], 167 | [ 8, 9, 10, 11, 0, 0, 0], 168 | [ 'x', 0, 0, 0, 0, 0, 12]] 169 | dropzone3 = [3,0] 170 | todo3 = [5, 10] 171 | true_cost3 = 18 172 | 173 | # Test Case 4 174 | warehouse4 = [[ 1, 17, 5, 18, 9, 19, 13], 175 | [ 2, 0, 6, 0, 10, 0, 14], 176 | [ 3, 0, 7, 0, 11, 0, 15], 177 | [ 4, 0, 8, 0, 12, 0, 16], 178 | [ 0, 0, 0, 0, 0, 0, 'x']] 179 | dropzone4 = [4,6] 180 | todo4 = [13, 11, 6, 17] 181 | true_cost4 = 41 182 | 183 | testing_suite = [[warehouse1, warehouse2, warehouse3, warehouse4], 184 | [dropzone1, dropzone2, dropzone3, dropzone4], 185 | [todo1, todo2, todo3, todo4], 186 | [true_cost1, true_cost2, true_cost3, true_cost4]] 187 | 188 | solution_check(testing_suite) #UNCOMMENT THIS LINE TO TEST YOUR CODE 189 | -------------------------------------------------------------------------------- /Project Runaway RObot/Adding Noise.py: -------------------------------------------------------------------------------- 1 | # ---------- 2 | # Part Two 3 | # 4 | # Now we'll make the scenario a bit more realistic. Now Traxbot's 5 | # sensor measurements are a bit noisy (though its motions are still 6 | # completetly noise-free and it still moves in an almost-circle). 7 | # You'll have to write a function that takes as input the next 8 | # noisy (x, y) sensor measurement and outputs the best guess 9 | # for the robot's next position. 10 | # 11 | # ---------- 12 | # YOUR JOB 13 | # 14 | # Complete the function estimate_next_pos. You will be considered 15 | # correct if your estimate is within 0.01 stepsizes of Traxbot's next 16 | # true position. 17 | # 18 | # ---------- 19 | # GRADING 20 | # 21 | # We will make repeated calls to your estimate_next_pos function. After 22 | # each call, we will compare your estimated position to the robot's true 23 | # position. As soon as you are within 0.01 stepsizes of the true position, 24 | # you will be marked correct and we will tell you how many steps it took 25 | # before your function successfully located the target bot. 26 | 27 | # These import steps give you access to libraries which you may (or may 28 | # not) want to use. 29 | from robot import * # Check the robot.py tab to see how this works. 30 | from math import * 31 | from matrix import * # Check the matrix.py tab to see how this works. 32 | import random 33 | import time 34 | 35 | def setup_kalman_filter(): 36 | """ 37 | Setup 7D Kalman Filter for this problem 38 | [x, y, v, a, theta, w, w'] 39 | """ 40 | # measurement function: reflect the fact that we observe x and y but not the two velocities and two accelerations 41 | H = matrix([[1., 0., 0., 0., 0.], 42 | [0., 1., 0., 0., 0.]]) 43 | # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal 44 | R = matrix([[0.1, 0.0], 45 | [0.0, 0.1]]) 46 | # 6d identity matrix 47 | I = matrix([[1.0, 0.0, 0.0, 0.0, 0.0], 48 | [0.0, 1.0, 0.0, 0.0, 0.0], 49 | [0.0, 0.0, 1.0, 0.0, 0.0], 50 | [0.0, 0.0, 0.0, 1.0, 0.0], 51 | [0.0, 0.0, 0.0, 0.0, 1.0]]) 52 | 53 | return H, R, I 54 | # This is the function you have to write. The argument 'measurement' is a 55 | # single (x, y) point. This function will have to be called multiple 56 | # times before you have enough information to accurately predict the 57 | # next position. The OTHER variable that your function returns will be 58 | # passed back to your function the next time it is called. You can use 59 | # this to keep track of important information over time. 60 | def estimate_next_pos(measurement, OTHER = None): 61 | """Estimate the next (x, y) position of the wandering Traxbot 62 | based on noisy (x, y) measurements.""" 63 | #stepsize 64 | dt = 1.0 65 | H, R, I = setup_kalman_filter() 66 | u = matrix([[0.], [0.], [0.], [0.], [0.]]) # external motion 67 | 68 | if OTHER == None: 69 | xy_estimate = measurement 70 | # initial uncertainty: 0 for positions x and y, 1000 for the two velocities, 1000 for the two acceleration 71 | P = matrix([[1000., 0., 0., 0., 0.], 72 | [0., 1000., 0., 0., 0.], 73 | [0., 0., 1000., 0., 0.], 74 | [0., 0., 0., 1000., 0.], 75 | [0., 0., 0., 0., 1000.]]) 76 | 77 | x = matrix([[0.], [0.], [0.], [0.], [0.]]) 78 | OTHER = [[measurement], P, x] 79 | return xy_estimate, OTHER 80 | elif len(OTHER[0])==1: 81 | OTHER[0].append(measurement) 82 | xy_estimate = measurement 83 | return xy_estimate, OTHER 84 | elif len(OTHER[0])==2: 85 | OTHER[0].append(measurement) 86 | parameters = [[0 for i in range(3)] for i in range(5)] 87 | parameters[0] = [OTHER[0][i][0] for i in range(len(OTHER[0]))] 88 | parameters[1] = [OTHER[0][i][1] for i in range(len(OTHER[0]))] 89 | parameters[2] = [0] + [sqrt((parameters[0][i+1]-parameters[0][i])**2+(parameters[1][i+1]-parameters[1][i])**2)/dt for i in range(len(parameters[0])-1)] 90 | parameters[3] = [0] + [atan2(parameters[1][i+1]-parameters[1][i], parameters[0][i+1]-parameters[0][i]) for i in range(len(parameters[0])-1)] 91 | parameters[4] = [0,0] + [(parameters[3][i]-parameters[3][i-1])/dt for i in range(2,len(parameters[0]))] 92 | 93 | v = parameters[2][-1] 94 | theta = parameters[3][-1] 95 | w = parameters[4][-1] 96 | 97 | OTHER[2] = matrix([[measurement[0]],[measurement[1]],[v],[theta],[w]]) 98 | x = matrix([[measurement[0]+v*dt*cos(theta+w*dt)], 99 | [measurement[1]+v*dt*sin(theta+w*dt)], 100 | [v], 101 | [theta+w*dt], 102 | [w]]) 103 | 104 | xy_estimate = (x.value[0][0], x.value[1][0]) 105 | return xy_estimate, OTHER 106 | 107 | OTHER[0].append(measurement) 108 | P = OTHER[1] 109 | # prediction 110 | x = OTHER[2] 111 | v = x.value[2][0] 112 | theta = x.value[3][0] 113 | w = x.value[4][0] 114 | 115 | # next state function: 7d 116 | F = matrix([[1, 0, dt*cos(theta+w*dt), -v*dt*sin(theta+w*dt), -v*dt**2*sin(theta+w*dt)], 117 | [0, 1, dt*sin(theta+w*dt), v*dt*cos(theta+w*dt), v*dt**2*cos(theta+w*dt)], 118 | [0, 0, 1, 0, 0], 119 | [0, 0, 0, 1, dt], 120 | [0, 0, 0, 0, 1]]) 121 | X1 = matrix([[x.value[0][0]+x.value[2][0]*dt*cos(x.value[3][0]+x.value[4][0]*dt)], 122 | [x.value[1][0]+x.value[2][0]*dt*sin(x.value[3][0]+x.value[4][0]*dt)], 123 | [x.value[2][0]], 124 | [x.value[3][0]+x.value[4][0]*dt], 125 | [x.value[4][0]]]) 126 | x = X1 127 | P = F * P * F.transpose() 128 | 129 | 130 | # measurement update 131 | Z = matrix([[measurement[0], measurement[1]]]) 132 | y = Z.transpose() - (H * x) 133 | S = H * P * H.transpose() + R 134 | K = P * H.transpose() * S.inverse() 135 | x = x + (K * y) 136 | P = (I - (K * H)) * P 137 | 138 | X = matrix([[x.value[0][0]+x.value[2][0]*dt*cos(x.value[3][0]+x.value[4][0]*dt)], 139 | [x.value[1][0]+x.value[2][0]*dt*sin(x.value[3][0]+x.value[4][0]*dt)], 140 | [x.value[2][0]], 141 | [x.value[3][0]+x.value[4][0]*dt], 142 | [x.value[4][0]]]) 143 | 144 | OTHER[1] = P 145 | OTHER[2] = x 146 | xy_estimate = (X.value[0][0], X.value[1][0]) 147 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 148 | # in this order for grading purposes. 149 | #print xy_estimate, measurement 150 | return xy_estimate, OTHER 151 | 152 | # A helper function you may find useful. 153 | def distance_between(point1, point2): 154 | """Computes distance between point1 and point2. Points are (x, y) pairs.""" 155 | x1, y1 = point1 156 | x2, y2 = point2 157 | return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) 158 | 159 | # This is here to give you a sense for how we will be running and grading 160 | # your code. Note that the OTHER variable allows you to store any 161 | # information that you want. 162 | def demo_grading(estimate_next_pos_fcn, target_bot, OTHER = None): 163 | localized = False 164 | distance_tolerance = 0.01 * target_bot.distance 165 | ctr = 0 166 | # if you haven't localized the target bot, make a guess about the next 167 | # position, then we move the bot and compare your guess to the true 168 | # next position. When you are close enough, we stop checking. 169 | while not localized and ctr <= 100: 170 | ctr += 1 171 | measurement = target_bot.sense() 172 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 173 | target_bot.move_in_circle() 174 | true_position = (target_bot.x, target_bot.y) 175 | error = distance_between(position_guess, true_position) 176 | print (error) 177 | if error <= distance_tolerance: 178 | print "You got it right! It took you ", ctr, " steps to localize." 179 | localized = True 180 | if ctr == 100: 181 | print "Sorry, it took you too many steps to localize the target." 182 | return localized 183 | 184 | def demo_grading2(estimate_next_pos_fcn, target_bot, OTHER = None): 185 | localized = False 186 | distance_tolerance = 0.01 * target_bot.distance 187 | ctr = 0 188 | # if you haven't localized the target bot, make a guess about the next 189 | # position, then we move the bot and compare your guess to the true 190 | # next position. When you are close enough, we stop checking. 191 | #For Visualization 192 | import turtle #You need to run this locally to use the turtle module 193 | window = turtle.Screen() 194 | window.bgcolor('white') 195 | size_multiplier= 25.0 #change Size of animation 196 | broken_robot = turtle.Turtle() 197 | broken_robot.shape('turtle') 198 | broken_robot.color('green') 199 | broken_robot.resizemode('user') 200 | broken_robot.shapesize(0.1, 0.1, 0.1) 201 | measured_broken_robot = turtle.Turtle() 202 | measured_broken_robot.shape('circle') 203 | measured_broken_robot.color('red') 204 | measured_broken_robot.resizemode('user') 205 | measured_broken_robot.shapesize(0.1, 0.1, 0.1) 206 | prediction = turtle.Turtle() 207 | prediction.shape('arrow') 208 | prediction.color('blue') 209 | prediction.resizemode('user') 210 | prediction.shapesize(0.1, 0.1, 0.1) 211 | prediction.penup() 212 | broken_robot.penup() 213 | measured_broken_robot.penup() 214 | 215 | #End of Visualization 216 | while not localized and ctr <= 1000: 217 | ctr += 1 218 | measurement = target_bot.sense() 219 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 220 | 221 | target_bot.move_in_circle() 222 | true_position = (target_bot.x, target_bot.y) 223 | error = distance_between(position_guess, true_position) 224 | if error <= distance_tolerance: 225 | print("You got it right! It took you ", ctr, " steps to localize.") 226 | localized = True 227 | if ctr == 1000: 228 | print("Sorry, it took you too many steps to localize the target.") 229 | #More Visualization 230 | measured_broken_robot.setheading(target_bot.heading*180/pi) 231 | measured_broken_robot.goto(measurement[0]*size_multiplier, measurement[1]*size_multiplier-200) 232 | measured_broken_robot.stamp() 233 | broken_robot.setheading(target_bot.heading*180/pi) 234 | broken_robot.goto(target_bot.x*size_multiplier, target_bot.y*size_multiplier-200) 235 | broken_robot.stamp() 236 | prediction.setheading(target_bot.heading*180/pi) 237 | prediction.goto(position_guess[0]*size_multiplier, position_guess[1]*size_multiplier-200) 238 | prediction.stamp() 239 | #End of Visualization 240 | time.sleep(5) 241 | return localized 242 | 243 | # This is a demo for what a strategy could look like. This one isn't very good. 244 | def naive_next_pos(measurement, OTHER = None): 245 | """This strategy records the first reported position of the target and 246 | assumes that eventually the target bot will eventually return to that 247 | position, so it always guesses that the first position will be the next.""" 248 | if not OTHER: # this is the first measurement 249 | OTHER = measurement 250 | xy_estimate = OTHER 251 | return xy_estimate, OTHER 252 | 253 | # This is how we create a target bot. Check the robot.py file to understand 254 | # How the robot class behaves. 255 | test_target = robot(2.1, 4.3, 0.5, 2*pi / 34.0, 1.5) 256 | measurement_noise = 0.05 * test_target.distance 257 | test_target.set_noise(0.0, 0.0, measurement_noise) 258 | 259 | demo_grading(estimate_next_pos, test_target) 260 | -------------------------------------------------------------------------------- /Project Runaway RObot/Chasing with a Plan.py: -------------------------------------------------------------------------------- 1 | # ---------- 2 | # Part Four 3 | # 4 | # Again, you'll track down and recover the runaway Traxbot. 5 | # But this time, your speed will be about the same as the runaway bot. 6 | # This may require more careful planning than you used last time. 7 | # 8 | # ---------- 9 | # YOUR JOB 10 | # 11 | # Complete the next_move function, similar to how you did last time. 12 | # 13 | # ---------- 14 | # GRADING 15 | # 16 | # Same as part 3. Again, try to catch the target in as few steps as possible. 17 | 18 | from robot import * 19 | from math import * 20 | from matrix import * 21 | from kalman import * 22 | import random 23 | import time 24 | 25 | def x_state(measure, dt=1.0, n=5): 26 | nn = len(measure) 27 | parameters = [[0 for j in range(nn)] for i in range(n)] 28 | parameters[0] = [measure[i][0] for i in range(nn)] 29 | parameters[1] = [measure[i][1] for i in range(nn)] 30 | parameters[2] = [0] + [sqrt((parameters[0][i+1]-parameters[0][i])**2+(parameters[1][i+1]-parameters[1][i])**2)/dt for i in range(len(parameters[0])-1)] 31 | parameters[3] = [0] + [atan2(parameters[1][i+1]-parameters[1][i], parameters[0][i+1]-parameters[0][i]) for i in range(len(parameters[0])-1)] 32 | parameters[4] = [0,0] + [(parameters[3][i]-parameters[3][i-1])/dt for i in range(2,len(parameters[0]))] 33 | 34 | return matrix([[parameters[0][-1]], 35 | [parameters[1][-1]], 36 | [parameters[2][-1]], 37 | [parameters[3][-1]], 38 | [parameters[4][-1]]]) 39 | 40 | def predict_state(x, dt=1.0): 41 | v = x.value[2][0] 42 | theta = x.value[3][0] 43 | w = x.value[4][0] 44 | 45 | X = matrix([[x.value[0][0]+v*dt*cos(theta+w*dt)], 46 | [x.value[1][0]+v*dt*sin(theta+w*dt)], 47 | [v], 48 | [theta+w*dt], 49 | [w]]) 50 | 51 | return X 52 | 53 | def F_mat(x, dt=1.0): 54 | v = x.value[2][0] 55 | theta = x.value[3][0] 56 | w = x.value[4][0] 57 | 58 | F = matrix([[1, 0, dt*cos(theta+w*dt), -v*dt*sin(theta+w*dt), -v*dt**2*sin(theta+w*dt)], 59 | [0, 1, dt*sin(theta+w*dt), v*dt*cos(theta+w*dt), v*dt**2*cos(theta+w*dt)], 60 | [0, 0, 1, 0, 0], 61 | [0, 0, 0, 1, dt], 62 | [0, 0, 0, 0, 1]]) 63 | return F 64 | 65 | def next_move(hunter_position, hunter_heading, target_measurement, max_distance, OTHER = None): 66 | # This function will be called after each time the target moves. 67 | dt =1.0 68 | 69 | if OTHER ==None : 70 | target_position = target_measurement 71 | turning = get_heading(hunter_position, target_position)-hunter_heading 72 | distance = max_distance 73 | OTHER = [target_measurement] 74 | return turning, distance, OTHER 75 | elif len(OTHER) == 1: 76 | target_position = (2*target_measurement[0]-OTHER[0][0], 2*target_measurement[1]-OTHER[0][1]) 77 | turning = get_heading(hunter_position, target_position)-hunter_heading 78 | distance = max_distance 79 | OTHER.append(target_measurement) 80 | return turning, distance, OTHER 81 | elif len(OTHER) == 2: 82 | OTHER.append(target_measurement) 83 | x = x_state(OTHER) 84 | OTHER = [[e for e in OTHER], x, 0] 85 | x = predict_state(x) 86 | target_position = (x.value[0][0], x.value[1][0]) 87 | turning = get_heading(hunter_position, target_position)-hunter_heading 88 | distance = max_distance 89 | return turning, distance, OTHER 90 | kalman1 = kalman(matrix([[0] for i in range(5)])) 91 | x = OTHER[1] 92 | P = matrix([[1000.0 if i==j else 0.0 for j in range(len(x.value))] for i in range(len(x.value))]) if OTHER[2]==0 else OTHER[2] 93 | z = target_measurement 94 | F = F_mat(x) 95 | x, P = kalman1.extended_kalman_filter(predict_state(OTHER[1]), z, F, P) 96 | OTHER = [OTHER[0]+[target_measurement], x, P] 97 | x = predict_state(x) 98 | target_position = (x.value[0][0], x.value[1][0]) 99 | if distance_between(hunter_position, target_position)>max_distance: 100 | x = predict_state(x) 101 | target_position = (x.value[0][0], x.value[1][0]) 102 | turning = angle_trunc(get_heading(hunter_position, target_position)-hunter_heading) 103 | distance = max_distance 104 | return turning, distance, OTHER 105 | turning = angle_trunc(get_heading(hunter_position, target_position)-hunter_heading) 106 | distance = distance_between(hunter_position, target_position) 107 | # The OTHER variable is a place for you to store any historical information about 108 | # the progress of the hunt (or maybe some localization information). Your return format 109 | # must be as follows in order to be graded properly. 110 | return turning, distance, OTHER 111 | 112 | def distance_between(point1, point2): 113 | """Computes distance between point1 and point2. Points are (x, y) pairs.""" 114 | x1, y1 = point1 115 | x2, y2 = point2 116 | return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) 117 | 118 | def demo_grading(hunter_bot, target_bot, next_move_fcn, OTHER = None): 119 | """Returns True if your next_move_fcn successfully guides the hunter_bot 120 | to the target_bot. This function is here to help you understand how we 121 | will grade your submission.""" 122 | max_distance = 0.98 * target_bot.distance # 0.98 is an example. It will change. 123 | separation_tolerance = 0.02 * target_bot.distance # hunter must be within 0.02 step size to catch target 124 | caught = False 125 | ctr = 0 126 | 127 | # We will use your next_move_fcn until we catch the target or time expires. 128 | while not caught and ctr < 100: 129 | 130 | # Check to see if the hunter has caught the target. 131 | hunter_position = (hunter_bot.x, hunter_bot.y) 132 | target_position = (target_bot.x, target_bot.y) 133 | separation = distance_between(hunter_position, target_position) 134 | print (separation) 135 | if separation < separation_tolerance: 136 | print "You got it right! It took you ", ctr, " steps to catch the target." 137 | caught = True 138 | 139 | # The target broadcasts its noisy measurement 140 | target_measurement = target_bot.sense() 141 | 142 | # This is where YOUR function will be called. 143 | turning, distance, OTHER = next_move_fcn(hunter_position, hunter_bot.heading, target_measurement, max_distance, OTHER) 144 | 145 | # Don't try to move faster than allowed! 146 | if distance > max_distance: 147 | distance = max_distance 148 | 149 | # We move the hunter according to your instructions 150 | hunter_bot.move(turning, distance) 151 | 152 | # The target continues its (nearly) circular motion. 153 | target_bot.move_in_circle() 154 | 155 | ctr += 1 156 | if ctr >= 100: 157 | print "It took too many steps to catch the target." 158 | return caught 159 | 160 | def demo_grading2(hunter_bot, target_bot, next_move_fcn, OTHER = None): 161 | """Returns True if your next_move_fcn successfully guides the hunter_bot 162 | to the target_bot. This function is here to help you understand how we 163 | will grade your submission.""" 164 | max_distance = 0.98 * target_bot.distance # 0.98 is an example. It will change. 165 | separation_tolerance = 0.02 * target_bot.distance # hunter must be within 0.02 step size to catch target 166 | caught = False 167 | ctr = 0 168 | #For Visualization 169 | import turtle 170 | window = turtle.Screen() 171 | window.bgcolor('white') 172 | chaser_robot = turtle.Turtle() 173 | chaser_robot.shape('arrow') 174 | chaser_robot.color('blue') 175 | chaser_robot.resizemode('user') 176 | chaser_robot.shapesize(0.3, 0.3, 0.3) 177 | broken_robot = turtle.Turtle() 178 | broken_robot.shape('turtle') 179 | broken_robot.color('green') 180 | broken_robot.resizemode('user') 181 | broken_robot.shapesize(0.3, 0.3, 0.3) 182 | size_multiplier = 15.0 #change size of animation 183 | chaser_robot.hideturtle() 184 | chaser_robot.penup() 185 | chaser_robot.goto(hunter_bot.x*size_multiplier, hunter_bot.y*size_multiplier-100) 186 | chaser_robot.showturtle() 187 | broken_robot.hideturtle() 188 | broken_robot.penup() 189 | broken_robot.goto(target_bot.x*size_multiplier, target_bot.y*size_multiplier-100) 190 | broken_robot.showturtle() 191 | measuredbroken_robot = turtle.Turtle() 192 | measuredbroken_robot.shape('circle') 193 | measuredbroken_robot.color('red') 194 | measuredbroken_robot.penup() 195 | measuredbroken_robot.resizemode('user') 196 | measuredbroken_robot.shapesize(0.1, 0.1, 0.1) 197 | broken_robot.pendown() 198 | chaser_robot.pendown() 199 | #End of Visualization 200 | # We will use your next_move_fcn until we catch the target or time expires. 201 | while not caught and ctr < 100: 202 | # Check to see if the hunter has caught the target. 203 | hunter_position = (hunter_bot.x, hunter_bot.y) 204 | target_position = (target_bot.x, target_bot.y) 205 | separation = distance_between(hunter_position, target_position) 206 | if separation < separation_tolerance: 207 | print "You got it right! It took you ", ctr, " steps to catch the target." 208 | caught = True 209 | 210 | # The target broadcasts its noisy measurement 211 | target_measurement = target_bot.sense() 212 | 213 | # This is where YOUR function will be called. 214 | turning, distance, OTHER = next_move_fcn(hunter_position, hunter_bot.heading, target_measurement, max_distance, OTHER) 215 | 216 | # Don't try to move faster than allowed! 217 | if distance > max_distance: 218 | distance = max_distance 219 | 220 | # We move the hunter according to your instructions 221 | hunter_bot.move(turning, distance) 222 | 223 | # The target continues its (nearly) circular motion. 224 | target_bot.move_in_circle() 225 | #Visualize it 226 | measuredbroken_robot.setheading(target_bot.heading*180/pi) 227 | measuredbroken_robot.goto(target_measurement[0]*size_multiplier, target_measurement[1]*size_multiplier-100) 228 | measuredbroken_robot.stamp() 229 | broken_robot.setheading(target_bot.heading*180/pi) 230 | broken_robot.goto(target_bot.x*size_multiplier, target_bot.y*size_multiplier-100) 231 | chaser_robot.setheading(hunter_bot.heading*180/pi) 232 | chaser_robot.goto(hunter_bot.x*size_multiplier, hunter_bot.y*size_multiplier-100) 233 | #End of visualization 234 | #time.sleep(0.5) 235 | ctr += 1 236 | if ctr >= 100: 237 | print "It took too many steps to catch the target." 238 | time.sleep(1) 239 | return caught 240 | 241 | def angle_trunc(a): 242 | """This maps all angles to a domain of [-pi, pi]""" 243 | while a < 0.0: 244 | a += pi * 2 245 | return ((a + pi) % (pi * 2)) - pi 246 | 247 | def get_heading(hunter_position, target_position): 248 | """Returns the angle, in radians, between the target and hunter positions""" 249 | hunter_x, hunter_y = hunter_position 250 | target_x, target_y = target_position 251 | heading = atan2(target_y - hunter_y, target_x - hunter_x) 252 | heading = angle_trunc(heading) 253 | return heading 254 | 255 | def naive_next_move(hunter_position, hunter_heading, target_measurement, max_distance, OTHER): 256 | """This strategy always tries to steer the hunter directly towards where the target last 257 | said it was and then moves forwards at full speed. This strategy also keeps track of all 258 | the target measurements, hunter positions, and hunter headings over time, but it doesn't 259 | do anything with that information.""" 260 | if not OTHER: # first time calling this function, set up my OTHER variables. 261 | measurements = [target_measurement] 262 | hunter_positions = [hunter_position] 263 | hunter_headings = [hunter_heading] 264 | OTHER = (measurements, hunter_positions, hunter_headings) # now I can keep track of history 265 | else: # not the first time, update my history 266 | OTHER[0].append(target_measurement) 267 | OTHER[1].append(hunter_position) 268 | OTHER[2].append(hunter_heading) 269 | measurements, hunter_positions, hunter_headings = OTHER # now I can always refer to these variables 270 | 271 | heading_to_target = get_heading(hunter_position, target_measurement) 272 | heading_difference = heading_to_target - hunter_heading 273 | turning = heading_difference # turn towards the target 274 | distance = max_distance # full speed ahead! 275 | return turning, distance, OTHER 276 | 277 | target = robot(0.0, 10.0, 0.0, 2*pi / 30, 1.5) 278 | measurement_noise = 0.02*target.distance 279 | target.set_noise(0.0, 0.0, measurement_noise) 280 | 281 | hunter = robot(-10.0, -10.0, 0.0) 282 | 283 | print demo_grading2(hunter, target, next_move) 284 | 285 | 286 | -------------------------------------------------------------------------------- /Project Runaway RObot/Noiseless Prediction.py: -------------------------------------------------------------------------------- 1 | # ---------- 2 | # Background 3 | # 4 | # A robotics company named Trax has created a line of small self-driving robots 5 | # designed to autonomously traverse desert environments in search of undiscovered 6 | # water deposits. 7 | # 8 | # A Traxbot looks like a small tank. Each one is about half a meter long and drives 9 | # on two continuous metal tracks. In order to maneuver itself, a Traxbot can do one 10 | # of two things: it can drive in a straight line or it can turn. So to make a 11 | # right turn, A Traxbot will drive forward, stop, turn 90 degrees, then continue 12 | # driving straight. 13 | # 14 | # This series of questions involves the recovery of a rogue Traxbot. This bot has 15 | # gotten lost somewhere in the desert and is now stuck driving in an almost-circle: it has 16 | # been repeatedly driving forward by some step size, stopping, turning a certain 17 | # amount, and repeating this process... Luckily, the Traxbot is still sending all 18 | # of its sensor data back to headquarters. 19 | # 20 | # In this project, we will start with a simple version of this problem and 21 | # gradually add complexity. By the end, you will have a fully articulated 22 | # plan for recovering the lost Traxbot. 23 | # 24 | # ---------- 25 | # Part One 26 | # 27 | # Let's start by thinking about circular motion (well, really it's polygon motion 28 | # that is close to circular motion). Assume that Traxbot lives on 29 | # an (x, y) coordinate plane and (for now) is sending you PERFECTLY ACCURATE sensor 30 | # measurements. 31 | # 32 | # With a few measurements you should be able to figure out the step size and the 33 | # turning angle that Traxbot is moving with. 34 | # With these two pieces of information, you should be able to 35 | # write a function that can predict Traxbot's next location. 36 | # 37 | # You can use the robot class that is already written to make your life easier. 38 | # You should re-familiarize yourself with this class, since some of the details 39 | # have changed. 40 | # 41 | # ---------- 42 | # YOUR JOB 43 | # 44 | # Complete the estimate_next_pos function. You will probably want to use 45 | # the OTHER variable to keep track of information about the runaway robot. 46 | # 47 | # ---------- 48 | # GRADING 49 | # 50 | # We will make repeated calls to your estimate_next_pos function. After 51 | # each call, we will compare your estimated position to the robot's true 52 | # position. As soon as you are within 0.01 stepsizes of the true position, 53 | # you will be marked correct and we will tell you how many steps it took 54 | # before your function successfully located the target bot. 55 | 56 | # These import steps give you access to libraries which you may (or may 57 | # not) want to use. 58 | from robot import * 59 | from math import * 60 | from matrix import * 61 | import random 62 | 63 | 64 | # This is the function you have to write. The argument 'measurement' is a 65 | # single (x, y) point. This function will have to be called multiple 66 | # times before you have enough information to accurately predict the 67 | # next position. The OTHER variable that your function returns will be 68 | # passed back to your function the next time it is called. You can use 69 | # this to keep track of important information over time. 70 | def estimate_next_pos(measurement, OTHER = None): 71 | """Estimate the next (x, y) position of the wandering Traxbot 72 | based on noisy (x, y) measurements.""" 73 | if OTHER == None: #one point 74 | xy_estimate = measurement 75 | OTHER = [measurement] 76 | elif len(OTHER) == 1: #two points 77 | OTHER.append(measurement) 78 | xy_estimate = (2*OTHER[-1][0]-OTHER[-2][0], 2*OTHER[-1][1]-OTHER[-2][1]) 79 | else: 80 | n = 3 81 | OTHER.append(measurement) 82 | step = sum([sqrt((OTHER[i+1][1]-OTHER[i][1])**2+(OTHER[i+1][0]-OTHER[i][0])**2) for i in range(len(OTHER)-1)])/(len(OTHER)-1.0) 83 | #a*x**2 + b*x + c 84 | if len(OTHER) > n: 85 | OTHER = OTHER[-n:] 86 | alpha = matrix([[len(OTHER),sum([e[0] for e in OTHER]),sum([e[0]**2 for e in OTHER])], 87 | [sum([e[0] for e in OTHER]),sum([e[0]**2 for e in OTHER]),sum([e[0]**3 for e in OTHER])], 88 | [sum([e[0]**2 for e in OTHER]),sum([e[0]**3 for e in OTHER]),sum([e[0]**4 for e in OTHER])]]) 89 | beta = matrix([[sum([e[1] for e in OTHER])],[sum([e[1]*e[0] for e in OTHER])],[sum([e[1]*e[0]**2 for e in OTHER])]]) 90 | solotion = alpha.inverse()*beta 91 | 92 | if abs(2*solotion.value[2][0]*OTHER[-1][0]+solotion.value[1][0]) <=1: #don`t exceed 45 angle 93 | flag_x = 1 if OTHER[-1][0]-OTHER[-2][0]>0 else -1 94 | x_change = 0.75*(OTHER[-1][0]-OTHER[-2][0]) 95 | x_estimate = OTHER[-1][0] + x_change*flag_x 96 | y_estimate = solotion.value[2][0]*x_estimate**2 + solotion.value[1][0]*x_estimate + solotion.value[0][0] 97 | while abs(sqrt((x_estimate- OTHER[-1][0])**2+(y_estimate-OTHER[-1][1])**2)-step) > 0.001: 98 | if abs(sqrt((x_estimate- OTHER[-1][0])**2+(y_estimate-OTHER[-1][1])**2)) > step: 99 | x_estimate -= 0.5*x_change*flag_x 100 | else: 101 | x_estimate += 0.5*x_change*flag_x 102 | x_change *= 0.5 103 | y_estimate = solotion.value[2][0]*x_estimate**2 + solotion.value[1][0]*x_estimate + solotion.value[0][0] 104 | else: 105 | #a*y**2 + b*y + c 106 | alpha = matrix([[len(OTHER),sum([e[1] for e in OTHER]),sum([e[1]**2 for e in OTHER])], 107 | [sum([e[1] for e in OTHER]),sum([e[1]**2 for e in OTHER]),sum([e[1]**3 for e in OTHER])], 108 | [sum([e[1]**2 for e in OTHER]),sum([e[1]**3 for e in OTHER]),sum([e[1]**4 for e in OTHER])]]) 109 | beta = matrix([[sum([e[0] for e in OTHER])],[sum([e[0]*e[1] for e in OTHER])],[sum([e[0]*e[1]**2 for e in OTHER])]]) 110 | solotion = alpha.inverse()*beta 111 | flag_y = 1 if OTHER[-1][1]-OTHER[-2][1]>0 else -1 112 | y_change = 0.75*(OTHER[-1][1]-OTHER[-2][1]) 113 | y_estimate = OTHER[-1][1] + y_change*flag_y 114 | x_estimate = solotion.value[2][0]*y_estimate**2 + solotion.value[1][0]*y_estimate + solotion.value[0][0] 115 | while abs(sqrt((x_estimate- OTHER[-1][0])**2+(y_estimate-OTHER[-1][1])**2)-step) > 0.001: 116 | if abs(sqrt((x_estimate- OTHER[-1][0])**2+(y_estimate-OTHER[-1][1])**2)) > step: 117 | y_estimate -= 0.5*y_change*flag_y 118 | else: 119 | y_estimate += 0.5*y_change*flag_y 120 | y_change *= 0.5 121 | x_estimate = solotion.value[2][0]*y_estimate**2 + solotion.value[1][0]*y_estimate + solotion.value[0][0] 122 | xy_estimate = (x_estimate, y_estimate) 123 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 124 | # in this order for grading purposes. 125 | print xy_estimate, OTHER 126 | return xy_estimate, OTHER 127 | # A helper function you may find useful. 128 | def distance_between(point1, point2): 129 | """Computes distance between point1 and point2. Points are (x, y) pairs.""" 130 | x1, y1 = point1 131 | x2, y2 = point2 132 | return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) 133 | 134 | # This is here to give you a sense for how we will be running and grading 135 | # your code. Note that the OTHER variable allows you to store any 136 | # information that you want. 137 | def demo_grading(estimate_next_pos_fcn, target_bot, OTHER = None): 138 | localized = False 139 | distance_tolerance = 0.01 * target_bot.distance 140 | ctr = 0 141 | # if you haven't localized the target bot, make a guess about the next 142 | # position, then we move the bot and compare your guess to the true 143 | # next position. When you are close enough, we stop checking. 144 | while not localized and ctr <= 10: 145 | ctr += 1 146 | measurement = target_bot.sense() 147 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 148 | target_bot.move_in_circle() 149 | true_position = (target_bot.x, target_bot.y) 150 | error = distance_between(position_guess, true_position) 151 | if error <= distance_tolerance: 152 | print "You got it right! It took you ", ctr, " steps to localize." 153 | localized = True 154 | if ctr == 10: 155 | print "Sorry, it took you too many steps to localize the target." 156 | return localized 157 | 158 | # This is a demo for what a strategy could look like. This one isn't very good. 159 | def naive_next_pos(measurement, OTHER = None): 160 | """This strategy records the first reported position of the target and 161 | assumes that eventually the target bot will eventually return to that 162 | position, so it always guesses that the first position will be the next.""" 163 | if not OTHER: # this is the first measurement 164 | OTHER = measurement 165 | xy_estimate = OTHER 166 | return xy_estimate, OTHER 167 | 168 | # This is how we create a target bot. Check the robot.py file to understand 169 | # How the robot class behaves. 170 | test_target = robot(2.1, 4.3, 0.5, 2*pi / 34.0, 1.5) 171 | test_target.set_noise(0.0, 0.0, 0.0) 172 | 173 | demo_grading(estimate_next_pos, test_target) 174 | -------------------------------------------------------------------------------- /Project Runaway RObot/The Chase Begins.py: -------------------------------------------------------------------------------- 1 | # ---------- 2 | # Part Three 3 | # 4 | # Now you'll actually track down and recover the runaway Traxbot. 5 | # In this step, your speed will be about twice as fast the runaway bot, 6 | # which means that your bot's distance parameter will be about twice that 7 | # of the runaway. You can move less than this parameter if you'd 8 | # like to slow down your bot near the end of the chase. 9 | # 10 | # ---------- 11 | # YOUR JOB 12 | # 13 | # Complete the next_move function. This function will give you access to 14 | # the position and heading of your bot (the hunter); the most recent 15 | # measurement received from the runaway bot (the target), the max distance 16 | # your bot can move in a given timestep, and another variable, called 17 | # OTHER, which you can use to keep track of information. 18 | # 19 | # Your function will return the amount you want your bot to turn, the 20 | # distance you want your bot to move, and the OTHER variable, with any 21 | # information you want to keep track of. 22 | # 23 | # ---------- 24 | # GRADING 25 | # 26 | # We will make repeated calls to your next_move function. After 27 | # each call, we will move the hunter bot according to your instructions 28 | # and compare its position to the target bot's true position 29 | # As soon as the hunter is within 0.01 stepsizes of the target, 30 | # you will be marked correct and we will tell you how many steps it took 31 | # before your function successfully located the target bot. 32 | # 33 | # As an added challenge, try to get to the target bot as quickly as 34 | # possible. 35 | 36 | from robot import * 37 | from math import * 38 | from matrix import * 39 | from kalman import * 40 | import random 41 | 42 | def x_state(measure, dt=1.0, n=5): 43 | nn = len(measure) 44 | parameters = [[0 for j in range(nn)] for i in range(n)] 45 | parameters[0] = [measure[i][0] for i in range(nn)] 46 | parameters[1] = [measure[i][1] for i in range(nn)] 47 | parameters[2] = [0] + [sqrt((parameters[0][i+1]-parameters[0][i])**2+(parameters[1][i+1]-parameters[1][i])**2)/dt for i in range(len(parameters[0])-1)] 48 | parameters[3] = [0] + [atan2(parameters[1][i+1]-parameters[1][i], parameters[0][i+1]-parameters[0][i]) for i in range(len(parameters[0])-1)] 49 | parameters[4] = [0,0] + [(parameters[3][i]-parameters[3][i-1])/dt for i in range(2,len(parameters[0]))] 50 | 51 | return matrix([[parameters[0][-1]], 52 | [parameters[1][-1]], 53 | [parameters[2][-1]], 54 | [parameters[3][-1]], 55 | [parameters[4][-1]]]) 56 | 57 | def predict_state(x, dt=1.0): 58 | v = x.value[2][0] 59 | theta = x.value[3][0] 60 | w = x.value[4][0] 61 | 62 | X = matrix([[x.value[0][0]+v*dt*cos(theta+w*dt)], 63 | [x.value[1][0]+v*dt*sin(theta+w*dt)], 64 | [v], 65 | [theta+w*dt], 66 | [w]]) 67 | 68 | return X 69 | 70 | def F_mat(x, dt=1.0): 71 | v = x.value[2][0] 72 | theta = x.value[3][0] 73 | w = x.value[4][0] 74 | 75 | F = matrix([[1, 0, dt*cos(theta+w*dt), -v*dt*sin(theta+w*dt), -v*dt**2*sin(theta+w*dt)], 76 | [0, 1, dt*sin(theta+w*dt), v*dt*cos(theta+w*dt), v*dt**2*cos(theta+w*dt)], 77 | [0, 0, 1, 0, 0], 78 | [0, 0, 0, 1, dt], 79 | [0, 0, 0, 0, 1]]) 80 | return F 81 | 82 | def next_move(hunter_position, hunter_heading, target_measurement, max_distance, OTHER = None): 83 | # This function will be called after each time the target moves. 84 | dt =1.0 85 | 86 | if OTHER ==None : 87 | target_position = target_measurement 88 | turning = get_heading(hunter_position, target_position)-hunter_heading 89 | distance = max_distance 90 | OTHER = [target_measurement] 91 | return turning, distance, OTHER 92 | elif len(OTHER) == 1: 93 | target_position = (2*target_measurement[0]-OTHER[0][0], 2*target_measurement[1]-OTHER[0][1]) 94 | turning = get_heading(hunter_position, target_position)-hunter_heading 95 | distance = max_distance 96 | OTHER.append(target_measurement) 97 | return turning, distance, OTHER 98 | elif len(OTHER) == 2: 99 | OTHER.append(target_measurement) 100 | print (OTHER) 101 | x = x_state(OTHER) 102 | OTHER = [[e for e in OTHER], x, 0] 103 | x = predict_state(x) 104 | target_position = (x.value[0][0], x.value[1][0]) 105 | turning = get_heading(hunter_position, target_position)-hunter_heading 106 | distance = max_distance 107 | return turning, distance, OTHER 108 | kalman1 = kalman(matrix([[0] for i in range(5)])) 109 | x = OTHER[1] 110 | P = matrix([[1000.0 if i==j else 0.0 for j in range(len(x.value))] for i in range(len(x.value))]) if OTHER[2]==0 else OTHER[2] 111 | z = target_measurement 112 | F = F_mat(x) 113 | x, P = kalman1.extended_kalman_filter(predict_state(OTHER[1]), z, F, P) 114 | OTHER = [OTHER[0]+[target_measurement], x, P] 115 | x = predict_state(x) 116 | target_position = (x.value[0][0], x.value[1][0]) 117 | turning = get_heading(hunter_position, target_position)-hunter_heading 118 | distance = max_distance if distance_between(hunter_position, target_position)>max_distance else distance_between(hunter_position, target_position) 119 | # The OTHER variable is a place for you to store any historical information about 120 | # the progress of the hunt (or maybe some localization information). Your return format 121 | # must be as follows in order to be graded properly. 122 | return turning, distance, OTHER 123 | 124 | def distance_between(point1, point2): 125 | """Computes distance between point1 and point2. Points are (x, y) pairs.""" 126 | x1, y1 = point1 127 | x2, y2 = point2 128 | return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) 129 | 130 | def demo_grading(hunter_bot, target_bot, next_move_fcn, OTHER = None): 131 | """Returns True if your next_move_fcn successfully guides the hunter_bot 132 | to the target_bot. This function is here to help you understand how we 133 | will grade your submission.""" 134 | max_distance = 1.94 * target_bot.distance # 1.94 is an example. It will change. 135 | separation_tolerance = 0.02 * target_bot.distance # hunter must be within 0.02 step size to catch target 136 | caught = False 137 | ctr = 0 138 | 139 | # We will use your next_move_fcn until we catch the target or time expires. 140 | while not caught and ctr < 1000: 141 | 142 | # Check to see if the hunter has caught the target. 143 | hunter_position = (hunter_bot.x, hunter_bot.y) 144 | target_position = (target_bot.x, target_bot.y) 145 | separation = distance_between(hunter_position, target_position) 146 | if separation < separation_tolerance: 147 | print "You got it right! It took you ", ctr, " steps to catch the target." 148 | caught = True 149 | 150 | # The target broadcasts its noisy measurement 151 | target_measurement = target_bot.sense() 152 | 153 | # This is where YOUR function will be called. 154 | turning, distance, OTHER = next_move_fcn(hunter_position, hunter_bot.heading, target_measurement, max_distance, OTHER) 155 | 156 | # Don't try to move faster than allowed! 157 | if distance > max_distance: 158 | distance = max_distance 159 | 160 | # We move the hunter according to your instructions 161 | hunter_bot.move(turning, distance) 162 | 163 | # The target continues its (nearly) circular motion. 164 | target_bot.move_in_circle() 165 | 166 | ctr += 1 167 | if ctr >= 1000: 168 | print "It took too many steps to catch the target." 169 | return caught 170 | 171 | def angle_trunc(a): 172 | """This maps all angles to a domain of [-pi, pi]""" 173 | while a < 0.0: 174 | a += pi * 2 175 | return ((a + pi) % (pi * 2)) - pi 176 | 177 | def get_heading(hunter_position, target_position): 178 | """Returns the angle, in radians, between the target and hunter positions""" 179 | hunter_x, hunter_y = hunter_position 180 | target_x, target_y = target_position 181 | heading = atan2(target_y - hunter_y, target_x - hunter_x) 182 | heading = angle_trunc(heading) 183 | return heading 184 | 185 | def naive_next_move(hunter_position, hunter_heading, target_measurement, max_distance, OTHER): 186 | """This strategy always tries to steer the hunter directly towards where the target last 187 | said it was and then moves forwards at full speed. This strategy also keeps track of all 188 | the target measurements, hunter positions, and hunter headings over time, but it doesn't 189 | do anything with that information.""" 190 | if not OTHER: # first time calling this function, set up my OTHER variables. 191 | measurements = [target_measurement] 192 | hunter_positions = [hunter_position] 193 | hunter_headings = [hunter_heading] 194 | OTHER = (measurements, hunter_positions, hunter_headings) # now I can keep track of history 195 | else: # not the first time, update my history 196 | OTHER[0].append(target_measurement) 197 | OTHER[1].append(hunter_position) 198 | OTHER[2].append(hunter_heading) 199 | measurements, hunter_positions, hunter_headings = OTHER # now I can always refer to these variables 200 | 201 | heading_to_target = get_heading(hunter_position, target_measurement) 202 | heading_difference = heading_to_target - hunter_heading 203 | turning = heading_difference # turn towards the target 204 | distance = max_distance # full speed ahead! 205 | return turning, distance, OTHER 206 | 207 | target = robot(0.0, 10.0, 0.0, 2*pi / 30, 1.5) 208 | measurement_noise = .05*target.distance 209 | target.set_noise(0.0, 0.0, measurement_noise) 210 | 211 | hunter = robot(-10.0, -10.0, 0.0) 212 | 213 | print demo_grading(hunter, target, next_move) 214 | 215 | 216 | 217 | 218 | 219 | -------------------------------------------------------------------------------- /Project Runaway RObot/kalman.py: -------------------------------------------------------------------------------- 1 | from matrix import * 2 | 3 | class kalman: 4 | 5 | def __init__(self, x=None, z=None, F=None, P=None): 6 | """ 7 | This function is called when you create a filter function. 8 | x -> state 9 | z -> measurement 10 | F -> state from last state to next state 11 | P -> x last covariances 12 | """ 13 | self.n = len(x.value) 14 | # measurement function: reflect the fact that we observe x and y 15 | self.H = matrix([[1.0 if i==j else 0.0 for j in range(self.n)] for i in range(2)]) 16 | # measurement noise uncertainty: use 2x2 matrix with 0.1 as main diagonal 17 | self.R = matrix([[2.0, 0.0], 18 | [0.0, 2.0]]) 19 | # n d identity matrix 20 | self.I = matrix([[1.0 if i==j else 0.0 for j in range(self.n)] for i in range(self.n)]) 21 | # external control motion 22 | self.u = matrix([[0.] for i in range(self.n)]) 23 | 24 | # F = F if F != None else I 25 | # P = P if P != None else I 26 | # z = z if z != None else matrix([[0,0]]) 27 | 28 | def kalman_filter(self, x, z, F, P): 29 | # prediction 30 | x = (F * x) + self.u # x -> last state 31 | P = F * P * F.transpose() 32 | 33 | # measurement update 34 | Z = matrix([z]) 35 | y = Z.transpose() - (self.H * x) 36 | S = self.H * P * self.H.transpose() + self.R 37 | K = P * self.H.transpose() * S.inverse() 38 | x = x + (K * y) 39 | P = (self.I - (K * H)) * P 40 | return x, P 41 | 42 | 43 | def extended_kalman_filter(self, x, z, F, P): 44 | """ 45 | Applies extended kalman filter on system 46 | 47 | z -> measurement 48 | x -> predict state 49 | u -> control vector 50 | P -> covariances 51 | F -> Function that returns F matrix for given 'x' 52 | H -> Measurement matrix 53 | R -> Measurement covariance 54 | """ 55 | 56 | # prediction 57 | x = x + self.u 58 | P = F * P * F.transpose() 59 | 60 | # measurement update 61 | Z = matrix([z]) 62 | y = Z.transpose() - (self.H * x) 63 | S = self.H * P * self.H.transpose() + self.R 64 | K = P * self.H.transpose() * S.inverse() 65 | x = x + (K * y) 66 | P = (self.I - (K * self.H)) * P 67 | 68 | return x, P 69 | -------------------------------------------------------------------------------- /Project Runaway RObot/kalman.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lijian-ml/CS373-Programming-a-Robotic-Car/0f55f6649b36d0306c14de4050c1ee90446993de/Project Runaway RObot/kalman.pyc -------------------------------------------------------------------------------- /Project Runaway RObot/matrix.py: -------------------------------------------------------------------------------- 1 | from math import * 2 | import random 3 | 4 | class matrix: 5 | 6 | def __init__(self, value): 7 | self.value = value 8 | self.dimx = len(value) 9 | self.dimy = len(value[0]) 10 | if value == [[]]: 11 | self.dimx = 0 12 | 13 | def zero(self, dimx, dimy): 14 | # check if valid dimensions 15 | if dimx < 1 or dimy < 1: 16 | raise ValueError, "Invalid size of matrix" 17 | else: 18 | self.dimx = dimx 19 | self.dimy = dimy 20 | self.value = [[0 for row in range(dimy)] for col in range(dimx)] 21 | 22 | def identity(self, dim): 23 | # check if valid dimension 24 | if dim < 1: 25 | raise ValueError, "Invalid size of matrix" 26 | else: 27 | self.dimx = dim 28 | self.dimy = dim 29 | self.value = [[0 for row in range(dim)] for col in range(dim)] 30 | for i in range(dim): 31 | self.value[i][i] = 1 32 | 33 | def show(self): 34 | for i in range(self.dimx): 35 | print self.value[i] 36 | print ' ' 37 | 38 | 39 | def __add__(self, other): 40 | # check if correct dimensions 41 | if self.dimx != other.dimx or self.dimx != other.dimx: 42 | raise ValueError, "Matrices must be of equal dimension to add" 43 | else: 44 | # add if correct dimensions 45 | res = matrix([[]]) 46 | res.zero(self.dimx, self.dimy) 47 | for i in range(self.dimx): 48 | for j in range(self.dimy): 49 | res.value[i][j] = self.value[i][j] + other.value[i][j] 50 | return res 51 | 52 | def __sub__(self, other): 53 | # check if correct dimensions 54 | if self.dimx != other.dimx or self.dimx != other.dimx: 55 | raise ValueError, "Matrices must be of equal dimension to subtract" 56 | else: 57 | # subtract if correct dimensions 58 | res = matrix([[]]) 59 | res.zero(self.dimx, self.dimy) 60 | for i in range(self.dimx): 61 | for j in range(self.dimy): 62 | res.value[i][j] = self.value[i][j] - other.value[i][j] 63 | return res 64 | 65 | def __mul__(self, other): 66 | # check if correct dimensions 67 | if self.dimy != other.dimx: 68 | raise ValueError, "Matrices must be m*n and n*p to multiply" 69 | else: 70 | # multiply if correct dimensions 71 | res = matrix([[]]) 72 | res.zero(self.dimx, other.dimy) 73 | for i in range(self.dimx): 74 | for j in range(other.dimy): 75 | for k in range(self.dimy): 76 | res.value[i][j] += self.value[i][k] * other.value[k][j] 77 | return res 78 | 79 | def transpose(self): 80 | # compute transpose 81 | res = matrix([[]]) 82 | res.zero(self.dimy, self.dimx) 83 | for i in range(self.dimx): 84 | for j in range(self.dimy): 85 | res.value[j][i] = self.value[i][j] 86 | return res 87 | 88 | 89 | def Cholesky(self, ztol= 1.0e-5): 90 | # Computes the upper triangular Cholesky factorization of 91 | # a positive definite matrix. 92 | # This code is based on http://adorio-research.org/wordpress/?p=4560 93 | res = matrix([[]]) 94 | res.zero(self.dimx, self.dimx) 95 | 96 | for i in range(self.dimx): 97 | S = sum([(res.value[k][i])**2 for k in range(i)]) 98 | d = self.value[i][i] - S 99 | if abs(d) < ztol: 100 | res.value[i][i] = 0.0 101 | else: 102 | if d < 0.0: 103 | raise ValueError, "Matrix not positive-definite" 104 | res.value[i][i] = sqrt(d) 105 | for j in range(i+1, self.dimx): 106 | S = sum([res.value[k][i] * res.value[k][j] for k in range(i)]) 107 | if abs(S) < ztol: 108 | S = 0.0 109 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 110 | return res 111 | 112 | def CholeskyInverse(self): 113 | # Computes inverse of matrix given its Cholesky upper Triangular 114 | # decomposition of matrix. 115 | # This code is based on http://adorio-research.org/wordpress/?p=4560 116 | 117 | res = matrix([[]]) 118 | res.zero(self.dimx, self.dimx) 119 | 120 | # Backward step for inverse. 121 | for j in reversed(range(self.dimx)): 122 | tjj = self.value[j][j] 123 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 124 | res.value[j][j] = 1.0/ tjj**2 - S/ tjj 125 | for i in reversed(range(j)): 126 | res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1,self.dimx)])/self.value[i][i] 127 | return res 128 | 129 | 130 | def inverse(self): 131 | aux = self.Cholesky() 132 | res = aux.CholeskyInverse() 133 | return res 134 | 135 | def __repr__(self): 136 | return repr(self.value) -------------------------------------------------------------------------------- /Project Runaway RObot/matrix.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lijian-ml/CS373-Programming-a-Robotic-Car/0f55f6649b36d0306c14de4050c1ee90446993de/Project Runaway RObot/matrix.pyc -------------------------------------------------------------------------------- /Project Runaway RObot/robot.py: -------------------------------------------------------------------------------- 1 | from math import * 2 | import random 3 | 4 | # helper function to map all angles onto [-pi, pi] 5 | def angle_trunc(a): 6 | while a < 0.0: 7 | a += pi * 2 8 | return ((a + pi) % (pi * 2)) - pi 9 | 10 | class robot: 11 | 12 | def __init__(self, x = 0.0, y = 0.0, heading = 0.0, turning = 2*pi/10, distance = 1.0): 13 | """This function is called when you create a new robot. It sets some of 14 | the attributes of the robot, either to their default values or to the values 15 | specified when it is created.""" 16 | self.x = x 17 | self.y = y 18 | self.heading = heading 19 | self.turning = turning # only applies to target robots who constantly move in a circle 20 | self.distance = distance # only applies to target bot, who always moves at same speed. 21 | self.turning_noise = 0.0 22 | self.distance_noise = 0.0 23 | self.measurement_noise = 0.0 24 | 25 | 26 | def set_noise(self, new_t_noise, new_d_noise, new_m_noise): 27 | """This lets us change the noise parameters, which can be very 28 | helpful when using particle filters.""" 29 | self.turning_noise = float(new_t_noise) 30 | self.distance_noise = float(new_d_noise) 31 | self.measurement_noise = float(new_m_noise) 32 | 33 | 34 | def move(self, turning, distance, tolerance = 0.001, max_turning_angle = pi): 35 | """This function turns the robot and then moves it forward.""" 36 | # apply noise, this doesn't change anything if turning_noise 37 | # and distance_noise are zero. 38 | turning = random.gauss(turning, self.turning_noise) 39 | distance = random.gauss(distance, self.distance_noise) 40 | 41 | # truncate to fit physical limitations 42 | turning = max(-max_turning_angle, turning) 43 | turning = min( max_turning_angle, turning) 44 | distance = max(0.0, distance) 45 | 46 | # Execute motion 47 | self.heading += turning 48 | self.heading = angle_trunc(self.heading) 49 | self.x += distance * cos(self.heading) 50 | self.y += distance * sin(self.heading) 51 | 52 | def move_in_circle(self): 53 | """This function is used to advance the runaway target bot.""" 54 | self.move(self.turning, self.distance) 55 | 56 | def sense(self): 57 | """This function represents the robot sensing its location. When 58 | measurements are noisy, this will return a value that is close to, 59 | but not necessarily equal to, the robot's (x, y) position.""" 60 | return (random.gauss(self.x, self.measurement_noise), 61 | random.gauss(self.y, self.measurement_noise)) 62 | 63 | def __repr__(self): 64 | """This allows us to print a robot's position""" 65 | return '[%.5f, %.5f]' % (self.x, self.y) 66 | 67 | 68 | -------------------------------------------------------------------------------- /Project Runaway RObot/robot.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lijian-ml/CS373-Programming-a-Robotic-Car/0f55f6649b36d0306c14de4050c1ee90446993de/Project Runaway RObot/robot.pyc -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 机器人人工智能 CS373 2 | 3 | 该课程为谷歌无人车之父塞巴斯蒂安·特龙于2012年4月在Udacity网站发布，讲授人工智能的基本方法，包括概率推论、规划和搜索、定位、跟踪和控制，所有内容都是以自动驾驶机车技术为重点。该课程需要初步的Python基础及部分概率知识、以及浓厚的兴趣。 4 | ### 课程内容包括: 5 | * 定位 6 | * 直方图滤波 7 | * 卡尔曼滤波 8 | * 粒子滤波 9 | * 规划 10 | * A*寻路算法 11 | * 动态规划 12 | * 控制 13 | * 轨迹平滑 14 | * PID 15 | * SLAM 16 | * Online Graph SLAM 17 | * 项目 18 | * 抓捕机器人 19 | 20 | * * * 21 | ### Udacity Class: 22 | [Udacity CS373 机器人人工智能 2012](https://classroom.udacity.com/courses/cs373 "Udacity Class") 23 | 这个repository是我学习Udacity课程期间完成的作业，欢迎大家Fork/Clone/下载，如有疑问欢迎交流。 24 | 25 | # Programming a Robotic Car CS373 26 | 27 | This course is about how to program all the major systems of a robotic car from the leader of Google and Stanford's autonomous driving teams, Sebastian Thrun. This class will teach you basic methods in Artificial Intelligence, including: probabilistic inference, planning and search, localization, tracking and control, all with a focus on robotics. Success in this course requires some programming experience with python, some basic concepts in probability and the most important thing is a lively interest in robotic. 28 | ### Course content: 29 | * Localization: 30 | * Histogramm Filters 31 | * Kalman Filters 32 | * Particle Filters 33 | * Planning: 34 | * A*-Search 35 | * Dynamic Programming 36 | * Control: 37 | * Smoothing 38 | * PID-Control 39 | * SLAM 40 | * Online Graph SLAM 41 | * Final Exam 42 | * Runaway RObot 43 | 44 | * * * 45 | ### Udacity Class: 46 | [Udacity CS373 Programming a Robotic Car 2012](http://www.udacity.com/overview/Course/cs373/CourseRev/feb2012 "Udacity Class") 47 | This repository contains all the homework projects, Welcome you to fork/clone/download my repository. I hope to communicate with you. 48 | --------------------------------------------------------------------------------