├── 1 - Localization └── localization.py ├── 2 - Kalman Filters ├── matrices.py └── matrices2.py ├── 3 - Particle Filters ├── newParticle.py └── particleFinal.py ├── 4 - Search ├── expansionGrid.py ├── firstSearch.py ├── implementAStar.py ├── leftTurnPolicy.py ├── optimumPolicy.py ├── printPath.py ├── stochasticMotion.py └── valueProgram.py ├── 5 - PID Control ├── PDController.py ├── PIDImplementation.py ├── constrainedSmoothing.py ├── cyclicSmoothing.py ├── implementPController.py ├── parameterOptimization.py ├── pathSmoothing.py └── racetrackControl.py ├── 6 - SLAM ├── confidentMeasurement.py ├── expand.py ├── funWithParameters.py ├── implementingSLAM.py ├── matrixExperiment.py ├── omegaAndXi.py ├── onlineSLAM.py └── segmentedCTE.py ├── Practice Exam └── warehouseRobot.py ├── Project - Runaway Robot ├── matrix.py ├── matrix.pyc ├── robot.py ├── robot.pyc ├── studentMain.py ├── studentMain1-noiselessPrediction.py ├── studentMain2A-addingNoise(sortaWorking).py ├── studentMain2B-addingNoise(polar).py ├── studentMain2C-addingNoise(my solution).py ├── studentMain3-theChaseBegins.py ├── studentMain4-chasingWithAPlan.py └── studentMainExample.py └── README.md /1 - Localization/localization.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 | 46 | def localize(colors,measurements,motions,sensor_right,p_move): 47 | # initializes p to a uniform distribution over a grid of the same dimensions as colors 48 | pinit = 1.0 / float(len(colors)) / float(len(colors[0])) 49 | p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))] 50 | 51 | # >>> Insert your code here <<< 52 | for i in range(len(measurements)): 53 | p = move(p, motions[i], p_move, colors) 54 | p = sense(p, measurements[i], sensor_right, colors) 55 | return p 56 | 57 | def sense(p, Z, pHit, colors): 58 | pMiss = 1. - pHit 59 | q = [[0 for row in range(len(colors[0]))] for col in range(len(colors))] 60 | for x in range(len(p[0])): 61 | for y in range(len(p)): 62 | hit = (Z == colors[y][x]) 63 | q[y][x] = (p[y][x] * (hit * pHit + (1-hit) * pMiss)) 64 | s = sum(map(sum,q)) 65 | for x in range(len(q[0])): 66 | for y in range(len(q)): 67 | q[y][x] = q[y][x] / s 68 | return q 69 | 70 | def move(p, U, p_move, colors): 71 | p_stay = 1. - p_move 72 | q = [[0 for row in range(len(colors[0]))] for col in range(len(colors))] 73 | for x in range(len(q[0])): 74 | for y in range(len(q)): 75 | s = p_move * p[(y-U[0]) % len(p)][(x-U[1]) % len(p[0])] 76 | s += p_stay * p[y][x] 77 | q[y][x] = s 78 | return q 79 | 80 | def show(p): 81 | rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p] 82 | print '[' + ',\n '.join(rows) + ']' 83 | 84 | ############################################################# 85 | # For the following test case, your output should be 86 | # [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465], 87 | # [0.00715, 0.01017, 0.08696, 0.07988, 0.00935], 88 | # [0.00739, 0.00894, 0.11272, 0.35350, 0.04065], 89 | # [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]] 90 | # (within a tolerance of +/- 0.001 for each entry) 91 | 92 | colors = [['R','G','G','R','R'], 93 | ['R','R','G','R','R'], 94 | ['R','R','G','G','R'], 95 | ['R','R','R','R','R']] 96 | measurements = ['G','G','G','G','G'] 97 | motions = [[0,0],[0,1],[1,0],[1,0],[0,1]] 98 | p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8) 99 | correct_answer = ( 100 | [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465], 101 | [0.00715, 0.01017, 0.08696, 0.07988, 0.00935], 102 | [0.00739, 0.00894, 0.11272, 0.35350, 0.04065], 103 | [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]]) 104 | print "Main problem:" 105 | show(p) # displays your answer 106 | show(correct_answer) 107 | print "\n" 108 | 109 | # test 1 110 | colors = [['G', 'G', 'G'], 111 | ['G', 'R', 'G'], 112 | ['G', 'G', 'G']] 113 | measurements = ['R'] 114 | motions = [[0,0]] 115 | sensor_right = 1.0 116 | p_move = 1.0 117 | p = localize(colors,measurements,motions,sensor_right,p_move) 118 | correct_answer = ( 119 | [[0.0, 0.0, 0.0], 120 | [0.0, 1.0, 0.0], 121 | [0.0, 0.0, 0.0]]) 122 | print "Test 1:" 123 | show(p) 124 | show(correct_answer) 125 | print "\n" 126 | 127 | # test 2 128 | colors = [['G', 'G', 'G'], 129 | ['G', 'R', 'R'], 130 | ['G', 'G', 'G']] 131 | measurements = ['R'] 132 | motions = [[0,0]] 133 | sensor_right = 1.0 134 | p_move = 1.0 135 | p = localize(colors,measurements,motions,sensor_right,p_move) 136 | correct_answer = ( 137 | [[0.0, 0.0, 0.0], 138 | [0.0, 0.5, 0.5], 139 | [0.0, 0.0, 0.0]]) 140 | print "Test 2:" 141 | show(p) 142 | show(correct_answer) 143 | print "\n" 144 | 145 | # test 3 146 | colors = [['G', 'G', 'G'], 147 | ['G', 'R', 'R'], 148 | ['G', 'G', 'G']] 149 | measurements = ['R'] 150 | motions = [[0,0]] 151 | sensor_right = 0.8 152 | p_move = 1.0 153 | p = localize(colors,measurements,motions,sensor_right,p_move) 154 | correct_answer = ( 155 | [[0.06666666666, 0.06666666666, 0.06666666666], 156 | [0.06666666666, 0.26666666666, 0.26666666666], 157 | [0.06666666666, 0.06666666666, 0.06666666666]]) 158 | print "Test 3:" 159 | show(p) 160 | show(correct_answer) 161 | print "\n" 162 | 163 | # test 4 164 | colors = [['G', 'G', 'G'], 165 | ['G', 'R', 'R'], 166 | ['G', 'G', 'G']] 167 | measurements = ['R', 'R'] 168 | motions = [[0,0], [0,1]] 169 | sensor_right = 0.8 170 | p_move = 1.0 171 | p = localize(colors,measurements,motions,sensor_right,p_move) 172 | correct_answer = ( 173 | [[0.03333333333, 0.03333333333, 0.03333333333], 174 | [0.13333333333, 0.13333333333, 0.53333333333], 175 | [0.03333333333, 0.03333333333, 0.03333333333]]) 176 | print "Test 4:" 177 | show(p) 178 | show(correct_answer) 179 | print "\n" 180 | 181 | # test 5 182 | colors = [['G', 'G', 'G'], 183 | ['G', 'R', 'R'], 184 | ['G', 'G', 'G']] 185 | measurements = ['R', 'R'] 186 | motions = [[0,0], [0,1]] 187 | sensor_right = 1.0 188 | p_move = 1.0 189 | p = localize(colors,measurements,motions,sensor_right,p_move) 190 | correct_answer = ( 191 | [[0.0, 0.0, 0.0], 192 | [0.0, 0.0, 1.0], 193 | [0.0, 0.0, 0.0]]) 194 | print "Test 5:" 195 | show(p) 196 | show(correct_answer) 197 | print "\n" 198 | 199 | # test 6 200 | colors = [['G', 'G', 'G'], 201 | ['G', 'R', 'R'], 202 | ['G', 'G', 'G']] 203 | measurements = ['R', 'R'] 204 | motions = [[0,0], [0,1]] 205 | sensor_right = 0.8 206 | p_move = 0.5 207 | p = localize(colors,measurements,motions,sensor_right,p_move) 208 | correct_answer = ( 209 | [[0.0289855072, 0.0289855072, 0.0289855072], 210 | [0.0724637681, 0.2898550724, 0.4637681159], 211 | [0.0289855072, 0.0289855072, 0.0289855072]]) 212 | print "Test 6:" 213 | show(p) 214 | show(correct_answer) 215 | print "\n" 216 | 217 | # test 7 218 | colors = [['G', 'G', 'G'], 219 | ['G', 'R', 'R'], 220 | ['G', 'G', 'G']] 221 | measurements = ['R', 'R'] 222 | motions = [[0,0], [0,1]] 223 | sensor_right = 1.0 224 | p_move = 0.5 225 | p = localize(colors,measurements,motions,sensor_right,p_move) 226 | correct_answer = ( 227 | [[0.0, 0.0, 0.0], 228 | [0.0, 0.33333333, 0.66666666], 229 | [0.0, 0.0, 0.0]]) 230 | print "Test 7:" 231 | show(p) 232 | show(correct_answer) 233 | print "\n" -------------------------------------------------------------------------------- /2 - Kalman Filters/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 | class matrix: 7 | 8 | # implements basic operations of a matrix class 9 | 10 | def __init__(self, value): 11 | self.value = value 12 | self.dimx = len(value) 13 | self.dimy = len(value[0]) 14 | if value == [[]]: 15 | self.dimx = 0 16 | 17 | def zero(self, dimx, dimy): 18 | # check if valid dimensions 19 | if dimx < 1 or dimy < 1: 20 | raise ValueError, "Invalid size of matrix" 21 | else: 22 | self.dimx = dimx 23 | self.dimy = dimy 24 | self.value = [[0 for row in range(dimy)] for col in range(dimx)] 25 | 26 | def identity(self, dim): 27 | # check if valid dimension 28 | if dim < 1: 29 | raise ValueError, "Invalid size of matrix" 30 | else: 31 | self.dimx = dim 32 | self.dimy = dim 33 | self.value = [[0 for row in range(dim)] for col in range(dim)] 34 | for i in range(dim): 35 | self.value[i][i] = 1 36 | 37 | def show(self): 38 | for i in range(self.dimx): 39 | print self.value[i] 40 | print ' ' 41 | 42 | def __add__(self, other): 43 | # check if correct dimensions 44 | if self.dimx != other.dimx or self.dimy != other.dimy: 45 | raise ValueError, "Matrices must be of equal dimensions to add" 46 | else: 47 | # add if correct dimensions 48 | res = matrix([[]]) 49 | res.zero(self.dimx, self.dimy) 50 | for i in range(self.dimx): 51 | for j in range(self.dimy): 52 | res.value[i][j] = self.value[i][j] + other.value[i][j] 53 | return res 54 | 55 | def __sub__(self, other): 56 | # check if correct dimensions 57 | if self.dimx != other.dimx or self.dimy != other.dimy: 58 | raise ValueError, "Matrices must be of equal dimensions to subtract" 59 | else: 60 | # subtract if correct dimensions 61 | res = matrix([[]]) 62 | res.zero(self.dimx, self.dimy) 63 | for i in range(self.dimx): 64 | for j in range(self.dimy): 65 | res.value[i][j] = self.value[i][j] - other.value[i][j] 66 | return res 67 | 68 | def __mul__(self, other): 69 | # check if correct dimensions 70 | if self.dimy != other.dimx: 71 | raise ValueError, "Matrices must be m*n and n*p to multiply" 72 | else: 73 | # subtract if correct dimensions 74 | res = matrix([[]]) 75 | res.zero(self.dimx, other.dimy) 76 | for i in range(self.dimx): 77 | for j in range(other.dimy): 78 | for k in range(self.dimy): 79 | res.value[i][j] += self.value[i][k] * other.value[k][j] 80 | return res 81 | 82 | def transpose(self): 83 | # compute transpose 84 | res = matrix([[]]) 85 | res.zero(self.dimy, self.dimx) 86 | for i in range(self.dimx): 87 | for j in range(self.dimy): 88 | res.value[j][i] = self.value[i][j] 89 | return res 90 | 91 | # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions 92 | 93 | def Cholesky(self, ztol=1.0e-5): 94 | # Computes the upper triangular Cholesky factorization of 95 | # a positive definite matrix. 96 | res = matrix([[]]) 97 | res.zero(self.dimx, self.dimx) 98 | 99 | for i in range(self.dimx): 100 | S = sum([(res.value[k][i])**2 for k in range(i)]) 101 | d = self.value[i][i] - S 102 | if abs(d) < ztol: 103 | res.value[i][i] = 0.0 104 | else: 105 | if d < 0.0: 106 | raise ValueError, "Matrix not positive-definite" 107 | res.value[i][i] = sqrt(d) 108 | for j in range(i+1, self.dimx): 109 | S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)]) 110 | if abs(S) < ztol: 111 | S = 0.0 112 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 113 | return res 114 | 115 | def CholeskyInverse(self): 116 | # Computes inverse of matrix given its Cholesky upper Triangular 117 | # decomposition of matrix. 118 | res = matrix([[]]) 119 | res.zero(self.dimx, self.dimx) 120 | 121 | # Backward step for inverse. 122 | for j in reversed(range(self.dimx)): 123 | tjj = self.value[j][j] 124 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 125 | res.value[j][j] = 1.0/tjj**2 - S/tjj 126 | for i in reversed(range(j)): 127 | 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] 128 | return res 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) 137 | 138 | 139 | ######################################## 140 | 141 | # Implement the filter function below 142 | 143 | def kalman_filter(x, P): 144 | for n in range(len(measurements)): 145 | 146 | # measurement update 147 | # y = z - H*x 148 | # S = H*P*Ht + R 149 | # K = P*Ht*S^-1 150 | # x1 = x + (K*y) 151 | # P1 = (I - K*H) * P 152 | Ht = H.transpose() 153 | z = matrix([[measurements[n]]]) 154 | y = z - (H * x) 155 | S = (H * P * Ht) + R 156 | Sinv = S.inverse() 157 | K = P * Ht * Sinv 158 | x = x + (K * y) 159 | P = (I - K * H) * P 160 | 161 | # prediction 162 | # x1 = F*x + u 163 | # P1 = F * P * Ft 164 | x = F * x + u 165 | P = F * P * F.transpose() 166 | 167 | return x,P 168 | 169 | ############################################ 170 | ### use the code below to test your filter! 171 | ############################################ 172 | 173 | measurements = [1, 2, 3] 174 | 175 | x = matrix([[0.], [0.]]) # initial state (location and velocity) 176 | P = matrix([[1000., 1.], [0., 1000.]]) # initial uncertainty 177 | u = matrix([[0.], [0.]]) # external motion 178 | F = matrix([[1., 1.], [0, 1.]]) # next state function 179 | H = matrix([[1., 0.]]) # measurement function 180 | R = matrix([[1.]]) # measurement uncertainty 181 | I = matrix([[1., 0.], [0., 1.]]) # identity matrix 182 | 183 | print kalman_filter(x, P) 184 | # output should be: 185 | # x: [[3.9996664447958645], [0.9999998335552873]] 186 | # P: [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]] 187 | -------------------------------------------------------------------------------- /2 - Kalman Filters/matrices2.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 | 185 | P = matrix([[0.,0.,0.,0.],[0.,0.,0.,0.],[0.,0.,1000.,0.],[0.,0.,0.,1000.]]) 186 | # initial uncertainty: 0 for positions x and y, 1000 for the two velocities 187 | F = matrix([[1.,0.,.1,0.],[0.,1.,0.,.1],[0.,0.,1.,0.],[0.,0.,0.,1.]]) 188 | # next state function: generalize the 2d version to 4d 189 | H = matrix([[1.,0.,0.,0.],[0.,1.,0.,0.]]) 190 | # measurement function: reflect the fact that we observe x and y but not the two velocities 191 | R = matrix([[.1,0],[0,.1]]) 192 | # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal 193 | I = matrix([[]]) 194 | I.identity(4) 195 | # 4d identity matrix 196 | 197 | 198 | ###### DO NOT MODIFY ANYTHING HERE ####### 199 | 200 | filter(x, P) 201 | -------------------------------------------------------------------------------- /3 - Particle Filters/newParticle.py: -------------------------------------------------------------------------------- 1 | # In this exercise, try to write a program that 2 | # will resample particles according to their weights. 3 | # Particles with higher weights should be sampled 4 | # more frequently (in proportion to their weight). 5 | 6 | # Don't modify anything below. Please scroll to the 7 | # bottom to enter your code. 8 | 9 | from math import * 10 | import random 11 | 12 | landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]] 13 | world_size = 100.0 14 | 15 | class robot: 16 | def __init__(self): 17 | self.x = random.random() * world_size 18 | self.y = random.random() * world_size 19 | self.orientation = random.random() * 2.0 * pi 20 | self.forward_noise = 0.0; 21 | self.turn_noise = 0.0; 22 | self.sense_noise = 0.0; 23 | 24 | def set(self, new_x, new_y, new_orientation): 25 | if new_x < 0 or new_x >= world_size: 26 | raise ValueError, 'X coordinate out of bound' 27 | if new_y < 0 or new_y >= world_size: 28 | raise ValueError, 'Y coordinate out of bound' 29 | if new_orientation < 0 or new_orientation >= 2 * pi: 30 | raise ValueError, 'Orientation must be in [0..2pi]' 31 | self.x = float(new_x) 32 | self.y = float(new_y) 33 | self.orientation = float(new_orientation) 34 | 35 | 36 | def set_noise(self, new_f_noise, new_t_noise, new_s_noise): 37 | # makes it possible to change the noise parameters 38 | # this is often useful in particle filters 39 | self.forward_noise = float(new_f_noise); 40 | self.turn_noise = float(new_t_noise); 41 | self.sense_noise = float(new_s_noise); 42 | 43 | 44 | def sense(self): 45 | Z = [] 46 | for i in range(len(landmarks)): 47 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 48 | dist += random.gauss(0.0, self.sense_noise) 49 | Z.append(dist) 50 | return Z 51 | 52 | 53 | def move(self, turn, forward): 54 | if forward < 0: 55 | raise ValueError, 'Robot cant move backwards' 56 | 57 | # turn, and add randomness to the turning command 58 | orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise) 59 | orientation %= 2 * pi 60 | 61 | # move, and add randomness to the motion command 62 | dist = float(forward) + random.gauss(0.0, self.forward_noise) 63 | x = self.x + (cos(orientation) * dist) 64 | y = self.y + (sin(orientation) * dist) 65 | x %= world_size # cyclic truncate 66 | y %= world_size 67 | 68 | # set particle 69 | res = robot() 70 | res.set(x, y, orientation) 71 | res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise) 72 | return res 73 | 74 | def Gaussian(self, mu, sigma, x): 75 | 76 | # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma 77 | return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) 78 | 79 | 80 | def measurement_prob(self, measurement): 81 | 82 | # calculates how likely a measurement should be 83 | 84 | prob = 1.0; 85 | for i in range(len(landmarks)): 86 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 87 | prob *= self.Gaussian(dist, self.sense_noise, measurement[i]) 88 | return prob 89 | 90 | def __repr__(self): 91 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation)) 92 | 93 | 94 | #myrobot = robot() 95 | #myrobot.set_noise(5.0, 0.1, 5.0) 96 | #myrobot.set(30.0, 50.0, pi/2) 97 | #myrobot = myrobot.move(-pi/2, 15.0) 98 | #print myrobot.sense() 99 | #myrobot = myrobot.move(-pi/2, 10.0) 100 | #print myrobot.sense() 101 | 102 | myrobot = robot() 103 | myrobot = myrobot.move(0.1, 5.0) 104 | Z = myrobot.sense() 105 | 106 | N = 1000 107 | p = [] 108 | for i in range(N): 109 | x = robot() 110 | x.set_noise(0.05, 0.05, 5.0) 111 | p.append(x) 112 | 113 | p2 = [] 114 | for i in range(N): 115 | p2.append(p[i].move(0.1, 5.0)) 116 | p = p2 117 | 118 | w = [] 119 | for i in range(N): 120 | w.append(p[i].measurement_prob(Z)) 121 | 122 | 123 | #### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW #### 124 | # You should make sure that p3 contains a list with particles 125 | # resampled according to their weights. 126 | # Also, DO NOT MODIFY p. 127 | 128 | wN = [] # normalized weights 129 | for i in range(N): 130 | wN.append(w[i]/sum(w)) 131 | 132 | p3 = [] 133 | for i in range(N): 134 | currentSum = 0. 135 | seek = random.random() 136 | for j in range(N): 137 | currentSum += wN[j] 138 | if seek < currentSum: 139 | p3.append(p[j]) 140 | break 141 | p3.sort() 142 | p = p3 143 | 144 | 145 | -------------------------------------------------------------------------------- /4 - Search/expansionGrid.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions: 3 | # 4 | # Modify the function search so that it returns 5 | # a table of values called expand. This table 6 | # will keep track of which step each node was 7 | # expanded. 8 | # 9 | # Make sure that the initial cell in the grid 10 | # you return has the value 0. 11 | # ---------- 12 | 13 | grid = [[0, 1, 0, 0, 0, 1, 0], 14 | [0, 1, 0, 1, 0, 1, 0], 15 | [0, 1, 0, 1, 0, 1, 0], 16 | [0, 1, 0, 1, 0, 1, 1], 17 | [0, 0, 0, 1, 0, 0, 0]] 18 | init = [0, 0] 19 | goal = [len(grid)-1, len(grid[0])-1] 20 | cost = 1 21 | 22 | delta = [[-1, 0], # go up 23 | [ 0,-1], # go left 24 | [ 1, 0], # go down 25 | [ 0, 1]] # go right 26 | 27 | delta_name = ['^', '<', 'v', '>'] 28 | 29 | def search(grid,init,goal,cost): 30 | # ---------------------------------------- 31 | # modify code below 32 | # ---------------------------------------- 33 | closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))] 34 | closed[init[0]][init[1]] = 1 35 | 36 | x = init[0] 37 | y = init[1] 38 | g = 0 39 | 40 | open = [[g, x, y]] 41 | expand = [[-1 for i in range(len(grid[0]))] for j in range(len(grid))] 42 | 43 | 44 | found = False # flag that is set when search is complete 45 | resign = False # flag set if we can't find expand 46 | 47 | step = 0 48 | 49 | while not found and not resign: 50 | 51 | if len(open) == 0: 52 | resign = True 53 | else: 54 | open.sort() 55 | open.reverse() 56 | next = open.pop() 57 | x = next[1] 58 | y = next[2] 59 | g = next[0] 60 | expand[x][y] = step 61 | step += 1 62 | if x == goal[0] and y == goal[1]: 63 | found = True 64 | else: 65 | for i in range(len(delta)): 66 | x2 = x + delta[i][0] 67 | y2 = y + delta[i][1] 68 | if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]): 69 | if closed[x2][y2] == 0 and grid[x2][y2] == 0: 70 | g2 = g + cost 71 | 72 | open.append([g2, x2, y2]) 73 | closed[x2][y2] = 1 74 | return expand 75 | 76 | print search(grid,init,goal,cost) -------------------------------------------------------------------------------- /4 - Search/firstSearch.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, 1, 0, 0, 0, 0], 19 | [0, 1, 0, 1, 0, 0], 20 | [0, 1, 0, 1, 0, 0], 21 | [0, 1, 0, 1, 0, 0], 22 | [0, 0, 0, 1, 0, 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 | # ---------------------------------------- 36 | # insert code here 37 | # ---------------------------------------- 38 | take = [0,init[0],init[1]] 39 | print "take:", take 40 | open = [] 41 | path = "fail" 42 | while True: 43 | if take[1] == goal[0] and take[2] == goal[1]: 44 | print "success!" 45 | path = take 46 | break 47 | for dir in delta: 48 | cell = [take[0]+cost,take[1]+dir[0],take[2]+dir[1]] 49 | if cell[1] > -1 and cell[1] < len(grid) and cell[2] > -1 and\ 50 | cell[2] < len(grid[0]) and grid[cell[1]][cell[2]] == 0: 51 | open.append(cell) 52 | grid[take[1]][take[2]] = 2 53 | if len(open) == 0: 54 | break 55 | open.sort() 56 | print "open:", open 57 | take = open[0] 58 | print "take:", take 59 | open.pop(0) 60 | return path 61 | 62 | print search(grid,init,goal,cost) -------------------------------------------------------------------------------- /4 - Search/implementAStar.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 | f = g + heuristic[x][y] 52 | 53 | open = [[f, g, x, y]] 54 | 55 | found = False # flag that is set when search is complete 56 | resign = False # flag set if we can't find expand 57 | count = 0 58 | 59 | while not found and not resign: 60 | if len(open) == 0: 61 | resign = True 62 | return "Fail" 63 | else: 64 | open.sort() 65 | open.reverse() 66 | next = open.pop() 67 | x = next[2] 68 | y = next[3] 69 | g = next[1] 70 | f = next[0] 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 | f2 = g2 + heuristic[x2][y2] 84 | open.append([f2, g2, x2, y2]) 85 | closed[x2][y2] = 1 86 | 87 | return expand 88 | 89 | for line in search(grid, init,goal,cost, heuristic): 90 | print line -------------------------------------------------------------------------------- /4 - Search/leftTurnPolicy.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, 1, 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 | 56 | # ---------------------------------------- 57 | # modify code below 58 | # ---------------------------------------- 59 | 60 | def optimum_policy2D(grid,init,goal,cost): 61 | value = [[[999 for facing in range(len(forward))]\ 62 | for col in range(len(grid[0]))]\ 63 | for row in range(len(grid))] 64 | policy = [[[' ' for facing in range(len(forward))]\ 65 | for col in range(len(grid[0]))]\ 66 | for row in range(len(grid))] 67 | change = True 68 | 69 | while change: 70 | change = False 71 | 72 | for y in range(len(grid)): 73 | for x in range(len(grid[0])): 74 | for f in range(len(forward)): 75 | if x == goal[1] and y == goal[0]: 76 | if value[y][x][f] > 0: 77 | value[y][x][f] = 0 78 | policy[y][x][f] = '*' 79 | change = True 80 | elif grid[y][x] == 0: 81 | for f2 in range(len(forward)): 82 | x2 = x + forward[f2][1] 83 | y2 = y + forward[f2][0] 84 | if x2 >= 0 and x2 < len(grid[0]) and y2 >= 0 and y2 < len(grid) and grid[y2][x2] == 0: 85 | targetVal = value[y2][x2][f2] 86 | for a in range(len(action)): 87 | if (f + action[a]) % len(forward) == f2: 88 | v2 = targetVal + cost[a] 89 | if v2 < value[y][x][f]: 90 | value[y][x][f] = v2 91 | policy[y][x][f] = action_name[a] 92 | change = True 93 | 94 | policy2D = [[' ' for x in range(len(grid[0]))] for y in range(len(grid))] 95 | x = init[1] 96 | y = init[0] 97 | f = init[2] 98 | policy2D[y][x] = policy[y][x][f] 99 | 100 | while policy[y][x][f] != '*': 101 | if policy[y][x][f] == 'R': 102 | f = (f - 1) % 4 103 | elif policy[y][x][f] == 'L': 104 | f = (f + 1) % 4 105 | x += forward[f][1] 106 | y += forward[f][0] 107 | policy2D[y][x] = policy[y][x][f] 108 | return policy2D 109 | 110 | for line in optimum_policy2D(grid,init,goal,cost): 111 | print line -------------------------------------------------------------------------------- /4 - Search/optimumPolicy.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 | change = True 38 | 39 | while change: 40 | change = False 41 | 42 | for x in range(len(grid)): 43 | for y in range(len(grid[0])): 44 | if goal[0] == x and goal[1] == y: 45 | if value[x][y] > 0: 46 | value[x][y] = 0 47 | 48 | change = True 49 | 50 | elif grid[x][y] == 0: 51 | for a in range(len(delta)): 52 | x2 = x + delta[a][0] 53 | y2 = y + delta[a][1] 54 | 55 | if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]) and grid[x2][y2] == 0: 56 | v2 = value[x2][y2] + cost 57 | 58 | if v2 < value[x][y]: 59 | change = True 60 | value[x][y] = v2 61 | 62 | policy = [[' ' for x in range(len(grid[0]))] for y in range(len(grid))] 63 | for x in range(len(grid[0])): 64 | for y in range(len(grid)): 65 | if value[y][x] == 99: 66 | continue 67 | if value[y][x] == 0: 68 | policy[y][x] = '*' 69 | continue 70 | surroundingCells = [99,99,99,99] 71 | for i in range(len(delta)): 72 | x2 = x + delta[i][1] 73 | y2 = y + delta[i][0] 74 | if x2 >= 0 and x2 < len(grid[0]) and y2 >= 0 and y2 < len(grid): 75 | surroundingCells[i] = value[y2][x2] 76 | minVal = min(surroundingCells) 77 | for j in range(len(surroundingCells)): 78 | if surroundingCells[j] == minVal and minVal != 99: 79 | policy[y][x] = delta_name[j] 80 | 81 | return policy 82 | 83 | for line in optimum_policy(grid, goal, cost): 84 | print line -------------------------------------------------------------------------------- /4 - Search/printPath.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions: 3 | # 4 | # Modify the the search function so that it returns 5 | # a shortest path as follows: 6 | # 7 | # [['>', 'v', ' ', ' ', ' ', ' '], 8 | # [' ', '>', '>', '>', '>', 'v'], 9 | # [' ', ' ', ' ', ' ', ' ', 'v'], 10 | # [' ', ' ', ' ', ' ', ' ', 'v'], 11 | # [' ', ' ', ' ', ' ', ' ', '*']] 12 | # 13 | # Where '>', '<', '^', and 'v' refer to right, left, 14 | # up, and down motions. Note that the 'v' should be 15 | # lowercase. '*' should mark the goal cell. 16 | # 17 | # You may assume that all test cases for this function 18 | # will have a path from init to goal. 19 | # ---------- 20 | 21 | grid = [[0, 0, 0, 0, 0, 0], 22 | [1, 1, 1, 1, 1, 0], 23 | [0, 0, 0, 0, 0, 0], 24 | [0, 0, 0, 0, 1, 1], 25 | [0, 0, 0, 0, 0, 0]] 26 | init = [0, 0] 27 | goal = [len(grid)-1, len(grid[0])-1] 28 | cost = 1 29 | expand = [[' ' for col in range(len(grid[0]))] for row in range(len(grid))] 30 | delta = [[-1, 0 ], # go up 31 | [ 0, -1], # go left 32 | [ 1, 0 ], # go down 33 | [ 0, 1 ]] # go right 34 | 35 | delta_name = ['^', '<', 'v', '>'] 36 | 37 | class Node: 38 | def __init__(self, x, y): 39 | self.children = [None,None,None,None] 40 | self.parent = None 41 | self.y = y 42 | self.x = x 43 | def findByCoords(self,x,y): 44 | if self.x == x and self.y == y: 45 | return self 46 | else: 47 | for child in self.children: 48 | if child != None: 49 | if child.findByCoords(x,y) != None: 50 | return child.findByCoords(x,y) 51 | 52 | 53 | def search(grid,init,goal,cost): 54 | # ---------------------------------------- 55 | # modify code below 56 | # ---------------------------------------- 57 | closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))] 58 | closed[init[0]][init[1]] = 1 59 | 60 | x = init[0] 61 | y = init[1] 62 | g = 0 63 | 64 | startNode = Node(x,y) 65 | currentNode = startNode 66 | 67 | open = [[g, x, y]] 68 | 69 | found = False # flag that is set when search is complete 70 | resign = False # flag set if we can't find expand 71 | 72 | while not found and not resign: 73 | if len(open) == 0: 74 | resign = True 75 | return 'fail' 76 | else: 77 | open.sort() 78 | open.reverse() 79 | next = open.pop() 80 | x = next[1] 81 | y = next[2] 82 | g = next[0] 83 | currentNode = startNode.findByCoords(x,y) 84 | 85 | if x == goal[0] and y == goal[1]: 86 | found = True 87 | expand[x][y] = '*' 88 | while True: 89 | parentNode = currentNode.parent 90 | for i in range(len(parentNode.children)): 91 | if parentNode.children[i] == currentNode: 92 | expand[parentNode.x][parentNode.y] = delta_name[i] 93 | if parentNode == startNode: 94 | break 95 | currentNode = parentNode 96 | else: 97 | for i in range(len(delta)): 98 | x2 = x + delta[i][0] 99 | y2 = y + delta[i][1] 100 | if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]): 101 | if closed[x2][y2] == 0 and grid[x2][y2] == 0: 102 | g2 = g + cost 103 | open.append([g2, x2, y2]) 104 | currentNode.children[i] = Node(x2,y2) 105 | childNode = currentNode.children[i] 106 | childNode.parent = currentNode 107 | closed[x2][y2] = 1 108 | 109 | return expand # make sure you return the shortest path 110 | output = search(grid, init, goal, cost) 111 | for i in output: 112 | print i -------------------------------------------------------------------------------- /4 - Search/stochasticMotion.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 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 | change = True 42 | 43 | while change: 44 | change = False 45 | 46 | for x in range(len(grid)): 47 | for y in range(len(grid[0])): 48 | if goal[0] == x and goal[1] == y: 49 | if value[x][y] > 0: 50 | value[x][y] = 0. 51 | policy[x][y] = '*' 52 | change = True 53 | 54 | elif grid[x][y] == 0: 55 | for a in range(len(delta)): 56 | totalCost = 0. 57 | for b in [-1,0,1]: 58 | x2 = x + delta[(a+b)%4][0] 59 | y2 = y + delta[(a+b)%4][1] 60 | prob = 0. 61 | if b == 0: 62 | prob = success_prob 63 | else: 64 | prob = failure_prob 65 | if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]) and grid[x2][y2] == 0: 66 | totalCost += value[x2][y2] * prob 67 | else: 68 | totalCost += collision_cost * prob 69 | totalCost += cost_step 70 | if totalCost < value[x][y]: 71 | change = True 72 | value[x][y] = totalCost 73 | policy[x][y] = delta_name[a] 74 | 75 | # for x in range(len(grid[0])): 76 | # for y in range(len(grid)): 77 | # if value[y][x] == 99: 78 | # continue 79 | # if value[y][x] == 0: 80 | # policy[y][x] = '*' 81 | # continue 82 | # surroundingCells = [99,99,99,99] 83 | # for i in range(len(delta)): 84 | # x2 = x + delta[i][1] 85 | # y2 = y + delta[i][0] 86 | # if x2 >= 0 and x2 < len(grid[0]) and y2 >= 0 and y2 < len(grid): 87 | # surroundingCells[i] = value[y2][x2] 88 | # minVal = min(surroundingCells) 89 | # for j in range(len(surroundingCells)): 90 | # if surroundingCells[j] == minVal and minVal != 99: 91 | # policy[y][x] = delta_name[j] 92 | 93 | return value, policy 94 | 95 | # --------------------------------------------- 96 | # Use the code below to test your solution 97 | # --------------------------------------------- 98 | 99 | grid = [[0, 0, 0, 0], 100 | [0, 0, 0, 0], 101 | [0, 0, 0, 0], 102 | [0, 1, 1, 0]] 103 | goal = [0, len(grid[0])-1] # Goal is in top right corner 104 | cost_step = 1 105 | collision_cost = 100. 106 | success_prob = 0.68 107 | 108 | value,policy = stochastic_value(grid,goal,cost_step,collision_cost,success_prob) 109 | for row in value: 110 | print row 111 | for row in policy: 112 | print row 113 | 114 | # Expected outputs: 115 | # 116 | # [57.9029, 40.2784, 26.0665, 0.0000] 117 | # [47.0547, 36.5722, 29.9937, 27.2698] 118 | # [53.1715, 42.0228, 37.7755, 45.0916] 119 | # [77.5858, 100.00, 100.00, 73.5458] 120 | # 121 | # ['>', 'v', 'v', '*'] 122 | # ['>', '>', '^', '<'] 123 | # ['>', '^', '^', '<'] 124 | # ['^', ' ', ' ', '^'] 125 | -------------------------------------------------------------------------------- /4 - Search/valueProgram.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, 1, 0, 0, 0, 0], 13 | [0, 1, 0, 0, 0, 0], 14 | [0, 1, 0, 0, 0, 0], 15 | [0, 1, 0, 0, 0, 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 | # insert code below 30 | # ---------------------------------------- 31 | value = [[99 for x in range(len(grid[0]))] for y in range(len(grid))] 32 | # make sure your function returns a grid of values as 33 | # demonstrated in the previous video. 34 | value[goal[0]][goal[1]] = 0 35 | openList = [] 36 | openList.append([0, goal[0],goal[1]]) 37 | while len(openList) != 0: 38 | openList.sort() 39 | currentCell = openList.pop(0) 40 | for i in range(len(delta)): 41 | targetX = currentCell[2] - delta[i][1] 42 | targetY = currentCell[1] - delta[i][0] 43 | if inGrid(grid, targetX, targetY): 44 | if grid[targetY][targetX] == 0 and value[targetY][targetX] == 99: 45 | openList.append([currentCell[0]+cost, targetY, targetX]) 46 | value[targetY][targetX] = currentCell[0] + cost 47 | return value 48 | 49 | 50 | def inGrid(grid, x, y): 51 | if x >= 0 and x < len(grid[0]) and y >= 0 and y < len(grid): 52 | return True 53 | else: 54 | return False 55 | 56 | for line in compute_value(grid, goal, cost): 57 | print line -------------------------------------------------------------------------------- /5 - PID Control/PDController.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 | from math import * 19 | import random 20 | 21 | 22 | # ------------------------------------------------ 23 | # 24 | # this is the robot class 25 | # 26 | 27 | class robot: 28 | 29 | # -------- 30 | # init: 31 | # creates robot and initializes location/orientation to 0, 0, 0 32 | # 33 | 34 | def __init__(self, length = 20.0): 35 | self.x = 0.0 36 | self.y = 0.0 37 | self.orientation = 0.0 38 | self.length = length 39 | self.steering_noise = 0.0 40 | self.distance_noise = 0.0 41 | self.steering_drift = 0.0 42 | 43 | # -------- 44 | # set: 45 | # sets a robot coordinate 46 | # 47 | 48 | def set(self, new_x, new_y, new_orientation): 49 | 50 | self.x = float(new_x) 51 | self.y = float(new_y) 52 | self.orientation = float(new_orientation) % (2.0 * pi) 53 | 54 | 55 | # -------- 56 | # set_noise: 57 | # sets the noise parameters 58 | # 59 | 60 | def set_noise(self, new_s_noise, new_d_noise): 61 | # makes it possible to change the noise parameters 62 | # this is often useful in particle filters 63 | self.steering_noise = float(new_s_noise) 64 | self.distance_noise = float(new_d_noise) 65 | 66 | # -------- 67 | # set_steering_drift: 68 | # sets the systematical steering drift parameter 69 | # 70 | 71 | def set_steering_drift(self, drift): 72 | self.steering_drift = drift 73 | 74 | # -------- 75 | # move: 76 | # steering = front wheel steering angle, limited by max_steering_angle 77 | # distance = total distance driven, most be non-negative 78 | 79 | def move(self, steering, distance, 80 | tolerance = 0.001, max_steering_angle = pi / 4.0): 81 | 82 | if steering > max_steering_angle: 83 | steering = max_steering_angle 84 | if steering < -max_steering_angle: 85 | steering = -max_steering_angle 86 | if distance < 0.0: 87 | distance = 0.0 88 | 89 | 90 | # make a new copy 91 | res = robot() 92 | res.length = self.length 93 | res.steering_noise = self.steering_noise 94 | res.distance_noise = self.distance_noise 95 | res.steering_drift = self.steering_drift 96 | 97 | # apply noise 98 | steering2 = random.gauss(steering, self.steering_noise) 99 | distance2 = random.gauss(distance, self.distance_noise) 100 | 101 | # apply steering drift 102 | steering2 += self.steering_drift 103 | 104 | # Execute motion 105 | turn = tan(steering2) * distance2 / res.length 106 | 107 | if abs(turn) < tolerance: 108 | 109 | # approximate by straight line motion 110 | 111 | res.x = self.x + (distance2 * cos(self.orientation)) 112 | res.y = self.y + (distance2 * sin(self.orientation)) 113 | res.orientation = (self.orientation + turn) % (2.0 * pi) 114 | 115 | else: 116 | 117 | # approximate bicycle model for motion 118 | 119 | radius = distance2 / turn 120 | cx = self.x - (sin(self.orientation) * radius) 121 | cy = self.y + (cos(self.orientation) * radius) 122 | res.orientation = (self.orientation + turn) % (2.0 * pi) 123 | res.x = cx + (sin(res.orientation) * radius) 124 | res.y = cy - (cos(res.orientation) * radius) 125 | 126 | return res 127 | 128 | def __repr__(self): 129 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 130 | 131 | ############## ADD / MODIFY CODE BELOW #################### 132 | 133 | # ------------------------------------------------------------------------ 134 | # 135 | # run - does a single control run. 136 | 137 | 138 | def run(param1, param2): 139 | myrobot = robot() 140 | myrobot.set(0.0, 1.0, 0.0) 141 | speed = 1.0 # motion distance is equal to speed (we assume time = 1) 142 | N = 100 143 | # 144 | # Enter code here 145 | # 146 | prevY = myrobot.y 147 | for i in range(N): 148 | steerAngle = -param1 * myrobot.y - param2 * (myrobot.y - prevY) 149 | prevY = myrobot.y 150 | myrobot = myrobot.move(steerAngle, speed) 151 | print myrobot 152 | 153 | # Call your function with parameters of 0.2 and 3.0 and print results 154 | run(0.2, 3.0) -------------------------------------------------------------------------------- /5 - PID Control/PIDImplementation.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 | from math import * 21 | import random 22 | 23 | 24 | # ------------------------------------------------ 25 | # 26 | # this is the robot class 27 | # 28 | 29 | class robot: 30 | 31 | # -------- 32 | # init: 33 | # creates robot and initializes location/orientation to 0, 0, 0 34 | # 35 | 36 | def __init__(self, length = 20.0): 37 | self.x = 0.0 38 | self.y = 0.0 39 | self.orientation = 0.0 40 | self.length = length 41 | self.steering_noise = 0.0 42 | self.distance_noise = 0.0 43 | self.steering_drift = 0.0 44 | 45 | # -------- 46 | # set: 47 | # sets a robot coordinate 48 | # 49 | 50 | def set(self, new_x, new_y, new_orientation): 51 | 52 | self.x = float(new_x) 53 | self.y = float(new_y) 54 | self.orientation = float(new_orientation) % (2.0 * pi) 55 | 56 | 57 | # -------- 58 | # set_noise: 59 | # sets the noise parameters 60 | # 61 | 62 | def set_noise(self, new_s_noise, new_d_noise): 63 | # makes it possible to change the noise parameters 64 | # this is often useful in particle filters 65 | self.steering_noise = float(new_s_noise) 66 | self.distance_noise = float(new_d_noise) 67 | 68 | # -------- 69 | # set_steering_drift: 70 | # sets the systematical steering drift parameter 71 | # 72 | 73 | def set_steering_drift(self, drift): 74 | self.steering_drift = drift 75 | 76 | # -------- 77 | # move: 78 | # steering = front wheel steering angle, limited by max_steering_angle 79 | # distance = total distance driven, most be non-negative 80 | 81 | def move(self, steering, distance, 82 | tolerance = 0.001, max_steering_angle = pi / 4.0): 83 | 84 | if steering > max_steering_angle: 85 | steering = max_steering_angle 86 | if steering < -max_steering_angle: 87 | steering = -max_steering_angle 88 | if distance < 0.0: 89 | distance = 0.0 90 | 91 | 92 | # make a new copy 93 | res = robot() 94 | res.length = self.length 95 | res.steering_noise = self.steering_noise 96 | res.distance_noise = self.distance_noise 97 | res.steering_drift = self.steering_drift 98 | 99 | # apply noise 100 | steering2 = random.gauss(steering, self.steering_noise) 101 | distance2 = random.gauss(distance, self.distance_noise) 102 | 103 | # apply steering drift 104 | steering2 += self.steering_drift 105 | 106 | # Execute motion 107 | turn = tan(steering2) * distance2 / res.length 108 | 109 | if abs(turn) < tolerance: 110 | 111 | # approximate by straight line motion 112 | 113 | res.x = self.x + (distance2 * cos(self.orientation)) 114 | res.y = self.y + (distance2 * sin(self.orientation)) 115 | res.orientation = (self.orientation + turn) % (2.0 * pi) 116 | 117 | else: 118 | 119 | # approximate bicycle model for motion 120 | 121 | radius = distance2 / turn 122 | cx = self.x - (sin(self.orientation) * radius) 123 | cy = self.y + (cos(self.orientation) * radius) 124 | res.orientation = (self.orientation + turn) % (2.0 * pi) 125 | res.x = cx + (sin(res.orientation) * radius) 126 | res.y = cy - (cos(res.orientation) * radius) 127 | 128 | return res 129 | 130 | 131 | 132 | 133 | def __repr__(self): 134 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 135 | 136 | 137 | 138 | 139 | ############## ADD / MODIFY CODE BELOW #################### 140 | 141 | # ------------------------------------------------------------------------ 142 | # 143 | # run - does a single control run. 144 | 145 | 146 | def run(param1, param2, param3): 147 | myrobot = robot() 148 | myrobot.set(0.0, 1.0, 0.0) 149 | speed = 1.0 # motion distance is equal to speed (we assume time = 1) 150 | N = 100 151 | myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree bias, this will be added in by the move function, you do not need to add it below! 152 | # 153 | # Enter code here 154 | # 155 | prevY = myrobot.y 156 | totalError = 0 157 | for i in range(N): 158 | steerAngle = -param1 * myrobot.y 159 | steerAngle += - param2 * (myrobot.y - prevY) 160 | steerAngle += - param3 * totalError 161 | prevY = myrobot.y 162 | totalError += prevY 163 | myrobot = myrobot.move(steerAngle, speed) 164 | print myrobot 165 | 166 | # Call your function with parameters of (0.2, 3.0, and 0.004) 167 | run(0.2, 3.0, 0.004) -------------------------------------------------------------------------------- /5 - PID Control/constrainedSmoothing.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 | # Enter code here. 24 | # The weight for each of the two new equations should be 0.5 * weight_smooth 25 | # 26 | # Make a deep copy of path into newpath 27 | from copy import deepcopy 28 | newpath = deepcopy(path) 29 | 30 | while True: 31 | totalChange = 0. 32 | for i in range(len(path)): 33 | if not fix[i]: 34 | for dim in range(len(path[i])): 35 | oldVal = newpath[i][dim] 36 | newpath[i][dim] = newpath[i][dim] + \ 37 | weight_data * (path[i][dim] - newpath[i][dim]) + \ 38 | weight_smooth * (newpath[(i+1)%len(path)][dim] + \ 39 | newpath[(i-1)%len(path)][dim] - 2 * newpath[i][dim]) + \ 40 | weight_smooth/2 * (2 * newpath[(i+1)%len(path)][dim] - \ 41 | newpath[(i+2)%len(path)][dim] - newpath[i][dim]) + \ 42 | weight_smooth/2 * (2 * newpath[(i-1)%len(path)][dim] - \ 43 | newpath[(i-2)%len(path)][dim] - newpath[i][dim]) 44 | totalChange += abs(oldVal - newpath[i][dim]) 45 | if totalChange < tolerance: 46 | break 47 | for line in newpath: 48 | print line 49 | return newpath 50 | 51 | # -------------- 52 | # Testing Instructions 53 | # 54 | # To test your code, call the solution_check function with the argument smooth: 55 | # solution_check(smooth) 56 | # 57 | 58 | def solution_check(smooth, eps = 0.0001): 59 | 60 | def test_case_str(path, fix): 61 | assert( len(path) == len(fix) ) 62 | 63 | if len(path) == 0: 64 | return '[]' 65 | if len(path) == 1: 66 | s = '[' + str(path[0]) + ']' 67 | if fix[0]: s += ' #fix' 68 | return s 69 | 70 | s = '[' + str(path[0]) + ',' 71 | if fix[0]: s += ' #fix' 72 | for pt,f in zip(path[1:-1],fix[1:-1]): 73 | s += '\n ' + str(pt) + ',' 74 | if f: s += ' #fix' 75 | s += '\n ' + str(path[-1]) + ']' 76 | if fix[-1]: s += ' #fix' 77 | return s 78 | 79 | 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]], 80 | [[0, 0],[2, 0],[4, 0],[4, 2],[4, 4],[2, 4],[0, 4],[0, 2]]] 81 | testfixpts = [[1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0], 82 | [1, 0, 1, 0, 1, 0, 1, 0]] 83 | 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]], 84 | [[0, 0],[2.0116767115496095, -0.7015439080661671],[4, 0],[4.701543905420104, 2.0116768147460418],[4, 4],[1.9883231877640861, 4.701543807525115],[0, 4],[-0.7015438099112995, 1.9883232808252207]]] 85 | 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]], 86 | [[0, 0],[1.9999953708444873, -0.6666702980585777],[4, 0],[4.666670298058577, 2.000005101453379],[4, 4],[1.9999948985466212, 4.6666612524128],[0, 4],[-0.6666612524127998, 2.000003692691148]]] 87 | newpaths = map(lambda p: smooth(*p), zip(testpaths,testfixpts)) 88 | 89 | correct = True 90 | 91 | for path,fix,p_answer,t_answer,newpath in zip(testpaths,testfixpts, 92 | pseudo_answers,true_answers, 93 | newpaths): 94 | if type(newpath) != list: 95 | print "Error: smooth did not return a list for the path:" 96 | print test_case_str(path,fix) + '\n' 97 | correct = False 98 | continue 99 | if len(newpath) != len(path): 100 | print "Error: smooth did not return a list of the correct length for the path:" 101 | print test_case_str(path,fix) + '\n' 102 | correct = False 103 | continue 104 | 105 | good_pairs = True 106 | for newpt,pt in zip(newpath,path): 107 | if len(newpt) != len(pt): 108 | good_pairs = False 109 | break 110 | if not good_pairs: 111 | print "Error: smooth did not return a list of coordinate pairs for the path:" 112 | print test_case_str(path,fix) + '\n' 113 | correct = False 114 | continue 115 | assert( good_pairs ) 116 | 117 | # check whether to check against true or pseudo answers 118 | answer = None 119 | if abs(newpath[1][0] - t_answer[1][0]) <= eps: 120 | answer = t_answer 121 | elif abs(newpath[1][0] - p_answer[1][0]) <= eps: 122 | answer = p_answer 123 | else: 124 | print 'smooth returned an incorrect answer for the path:' 125 | print test_case_str(path,fix) + '\n' 126 | continue 127 | assert( answer is not None ) 128 | 129 | entries_match = True 130 | for p,q in zip(newpath,answer): 131 | for pi,qi in zip(p,q): 132 | if abs(pi - qi) > eps: 133 | entries_match = False 134 | break 135 | if not entries_match: break 136 | if not entries_match: 137 | print 'smooth returned an incorrect answer for the path:' 138 | print test_case_str(path,fix) + '\n' 139 | continue 140 | 141 | assert( entries_match ) 142 | if answer == t_answer: 143 | print 'smooth returned the correct answer for the path:' 144 | print test_case_str(path,fix) + '\n' 145 | elif answer == p_answer: 146 | print 'smooth returned a correct* answer for the path:' 147 | print test_case_str(path,fix) 148 | print '''*However, your answer uses the "nonsimultaneous" update method, which 149 | is not technically correct. You should modify your code so that newpath[i][j] is only 150 | updated once per iteration, or else the intermediate updates made to newpath[i][j] 151 | will affect the final answer.\n''' 152 | 153 | solution_check(smooth) 154 | -------------------------------------------------------------------------------- /5 - PID Control/cyclicSmoothing.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 | from copy import deepcopy 31 | 32 | # Do not modify path inside your function. 33 | path=[[0, 0], 34 | [1, 0], 35 | [2, 0], 36 | [3, 0], 37 | [4, 0], 38 | [5, 0], 39 | [6, 0], 40 | [6, 1], 41 | [6, 2], 42 | [6, 3], 43 | [5, 3], 44 | [4, 3], 45 | [3, 3], 46 | [2, 3], 47 | [1, 3], 48 | [0, 3], 49 | [0, 2], 50 | [0, 1]] 51 | 52 | ############# ONLY ENTER CODE BELOW THIS LINE ########## 53 | 54 | # ------------------------------------------------ 55 | # smooth coordinates 56 | # If your code is timing out, make the tolerance parameter 57 | # larger to decrease run time. 58 | # 59 | 60 | def smooth(path, weight_data = 0.1, weight_smooth = 0.1, tolerance = 0.00001): 61 | 62 | # 63 | # Enter code here 64 | # 65 | # Make a deep copy of path into newpath 66 | newpath = deepcopy(path) 67 | 68 | while True: 69 | totalChange = 0. 70 | for i in range(len(path)): 71 | for dim in range(len(path[i])): 72 | oldVal = newpath[i][dim] 73 | newpath[i][dim] = newpath[i][dim] + \ 74 | weight_data * (path[i][dim] - newpath[i][dim]) + \ 75 | weight_smooth * (newpath[(i+1)%len(path)][dim] + \ 76 | newpath[(i-1)%len(path)][dim] - 2 * newpath[i][dim]) 77 | totalChange += abs(oldVal - newpath[i][dim]) 78 | if totalChange < tolerance: 79 | break 80 | return newpath 81 | 82 | # thank you - EnTerr - for posting this on our discussion forum 83 | 84 | #newpath = smooth(path) 85 | #for i in range(len(path)): 86 | # print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']' 87 | 88 | 89 | ##### TESTING ###### 90 | 91 | # -------------------------------------------------- 92 | # check if two numbers are 'close enough,'used in 93 | # solution_check function. 94 | # 95 | def close_enough(user_answer, true_answer, epsilon = 0.001): 96 | if abs(user_answer - true_answer) > epsilon: 97 | return False 98 | return True 99 | 100 | # -------------------------------------------------- 101 | # check your solution against our reference solution for 102 | # a variety of test cases (given below) 103 | # 104 | def solution_check(newpath, answer): 105 | if type(newpath) != type(answer): 106 | print "Error. You do not return a list." 107 | return False 108 | if len(newpath) != len(answer): 109 | print 'Error. Your newpath is not the correct length.' 110 | return False 111 | if len(newpath[0]) != len(answer[0]): 112 | print 'Error. Your entries do not contain an (x, y) coordinate pair.' 113 | return False 114 | for i in range(len(newpath)): 115 | for j in range(len(newpath[0])): 116 | if not close_enough(newpath[i][j], answer[i][j]): 117 | print 'Error, at least one of your entries is not correct.' 118 | return False 119 | print "Test case correct!" 120 | return True 121 | 122 | # -------------- 123 | # Testing Instructions 124 | # 125 | # To test your code, call the solution_check function with 126 | # two arguments. The first argument should be the result of your 127 | # smooth function. The second should be the corresponding answer. 128 | # For example, calling 129 | # 130 | # solution_check(smooth(testpath1), answer1) 131 | # 132 | # should return True if your answer is correct and False if 133 | # it is not. 134 | 135 | testpath1 = [[0, 0], 136 | [1, 0], 137 | [2, 0], 138 | [3, 0], 139 | [4, 0], 140 | [5, 0], 141 | [6, 0], 142 | [6, 1], 143 | [6, 2], 144 | [6, 3], 145 | [5, 3], 146 | [4, 3], 147 | [3, 3], 148 | [2, 3], 149 | [1, 3], 150 | [0, 3], 151 | [0, 2], 152 | [0, 1]] 153 | 154 | answer1 = [[0.4705860385182691, 0.4235279620576893], 155 | [1.1764695730296597, 0.16470408411716733], 156 | [2.058823799247812, 0.07058633859438503], 157 | [3.000001503542886, 0.04705708651959327], 158 | [3.9411790099468273, 0.07058689299792453], 159 | [4.8235326678889345, 0.16470511854183797], 160 | [5.529415336860586, 0.4235293374365447], 161 | [5.76470933698621, 1.1058829941330384], 162 | [5.764708805535902, 1.8941189433780983], 163 | [5.5294138118186265, 2.5764724018811056], 164 | [4.823530348360371, 2.835296251305122], 165 | [3.941176199414957, 2.929413985845729], 166 | [2.9999985709076413, 2.952943245204772], 167 | [2.0588211310939526, 2.9294134622132018], 168 | [1.1764675231284938, 2.8352952720424938], 169 | [0.4705848811030855, 2.5764710948028178], 170 | [0.23529088056307781, 1.8941174802285707], 171 | [0.23529138316655338, 1.1058815684272394]] 172 | 173 | testpath2 = [[1, 0], # Move in the shape of a plus sign 174 | [2, 0], 175 | [2, 1], 176 | [3, 1], 177 | [3, 2], 178 | [2, 2], 179 | [2, 3], 180 | [1, 3], 181 | [1, 2], 182 | [0, 2], 183 | [0, 1], 184 | [1, 1]] 185 | 186 | answer2 = [[1.2222234770374059, 0.4444422843711052], 187 | [1.7777807251383388, 0.4444432993123497], 188 | [2.111114925633848, 0.8888894279539462], 189 | [2.5555592020540376, 1.2222246475393077], 190 | [2.5555580686154244, 1.7777817817879298], 191 | [2.111111849558437, 2.1111159707965514], 192 | [1.7777765871460525, 2.55556033483712], 193 | [1.2222194640861452, 2.5555593592828543], 194 | [0.8888853322565222, 2.111113321684573], 195 | [0.44444105139827167, 1.777778212019149], 196 | [0.44444210978390364, 1.2222211690821811], 197 | [0.8888882042812255, 0.8888870211766268]] 198 | 199 | solution_check(smooth(testpath1), answer1) 200 | solution_check(smooth(testpath2), answer2) -------------------------------------------------------------------------------- /5 - PID Control/implementPController.py: -------------------------------------------------------------------------------- 1 | # ----------- 2 | # User Instructions 3 | # 4 | # Implement a P controller by running 100 iterations 5 | # of robot motion. The desired trajectory for the 6 | # robot is the x-axis. The steering angle should be set 7 | # by the parameter tau so that: 8 | # 9 | # steering = -tau * crosstrack_error 10 | # 11 | # Note that tau is called "param" in the function 12 | # below. 13 | # 14 | # Your code should print output that looks like 15 | # the output shown in the video. That is, at each step: 16 | # print myrobot, steering 17 | # 18 | # Only modify code at the bottom! 19 | # ------------ 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 | def __repr__(self): 132 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 133 | 134 | ############## ADD / MODIFY CODE BELOW #################### 135 | 136 | # ------------------------------------------------------------------------ 137 | # 138 | # run - does a single control run 139 | 140 | 141 | def run(param): 142 | myrobot = robot() 143 | myrobot.set(0.0, 1.0, 0.0) 144 | speed = 1.0 # motion distance is equal to speed (we assume time = 1) 145 | N = 100 146 | # 147 | # Add Code Here 148 | # 149 | for i in range(N): 150 | steerAngle = -param * myrobot.y 151 | myrobot = myrobot.move(steerAngle, speed) 152 | print myrobot 153 | 154 | run(0.1) # call function with parameter tau of 0.1 and print results -------------------------------------------------------------------------------- /5 - PID Control/parameterOptimization.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 | from math import * 17 | import random 18 | 19 | 20 | # ------------------------------------------------ 21 | # 22 | # this is the robot class 23 | # 24 | 25 | class robot: 26 | 27 | # -------- 28 | # init: 29 | # creates robot and initializes location/orientation to 0, 0, 0 30 | # 31 | 32 | def __init__(self, length = 20.0): 33 | self.x = 0.0 34 | self.y = 0.0 35 | self.orientation = 0.0 36 | self.length = length 37 | self.steering_noise = 0.0 38 | self.distance_noise = 0.0 39 | self.steering_drift = 0.0 40 | 41 | # -------- 42 | # set: 43 | # sets a robot coordinate 44 | # 45 | 46 | def set(self, new_x, new_y, new_orientation): 47 | 48 | self.x = float(new_x) 49 | self.y = float(new_y) 50 | self.orientation = float(new_orientation) % (2.0 * pi) 51 | 52 | 53 | # -------- 54 | # set_noise: 55 | # sets the noise parameters 56 | # 57 | 58 | def set_noise(self, new_s_noise, new_d_noise): 59 | # makes it possible to change the noise parameters 60 | # this is often useful in particle filters 61 | self.steering_noise = float(new_s_noise) 62 | self.distance_noise = float(new_d_noise) 63 | 64 | # -------- 65 | # set_steering_drift: 66 | # sets the systematical steering drift parameter 67 | # 68 | 69 | def set_steering_drift(self, drift): 70 | self.steering_drift = drift 71 | 72 | # -------- 73 | # move: 74 | # steering = front wheel steering angle, limited by max_steering_angle 75 | # distance = total distance driven, most be non-negative 76 | 77 | def move(self, steering, distance, 78 | tolerance = 0.001, max_steering_angle = pi / 4.0): 79 | 80 | if steering > max_steering_angle: 81 | steering = max_steering_angle 82 | if steering < -max_steering_angle: 83 | steering = -max_steering_angle 84 | if distance < 0.0: 85 | distance = 0.0 86 | 87 | 88 | # make a new copy 89 | res = robot() 90 | res.length = self.length 91 | res.steering_noise = self.steering_noise 92 | res.distance_noise = self.distance_noise 93 | res.steering_drift = self.steering_drift 94 | 95 | # apply noise 96 | steering2 = random.gauss(steering, self.steering_noise) 97 | distance2 = random.gauss(distance, self.distance_noise) 98 | 99 | # apply steering drift 100 | steering2 += self.steering_drift 101 | 102 | # Execute motion 103 | turn = tan(steering2) * distance2 / res.length 104 | 105 | if abs(turn) < tolerance: 106 | 107 | # approximate by straight line motion 108 | 109 | res.x = self.x + (distance2 * cos(self.orientation)) 110 | res.y = self.y + (distance2 * sin(self.orientation)) 111 | res.orientation = (self.orientation + turn) % (2.0 * pi) 112 | 113 | else: 114 | 115 | # approximate bicycle model for motion 116 | 117 | radius = distance2 / turn 118 | cx = self.x - (sin(self.orientation) * radius) 119 | cy = self.y + (cos(self.orientation) * radius) 120 | res.orientation = (self.orientation + turn) % (2.0 * pi) 121 | res.x = cx + (sin(res.orientation) * radius) 122 | res.y = cy - (cos(res.orientation) * radius) 123 | 124 | return res 125 | 126 | 127 | 128 | 129 | def __repr__(self): 130 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 131 | 132 | 133 | # ------------------------------------------------------------------------ 134 | # 135 | # run - does a single control run. 136 | 137 | 138 | def run(params, printflag = False): 139 | myrobot = robot() 140 | myrobot.set(0.0, 1.0, 0.0) 141 | speed = 1.0 142 | err = 0.0 143 | int_crosstrack_error = 0.0 144 | N = 100 145 | # myrobot.set_noise(0.1, 0.0) 146 | myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree steering error 147 | 148 | 149 | crosstrack_error = myrobot.y 150 | 151 | 152 | for i in range(N * 2): 153 | 154 | diff_crosstrack_error = myrobot.y - crosstrack_error 155 | crosstrack_error = myrobot.y 156 | int_crosstrack_error += crosstrack_error 157 | 158 | steer = - params[0] * crosstrack_error \ 159 | - params[1] * diff_crosstrack_error \ 160 | - int_crosstrack_error * params[2] 161 | myrobot = myrobot.move(steer, speed) 162 | if i >= N: 163 | err += (crosstrack_error ** 2) 164 | if printflag: 165 | print myrobot, steer 166 | return err / float(N) 167 | 168 | 169 | def twiddle(tol = 0.00002): #Make this tolerance bigger if you are timing out! 170 | ############## ADD CODE BELOW #################### 171 | 172 | # ------------- 173 | # Add code here 174 | # ------------- 175 | p = [0.,0.,0.] 176 | dp = [1.,1.,1.] 177 | 178 | bestErr = run(p) 179 | while sum(dp) > tol: 180 | print p 181 | for i in range(3): 182 | p[i] += dp[i] 183 | err = run(p) 184 | if err < bestErr: 185 | bestErr = err 186 | dp[i] *= 1.1 187 | continue 188 | p[i] -= 2*dp[i] 189 | err = run(p) 190 | if err < bestErr: 191 | bestErr = err 192 | dp[i] *= 1.1 193 | continue 194 | p[i] += dp[i] 195 | dp[i] *= 0.9 196 | 197 | print p, dp 198 | return run(p) 199 | 200 | print run([0.,0.,0.]) 201 | print twiddle() -------------------------------------------------------------------------------- /5 - PID Control/pathSmoothing.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 | ####################### 42 | ### ENTER CODE HERE ### 43 | ####################### 44 | while True: 45 | totalChange = 0. 46 | for i in range(len(path)): 47 | if i != 0 and i != (len(path) - 1): 48 | for dim in range(len(path[i])): 49 | oldVal = newpath[i][dim] 50 | newpath[i][dim] = newpath[i][dim] + \ 51 | weight_data * (path[i][dim] - newpath[i][dim]) + \ 52 | weight_smooth * (newpath[i+1][dim] + newpath[i-1][dim] - 2 * newpath[i][dim]) 53 | totalChange += abs(oldVal - newpath[i][dim]) 54 | if totalChange < tolerance: 55 | break 56 | return newpath # Leave this line for the grader! 57 | 58 | printpaths(path,smooth(path)) -------------------------------------------------------------------------------- /5 - PID Control/racetrackControl.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 | # 142 | # 143 | # Add code here 144 | # 145 | # 146 | center1 = [radius, radius] #y,x 147 | center2 = [radius, 3*radius] 148 | if self.x > radius and self.x < 3*radius: 149 | if self.y > radius: 150 | cte = self.y - (2*radius) 151 | else: 152 | cte = -1.0 * self.y 153 | else: 154 | if self.x < 2*radius: 155 | cte = ((self.x - center1[1])**2 + (self.y - center1[0])**2)**0.5 - radius 156 | else: 157 | cte = ((self.x - center2[1])**2 + (self.y - center2[0])**2)**0.5 - radius 158 | 159 | return cte 160 | 161 | ############## ONLY ADD / MODIFY CODE ABOVE THIS LINE #################### 162 | 163 | 164 | 165 | 166 | # ------------------------------------------------------------------------ 167 | # 168 | # run - does a single control run. 169 | 170 | 171 | def run(params, radius, printflag = False): 172 | myrobot = robot() 173 | myrobot.set(0.0, radius, pi / 2.0) 174 | speed = 1.0 # motion distance is equal to speed (we assume time = 1) 175 | err = 0.0 176 | int_crosstrack_error = 0.0 177 | N = 200 178 | 179 | crosstrack_error = myrobot.cte(radius) # You need to define the cte function! 180 | 181 | for i in range(N*2): 182 | diff_crosstrack_error = - crosstrack_error 183 | crosstrack_error = myrobot.cte(radius) 184 | diff_crosstrack_error += crosstrack_error 185 | int_crosstrack_error += crosstrack_error 186 | steer = - params[0] * crosstrack_error \ 187 | - params[1] * diff_crosstrack_error \ 188 | - params[2] * int_crosstrack_error 189 | myrobot = myrobot.move(steer, speed) 190 | if i >= N: 191 | err += crosstrack_error ** 2 192 | if printflag: 193 | print myrobot 194 | return err / float(N) 195 | 196 | radius = 25.0 197 | params = [10.0, 15.0, 0] 198 | err = run(params, radius, True) 199 | print '\nFinal parameters: ', params, '\n ->', err 200 | -------------------------------------------------------------------------------- /6 - SLAM/confidentMeasurement.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, 1.0*5, -1.0*5], 369 | [0.0, 0.0, -1.0*5, 1.0*5]]) 370 | Xi += matrix([[0.0], 371 | [0.0], 372 | [-Z2*5], 373 | [Z2*5]]) 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) -------------------------------------------------------------------------------- /6 - 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 | # 355 | # 356 | # Add your code here. 357 | # 358 | # 359 | Omega = Omega.expand(4,4,[0,1,2],[0,1,2]) 360 | Xi = Xi.expand(4,1,[0,1,2],[0]) 361 | 362 | Omega += matrix([[1.0, 0.0, 0.0, -1.0], 363 | [0.0, 0.0, 0.0, 0.0], 364 | [0.0, 0.0, 0.0, 0.0], 365 | [-1.0, 0.0, 0.0, 1.0]]) 366 | Xi += matrix([[-Z0], 367 | [0.0], 368 | [0.0], 369 | [Z0]]) 370 | Omega += matrix([[0.0, 0.0, 0.0, 0.0], 371 | [0.0, 1.0, 0.0, -1.0], 372 | [0.0, 0.0, 0.0, 0.0], 373 | [0.0, -1.0, 0.0, 1.0]]) 374 | Xi += matrix([[0.0], 375 | [-Z1], 376 | [0.0], 377 | [Z1]]) 378 | Omega += matrix([[0.0, 0.0, 0.0, 0.0], 379 | [0.0, 0.0, 0.0, 0.0], 380 | [0.0, 0.0, 1.0, -1.0], 381 | [0.0, 0.0, -1.0, 1.0]]) 382 | Xi += matrix([[0.0], 383 | [0.0], 384 | [-Z2], 385 | [Z2]]) 386 | Omega.show('Omega: ') 387 | Xi.show('Xi: ') 388 | mu = Omega.inverse() * Xi 389 | mu.show('Mu: ') 390 | 391 | return mu 392 | 393 | doit(-3, 5, 3, 10, 5, 2) -------------------------------------------------------------------------------- /6 - SLAM/matrixExperiment.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 | res = aux.CholeskyInverse() 292 | return res 293 | 294 | # ------------ 295 | # 296 | # prints matrix (needs work!) 297 | # 298 | 299 | 300 | def __repr__(self): 301 | return repr(self.value) 302 | 303 | 304 | 305 | # ###################################################################### 306 | # ###################################################################### 307 | # ###################################################################### 308 | 309 | 310 | """ 311 | For the following example, you would call doit(-3, 5, 3): 312 | 3 robot positions 313 | initially: -3 314 | moves by 5 315 | moves by 3 316 | 317 | 318 | 319 | which should return a mu of: 320 | [[-3.0], 321 | [2.0], 322 | [5.0]] 323 | """ 324 | def doit(initial_pos, move1, move2): 325 | # 326 | # 327 | # Add your code here. 328 | # 329 | # 330 | omega = matrix([[[2],[0],[-1],[0]],[[-1],[0],[2],[0]]]) 331 | xi = matrix([[-8],[0],[2],[0]]) 332 | mu = omega.inverse() * xi 333 | print "omega:",omega 334 | print "xi:",xi 335 | print "mu:",mu 336 | 337 | return mu 338 | 339 | doit(-3, 5, 3) -------------------------------------------------------------------------------- /6 - SLAM/omegaAndXi.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 | res = aux.CholeskyInverse() 292 | return res 293 | 294 | # ------------ 295 | # 296 | # prints matrix (needs work!) 297 | # 298 | 299 | 300 | def __repr__(self): 301 | return repr(self.value) 302 | 303 | 304 | 305 | # ###################################################################### 306 | # ###################################################################### 307 | # ###################################################################### 308 | 309 | 310 | """ 311 | For the following example, you would call doit(-3, 5, 3): 312 | 3 robot positions 313 | initially: -3 314 | moves by 5 315 | moves by 3 316 | 317 | 318 | 319 | which should return a mu of: 320 | [[-3.0], 321 | [2.0], 322 | [5.0]] 323 | """ 324 | def doit(initial_pos, move1, move2): 325 | # 326 | # 327 | # Add your code here. 328 | # 329 | # 330 | omega = matrix([[2,-1,0],[-1,2,-1],[0,-1,1]]) 331 | xi = matrix([[-8],[2],[3]]) 332 | mu = omega.inverse() * xi 333 | print "omega:",omega 334 | print "xi:",xi 335 | print "mu:",mu 336 | 337 | # oops, I guess I hard coded the whole thing. Let's try again :) 338 | 339 | omega = matrix([[1,0,0],[0,0,0],[0,0,0]]) 340 | xi = matrix([[initial_pos],[0],[0]]) 341 | omega += matrix([[1,-1,0],[-1,1,0],[0,0,0]]) 342 | xi += matrix([[-move1],[move1],[0]]) 343 | omega += matrix([[0,0,0],[0,1,-1],[0,-1,1]]) 344 | xi += matrix([[0],[-move2],[move2]]) 345 | print "omega:",omega 346 | print "xi:",xi 347 | print "mu:",mu 348 | 349 | return mu 350 | 351 | doit(-3, 5, 3) -------------------------------------------------------------------------------- /Practice Exam/warehouseRobot.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 | directions = [[-1, 0], # Y,X clockwise starting at UP 80 | [-1, 1], 81 | [ 0, 1], 82 | [ 1, 1], 83 | [ 1, 0], 84 | [ 1,-1], 85 | [ 0,-1], 86 | [-1,-1]] 87 | costs = [1, 1.5, 1, 1.5, 1, 1.5, 1, 1.5] # cost for each direction 88 | cost = 0 89 | while len(todo) > 0: 90 | x0 = dropzone[1] # start location 91 | y0 = dropzone[0] 92 | xN = 99 # target location 93 | yN = 99 94 | currentTarget = todo.pop(0) 95 | for y in range(len(warehouse)): 96 | for x in range(len(warehouse[0])): 97 | if warehouse[y][x] == currentTarget: 98 | xN = x 99 | yN = y 100 | # A* 101 | h = [[99 for x in range(len(warehouse[0]))] for y in range(len(warehouse))] 102 | change = True 103 | while change: 104 | change = False 105 | for y in range(len(warehouse)): 106 | for x in range(len(warehouse[0])): 107 | if y == yN and x == xN and h[y][x] == 99: 108 | print x,y 109 | h[y][x] = 0 110 | change = True 111 | if warehouse[y][x] == 0 or (x == x0 and y == y0): 112 | for dir in range(len(directions)): 113 | x1 = x + directions[dir][1] 114 | y1 = y + directions[dir][0] 115 | if x1 >= 0 and x1 < len(warehouse[0]) and y1 >= 0 and y1 < len(warehouse): 116 | c1 = h[y1][x1] + costs[dir] 117 | if c1 < h[y][x]: 118 | h[y][x] = c1 119 | change = True 120 | 121 | cost += 2*h[y0][x0] 122 | warehouse[yN][xN] = 0 123 | 124 | return cost 125 | 126 | ################# TESTING ################## 127 | 128 | # ------------------------------------------ 129 | # solution check - Checks your plan function using 130 | # data from list called test[]. Uncomment the call 131 | # to solution_check to test your code. 132 | # 133 | def solution_check(test, epsilon = 0.00001): 134 | answer_list = [] 135 | 136 | import time 137 | start = time.clock() 138 | correct_answers = 0 139 | for i in range(len(test[0])): 140 | user_cost = plan(test[0][i], test[1][i], test[2][i]) 141 | true_cost = test[3][i] 142 | if abs(user_cost - true_cost) < epsilon: 143 | print "\nTest case", i+1, "passed!" 144 | answer_list.append(1) 145 | correct_answers += 1 146 | #print "#############################################" 147 | else: 148 | print "\nTest case ", i+1, "unsuccessful. Your answer ", user_cost, "was not within ", epsilon, "of ", true_cost 149 | answer_list.append(0) 150 | runtime = time.clock() - start 151 | if runtime > 1: 152 | print "Your code is too slow, try to optimize it! Running time was: ", runtime 153 | return False 154 | if correct_answers == len(answer_list): 155 | print "\nYou passed all test cases!" 156 | return True 157 | else: 158 | print "\nYou passed", correct_answers, "of", len(answer_list), "test cases. Try to get them all!" 159 | return False 160 | #Testing environment 161 | # Test Case 1 162 | warehouse1 = [[ 1, 2, 3], 163 | [ 0, 0, 0], 164 | [ 0, 0, 0]] 165 | dropzone1 = [2,0] 166 | todo1 = [2, 1] 167 | true_cost1 = 9 168 | # Test Case 2 169 | warehouse2 = [[ 1, 2, 3, 4], 170 | [ 0, 0, 0, 0], 171 | [ 5, 6, 7, 0], 172 | [ 'x', 0, 0, 8]] 173 | dropzone2 = [3,0] 174 | todo2 = [2, 5, 1] 175 | true_cost2 = 21 176 | 177 | # Test Case 3 178 | warehouse3 = [[ 1, 2, 3, 4, 5, 6, 7], 179 | [ 0, 0, 0, 0, 0, 0, 0], 180 | [ 8, 9, 10, 11, 0, 0, 0], 181 | [ 'x', 0, 0, 0, 0, 0, 12]] 182 | dropzone3 = [3,0] 183 | todo3 = [5, 10] 184 | true_cost3 = 18 185 | 186 | # Test Case 4 187 | warehouse4 = [[ 1, 17, 5, 18, 9, 19, 13], 188 | [ 2, 0, 6, 0, 10, 0, 14], 189 | [ 3, 0, 7, 0, 11, 0, 15], 190 | [ 4, 0, 8, 0, 12, 0, 16], 191 | [ 0, 0, 0, 0, 0, 0, 'x']] 192 | dropzone4 = [4,6] 193 | todo4 = [13, 11, 6, 17] 194 | true_cost4 = 41 195 | 196 | testing_suite = [[warehouse1, warehouse2, warehouse3, warehouse4], 197 | [dropzone1, dropzone2, dropzone3, dropzone4], 198 | [todo1, todo2, todo3, todo4], 199 | [true_cost1, true_cost2, true_cost3, true_cost4]] 200 | 201 | 202 | solution_check(testing_suite) #UNCOMMENT THIS LINE TO TEST YOUR CODE -------------------------------------------------------------------------------- /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) 137 | -------------------------------------------------------------------------------- /Project - Runaway Robot/matrix.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jeremy-shannon/udacity-AI-for-robotics/39d5fd210602973cab9a972a0ed1770fe2d50ba9/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/jeremy-shannon/udacity-AI-for-robotics/39d5fd210602973cab9a972a0ed1770fe2d50ba9/Project - Runaway Robot/robot.pyc -------------------------------------------------------------------------------- /Project - Runaway Robot/studentMain1-noiselessPrediction.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 | # assume constant velocity, and a constant change in angle 74 | 75 | if not OTHER: 76 | OTHER = [] 77 | avgdist = 0 78 | avgdtheta = 0 79 | OTHER.append(measurement) 80 | dist = [] 81 | theta = [] 82 | if len(OTHER) > 1: 83 | for i in range(len(OTHER)-1): 84 | dist.append(distance_between(OTHER[i+1],OTHER[i])) 85 | theta.append(atan2(OTHER[i+1][1]-OTHER[i][1], OTHER[i+1][0]-OTHER[i][0])) 86 | else: 87 | dist = [0] 88 | theta = [0] 89 | avgdist = sum(dist) / float(len(dist)) 90 | dtheta = [] 91 | if len(theta) > 1: 92 | for i in range(len(theta)-1): 93 | dtheta.append((theta[i+1]-theta[i])%(2*pi)) 94 | avgdtheta = sum(dtheta) / float(len(dtheta)) 95 | 96 | xy_estimate = (measurement[0]+avgdist*cos(theta[len(theta)-1]+avgdtheta), measurement[1]+avgdist*sin(theta[len(theta)-1]+avgdtheta)) 97 | 98 | print "meas:", measurement 99 | print "est:", xy_estimate 100 | 101 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 102 | # in this order for grading purposes. 103 | return xy_estimate, OTHER 104 | 105 | # A helper function you may find useful. 106 | def distance_between(point1, point2): 107 | """Computes distance between point1 and point2. Points are (x, y) pairs.""" 108 | x1, y1 = point1 109 | x2, y2 = point2 110 | return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) 111 | 112 | # This is here to give you a sense for how we will be running and grading 113 | # your code. Note that the OTHER variable allows you to store any 114 | # information that you want. 115 | def demo_grading3333(estimate_next_pos_fcn, target_bot, OTHER = None): 116 | localized = False 117 | distance_tolerance = 0.01 * target_bot.distance 118 | ctr = 0 119 | # if you haven't localized the target bot, make a guess about the next 120 | # position, then we move the bot and compare your guess to the true 121 | # next position. When you are close enough, we stop checking. 122 | while not localized and ctr <= 10: 123 | ctr += 1 124 | measurement = target_bot.sense() 125 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 126 | target_bot.move_in_circle() 127 | true_position = (target_bot.x, target_bot.y) 128 | error = distance_between(position_guess, true_position) 129 | print "error:", error 130 | print "distance tolerance:", distance_tolerance 131 | if error <= distance_tolerance: 132 | print "You got it right! It took you ", ctr, " steps to localize." 133 | localized = True 134 | if ctr == 10: 135 | print "Sorry, it took you too many steps to localize the target." 136 | return localized 137 | 138 | def demo_grading(estimate_next_pos_fcn, target_bot, OTHER = None): 139 | localized = False 140 | distance_tolerance = 0.01 * target_bot.distance 141 | ctr = 0 142 | # if you haven't localized the target bot, make a guess about the next 143 | # position, then we move the bot and compare your guess to the true 144 | # next position. When you are close enough, we stop checking. 145 | #For Visualization 146 | import turtle #You need to run this locally to use the turtle module 147 | window = turtle.Screen() 148 | window.bgcolor('white') 149 | size_multiplier= 25.0 #change Size of animation 150 | broken_robot = turtle.Turtle() 151 | broken_robot.shape('turtle') 152 | broken_robot.color('green') 153 | broken_robot.resizemode('user') 154 | broken_robot.shapesize(0.1, 0.1, 0.1) 155 | measured_broken_robot = turtle.Turtle() 156 | measured_broken_robot.shape('circle') 157 | measured_broken_robot.color('red') 158 | measured_broken_robot.resizemode('user') 159 | measured_broken_robot.shapesize(0.1, 0.1, 0.1) 160 | prediction = turtle.Turtle() 161 | prediction.shape('arrow') 162 | prediction.color('blue') 163 | prediction.resizemode('user') 164 | prediction.shapesize(0.1, 0.1, 0.1) 165 | prediction.penup() 166 | broken_robot.penup() 167 | measured_broken_robot.penup() 168 | #End of Visualization 169 | while not localized and ctr <= 10: 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 | if error <= distance_tolerance: 177 | print "You got it right! It took you ", ctr, " steps to localize." 178 | localized = True 179 | if ctr == 10: 180 | print "Sorry, it took you too many steps to localize the target." 181 | #More Visualization 182 | measured_broken_robot.setheading(target_bot.heading*180/pi) 183 | measured_broken_robot.goto(measurement[0]*size_multiplier, measurement[1]*size_multiplier-200) 184 | measured_broken_robot.stamp() 185 | broken_robot.setheading(target_bot.heading*180/pi) 186 | broken_robot.goto(target_bot.x*size_multiplier, target_bot.y*size_multiplier-200) 187 | broken_robot.stamp() 188 | prediction.setheading(target_bot.heading*180/pi) 189 | prediction.goto(position_guess[0]*size_multiplier, position_guess[1]*size_multiplier-200) 190 | prediction.stamp() 191 | #End of Visualization 192 | return localized 193 | # This is a demo for what a strategy could look like. This one isn't very good. 194 | def naive_next_pos(measurement, OTHER = None): 195 | """This strategy records the first reported position of the target and 196 | assumes that eventually the target bot will eventually return to that 197 | position, so it always guesses that the first position will be the next.""" 198 | if not OTHER: # this is the first measurement 199 | OTHER = measurement 200 | xy_estimate = OTHER 201 | print "naive meas:", measurement, "est:", xy_estimate 202 | return xy_estimate, OTHER 203 | 204 | # This is how we create a target bot. Check the robot.py file to understand 205 | # How the robot class behaves. 206 | test_target = robot(2.1, 4.3, 0.5, 2*pi / 34.0, 1.5) 207 | test_target.set_noise(0.0, 0.0, 0.0) 208 | 209 | demo_grading(estimate_next_pos, test_target) -------------------------------------------------------------------------------- /Project - Runaway Robot/studentMain2A-addingNoise(sortaWorking).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 | 34 | # This is the function you have to write. Note that measurement is a 35 | # single (x, y) point. This function will have to be called multiple 36 | # times before you have enough information to accurately predict the 37 | # next position. The OTHER variable that your function returns will be 38 | # passed back to your function the next time it is called. You can use 39 | # this to keep track of important information over time. 40 | ######################### CARTESIAN ################################### 41 | def estimate_next_pos2(measurement, OTHER = None): 42 | """Estimate the next (x, y) position of the wandering Traxbot 43 | based on noisy (x, y) measurements.""" 44 | 45 | #print "meas:", measurement 46 | 47 | x1 = measurement[0] 48 | y1 = measurement[1] 49 | 50 | if not OTHER: 51 | OTHER = [[],[],[]] 52 | # inital state matrix (polar location and angular velocity) 53 | x = matrix([[0.], [0.], [0.], [0.], [0.], [0.]]) 54 | # initial uncertainty: x, y, vx, vy, ax, ay 55 | P = matrix([[1000.,0.,0.,0.,0.,0.], 56 | [0.,1000.,0.,0.,0.,0.], 57 | [0.,0.,1000.,0.,0.,0.], 58 | [0.,0.,0.,1000.,0.,0.], 59 | [0.,0.,0.,0.,1000.,0.], 60 | [0.,0.,0.,0.,0.,1000.]]) 61 | else: 62 | # pull state variables (x) and uncertainty (P) from OTHER 63 | x = OTHER[0] 64 | P = OTHER[1] 65 | 66 | OTHER[2].append(measurement) 67 | 68 | # assume dt = 1 69 | dt = 1. 70 | # external motion 71 | u = matrix([[0.], [0.], [0.], [0.], [0.], [0.]]) 72 | # next state function: 73 | F = matrix([[1.,0.,dt,0.,0.,0.], 74 | [0.,1.,0.,dt,0.,0.], 75 | [0.,0.,1.,0.,dt,0.], 76 | [0.,0.,0.,1.,0.,dt], 77 | [0.,0.,0.,0.,1.,0.], # constant acceleration? 78 | [0.,0.,0.,0.,0.,1.]]) 79 | # measurement function: 80 | H = matrix([[1.,0.,0.,0.,0.,0.], 81 | [0.,1.,0.,0.,0.,0.]]) 82 | # measurement uncertainty: 83 | R = matrix([[.015,0.], 84 | [0.,.015]]) 85 | # 5d identity matrix 86 | I = matrix([[]]) 87 | I.identity(6) 88 | 89 | for i in range(len(OTHER[2])): 90 | # prediction 91 | x = (F * x) + u 92 | P = F * P * F.transpose() 93 | 94 | # measurement update 95 | Z = matrix([[OTHER[2][i][0],OTHER[2][i][1]]]) 96 | y = Z.transpose() - (H * x) 97 | S = H * P * H.transpose() + R 98 | K = P * H.transpose() * S.inverse() 99 | x = x + (K * y) 100 | P = (I - (K * H)) * P 101 | 102 | OTHER[0] = x 103 | OTHER[1] = P 104 | 105 | #print "x:" 106 | #x.show() 107 | #print "P:" 108 | #P.show() 109 | 110 | xy_estimate = (x.value[0][0], x.value[1][0]) 111 | print xy_estimate 112 | 113 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 114 | # in this order for grading purposes. 115 | return xy_estimate, OTHER 116 | 117 | ######################### POLAR ################################### 118 | def estimate_next_pos(measurement, OTHER = None): 119 | """Estimate the next (x, y) position of the wandering Traxbot 120 | based on noisy (x, y) measurements.""" 121 | 122 | #print "meas:", measurement 123 | 124 | x1 = measurement[0] 125 | y1 = measurement[1] 126 | 127 | if not OTHER: 128 | OTHER = [[],[],[],[]] 129 | # inital guesses: 130 | x0 = 0. 131 | y0 = 0. 132 | dist0 = 0. 133 | theta0 = 0. 134 | dtheta0 = 0. 135 | # initial uncertainty: 136 | P = matrix([[1000.,0.,0.], 137 | [0.,1000.,0.], 138 | [0.,0.,1000.]]) 139 | else: 140 | # pull previous measurement, state variables (x), and uncertainty (P) from OTHER 141 | lastMeasurement = OTHER[0][len(OTHER[0])-1] 142 | x0 = lastMeasurement[0] 143 | y0 = lastMeasurement[1] 144 | dist0 = OTHER[1].value[0][0] 145 | theta0 = OTHER[1].value[1][0] % (2*pi) 146 | dtheta0 = OTHER[1].value[2][0] 147 | P = OTHER[2] 148 | 149 | # convert measurement to dist and theta (based on previous estimate) 150 | dist = distance_between([x1,y1],[x0,y0]) 151 | theta = atan2(y1-y0,x1-x0) 152 | 153 | # time step 154 | dt = 1. 155 | 156 | # state matrix (polar location and angular velocity) 157 | x = matrix([[dist0], [theta0], [dtheta0]]) 158 | # external motion 159 | u = matrix([[0.], [0.], [0.]]) 160 | # next state function: 161 | F = matrix([[1.,0.,0.], 162 | [0.,1.,dt], # theta is the only thing that should change, by dtheta 163 | [0.,0.,1.]]) 164 | # measurement function: 165 | H = matrix([[1.,0.,0.], 166 | [0.,1.,0.]]) 167 | # measurement uncertainty: 168 | R = matrix([[.05,0.], 169 | [0.,.05]]) 170 | # 5d identity matrix 171 | I = matrix([[]]) 172 | I.identity(3) 173 | 174 | OTHER[3].append([dist,theta]) 175 | 176 | #for i in range(len(OTHER[3])): 177 | # prediction 178 | x = (F * x) + u 179 | P = F * P * F.transpose() 180 | 181 | # measurement update 182 | #Z = matrix([[OTHER[3][i][0],OTHER[3][i][1]]]) 183 | Z = matrix([[dist,theta]]) 184 | y = Z.transpose() - (H * x) 185 | S = H * P * H.transpose() + R 186 | K = P * H.transpose() * S.inverse() 187 | x = x + (K * y) 188 | P = (I - (K * H)) * P 189 | 190 | OTHER[0].append(measurement) 191 | OTHER[1] = x 192 | OTHER[2] = P 193 | 194 | #print "x:" 195 | #x.show() 196 | #print "P:" 197 | #P.show() 198 | 199 | xy_estimate = (x1+x.value[0][0]*cos((x.value[1][0]+x.value[2][0])%(2*pi)), 200 | y1+x.value[0][0]*sin((x.value[1][0]+x.value[2][0])%(2*pi))) 201 | #xy_estimate = (x1+x.value[0][0]*cos((x.value[1][0])), 202 | # y1+x.value[0][0]*sin((x.value[1][0]))) 203 | #print xy_estimate 204 | 205 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 206 | # in this order for grading purposes. 207 | return xy_estimate, OTHER 208 | 209 | 210 | # A helper function you may find useful. 211 | def distance_between(point1, point2): 212 | """Computes distance between point1 and point2. Points are (x, y) pairs.""" 213 | x1, y1 = point1 214 | x2, y2 = point2 215 | return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) 216 | 217 | # This is here to give you a sense for how we will be running and grading 218 | # your code. Note that the OTHER variable allows you to store any 219 | # information that you want. 220 | def demo_grading(estimate_next_pos_fcn, target_bot, OTHER = None): 221 | localized = False 222 | distance_tolerance = 0.01 * target_bot.distance 223 | ctr = 0 224 | # if you haven't localized the target bot, make a guess about the next 225 | # position, then we move the bot and compare your guess to the true 226 | # next position. When you are close enough, we stop checking. 227 | while not localized and ctr <= 1000: 228 | ctr += 1 229 | measurement = target_bot.sense() 230 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 231 | target_bot.move_in_circle() 232 | true_position = (target_bot.x, target_bot.y) 233 | error = distance_between(position_guess, true_position) 234 | if error <= distance_tolerance: 235 | print "You got it right! It took you ", ctr, " steps to localize." 236 | localized = True 237 | if ctr == 1000: 238 | print "Sorry, it took you too many steps to localize the target." 239 | return localized 240 | 241 | # This is a demo for what a strategy could look like. This one isn't very good. 242 | def naive_next_pos(measurement, OTHER = None): 243 | """This strategy records the first reported position of the target and 244 | assumes that eventually the target bot will eventually return to that 245 | position, so it always guesses that the first position will be the next.""" 246 | if not OTHER: # this is the first measurement 247 | OTHER = measurement 248 | xy_estimate = OTHER 249 | return xy_estimate, OTHER 250 | 251 | def demo_grading_visual(estimate_next_pos_fcn, target_bot, OTHER = None): 252 | localized = False 253 | distance_tolerance = 0.01 * target_bot.distance 254 | ctr = 0 255 | # if you haven't localized the target bot, make a guess about the next 256 | # position, then we move the bot and compare your guess to the true 257 | # next position. When you are close enough, we stop checking. 258 | #For Visualization 259 | import turtle #You need to run this locally to use the turtle module 260 | window = turtle.Screen() 261 | window.bgcolor('white') 262 | size_multiplier= 25.0 #change Size of animation 263 | broken_robot = turtle.Turtle() 264 | broken_robot.shape('turtle') 265 | broken_robot.color('green') 266 | broken_robot.resizemode('user') 267 | broken_robot.shapesize(0.1, 0.1, 0.1) 268 | measured_broken_robot = turtle.Turtle() 269 | measured_broken_robot.shape('circle') 270 | measured_broken_robot.color('red') 271 | measured_broken_robot.resizemode('user') 272 | measured_broken_robot.shapesize(0.1, 0.1, 0.1) 273 | prediction = turtle.Turtle() 274 | prediction.shape('arrow') 275 | prediction.color('blue') 276 | prediction.resizemode('user') 277 | prediction.shapesize(0.1, 0.1, 0.1) 278 | prediction.penup() 279 | broken_robot.penup() 280 | measured_broken_robot.penup() 281 | #End of Visualization 282 | while not localized and ctr <= 1000: 283 | ctr += 1 284 | measurement = target_bot.sense() 285 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 286 | target_bot.move_in_circle() 287 | true_position = (target_bot.x, target_bot.y) 288 | error = distance_between(position_guess, true_position) 289 | if error <= distance_tolerance: 290 | print "You got it right! It took you ", ctr, " steps to localize." 291 | localized = True 292 | if ctr == 1000: 293 | print "Sorry, it took you too many steps to localize the target." 294 | #More Visualization 295 | measured_broken_robot.setheading(target_bot.heading*180/pi) 296 | measured_broken_robot.goto(measurement[0]*size_multiplier, measurement[1]*size_multiplier-200) 297 | measured_broken_robot.stamp() 298 | broken_robot.setheading(target_bot.heading*180/pi) 299 | broken_robot.goto(target_bot.x*size_multiplier, target_bot.y*size_multiplier-200) 300 | broken_robot.stamp() 301 | prediction.setheading(target_bot.heading*180/pi) 302 | prediction.goto(position_guess[0]*size_multiplier, position_guess[1]*size_multiplier-200) 303 | prediction.stamp() 304 | #End of Visualization 305 | return localized 306 | 307 | 308 | # This is how we create a target bot. Check the robot.py file to understand 309 | # How the robot class behaves. 310 | test_target = robot(2.1, 4.3, 0.5, 2*pi / 34.0, 1.5) 311 | measurement_noise = 0.05 * test_target.distance 312 | test_target.set_noise(0.0, 0.0, measurement_noise) 313 | 314 | demo_grading_visual(estimate_next_pos, test_target) 315 | 316 | 317 | 318 | 319 | -------------------------------------------------------------------------------- /Project - Runaway Robot/studentMain2B-addingNoise(polar).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 | 34 | # This is the function you have to write. Note that measurement is a 35 | # single (x, y) point. This function will have to be called multiple 36 | # times before you have enough information to accurately predict the 37 | # next position. The OTHER variable that your function returns will be 38 | # passed back to your function the next time it is called. You can use 39 | # this to keep track of important information over time. 40 | ######################### CARTESIAN ################################### 41 | def estimate_next_pos2(measurement, OTHER = None): 42 | """Estimate the next (x, y) position of the wandering Traxbot 43 | based on noisy (x, y) measurements.""" 44 | 45 | #print "meas:", measurement 46 | 47 | x1 = measurement[0] 48 | y1 = measurement[1] 49 | 50 | if not OTHER: 51 | OTHER = [[],[]] 52 | # inital state matrix (polar location and angular velocity) 53 | x = matrix([[0.], [0.], [0.], [0.], [0.], [0.]]) 54 | # initial uncertainty: x, y, vx, vy, ax, ay 55 | P = matrix([[1000.,0.,0.,0.,0.,0.], 56 | [0.,1000.,0.,0.,0.,0.], 57 | [0.,0.,1000.,0.,0.,0.], 58 | [0.,0.,0.,1000.,0.,0.], 59 | [0.,0.,0.,0.,1000.,0.], 60 | [0.,0.,0.,0.,0.,1000.]]) 61 | else: 62 | # pull state variables (x) and uncertainty (P) from OTHER 63 | x = OTHER[0] 64 | P = OTHER[1] 65 | 66 | # assume dt = 1 67 | dt = 1.5 68 | # external motion 69 | u = matrix([[0.], [0.], [0.], [0.], [0.], [0.]]) 70 | # next state function: 71 | F = matrix([[1.,0.,dt,0.,0.,0.], 72 | [0.,1.,0.,dt,0.,0.], 73 | [0.,0.,1.,0.,dt,0.], 74 | [0.,0.,0.,1.,0.,dt], 75 | [0.,0.,0.,0.,1.,0.], # constant acceleration? 76 | [0.,0.,0.,0.,0.,1.]]) 77 | # measurement function: 78 | H = matrix([[1.,0.,0.,0.,0.,0.], 79 | [0.,1.,0.,0.,0.,0.]]) 80 | # measurement uncertainty: 81 | R = matrix([[.015,0.], 82 | [0.,.015]]) 83 | # 5d identity matrix 84 | I = matrix([[]]) 85 | I.identity(6) 86 | 87 | 88 | # prediction 89 | x = (F * x) + u 90 | P = F * P * F.transpose() 91 | 92 | # measurement update 93 | Z = matrix([[x1,y1]]) 94 | y = Z.transpose() - (H * x) 95 | S = H * P * H.transpose() + R 96 | K = P * H.transpose() * S.inverse() 97 | x = x + (K * y) 98 | P = (I - (K * H)) * P 99 | 100 | OTHER[0] = x 101 | OTHER[1] = P 102 | 103 | #print "x:" 104 | #x.show() 105 | #print "P:" 106 | #P.show() 107 | 108 | xy_estimate = (x.value[0][0], x.value[1][0]) 109 | print xy_estimate 110 | 111 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 112 | # in this order for grading purposes. 113 | return xy_estimate, OTHER 114 | 115 | ######################### POLAR ################################### 116 | def estimate_next_pos(measurement, OTHER = None): 117 | """Estimate the next (x, y) position of the wandering Traxbot 118 | based on noisy (x, y) measurements.""" 119 | 120 | #print "meas:", measurement 121 | 122 | x1 = measurement[0] 123 | y1 = measurement[1] 124 | 125 | if not OTHER: 126 | OTHER = [[],[]] 127 | # inital guesses: 128 | x0 = 0. # x0, y0 = center point 129 | y0 = 0. 130 | r = 0. 131 | theta = 0. 132 | dtheta = 0. 133 | # initial uncertainty: 134 | P = matrix([[1000.,0.,0.,0.,0.], 135 | [0.,1000.,0.,0.,0.], 136 | [0.,0.,1000.,0.,0.], 137 | [0.,0.,0.,1000.,0.], 138 | [0.,0.,0.,0.,1000.]]) 139 | else: 140 | # pull state variables (x) and uncertainty (P) from OTHER 141 | x0 = OTHER[0].value[0][0] 142 | y0 = OTHER[0].value[1][0] 143 | r = OTHER[0].value[2][0] 144 | theta = OTHER[0].value[3][0] % (2*pi) 145 | dtheta = OTHER[0].value[4][0] 146 | P = OTHER[1] 147 | 148 | # convert measurement to polar coordinates (based on previous estimate) 149 | x01 = x1 - r * cos((theta+dtheta)%(2*pi)) 150 | y01 = y1 - r * sin((theta+dtheta)%(2*pi)) 151 | r1 = distance_between([x1,y1],[x0,y0]) 152 | theta1 = atan2(y1-y0,x1-x0) 153 | 154 | # time step 155 | dt = 1. 156 | 157 | # state matrix (polar location and angular velocity) 158 | x = matrix([[x0], [y0], [r], [theta], [dtheta]]) 159 | # external motion 160 | u = matrix([[0.], [0.], [0.], [0.], [0.]]) 161 | # next state function: 162 | F = matrix([[1.,0.,0.,0.,0.], 163 | [0.,1.,0.,0.,0.], 164 | [0.,0.,1.,0.,0.], 165 | [0.,0.,0.,1.,dt], # theta is the only thing that should change, by dtheta 166 | [0.,0.,0.,0.,1.]]) 167 | # measurement function: 168 | H = matrix([[1.,0.,0.,0.,0.], 169 | [0.,1.,0.,0.,0.], 170 | [0.,0.,1.,0.,0.], 171 | [0.,0.,0.,1.,0.]]) 172 | # measurement uncertainty: 173 | R = matrix([[.015,0.,0.,0.], 174 | [0.,.015,0.,0.], 175 | [0.,0.,.015,0.], 176 | [0.,0.,0.,.015]]) 177 | # 5d identity matrix 178 | I = matrix([[]]) 179 | I.identity(5) 180 | 181 | 182 | # prediction 183 | x = (F * x) + u 184 | P = F * P * F.transpose() 185 | 186 | # measurement update 187 | Z = matrix([[x01,y01,r1,theta1]]) 188 | y = Z.transpose() - (H * x) 189 | S = H * P * H.transpose() + R 190 | K = P * H.transpose() * S.inverse() 191 | x = x + (K * y) 192 | P = (I - (K * H)) * P 193 | 194 | OTHER[0] = x 195 | OTHER[1] = P 196 | 197 | #print "x:" 198 | #x.show() 199 | #print "P:" 200 | #P.show() 201 | 202 | xy_estimate = (x.value[0][0]+x.value[2][0]*cos((x.value[3][0]+x.value[4][0])%(2*pi)), 203 | x.value[1][0]+x.value[2][0]*sin((x.value[3][0]+x.value[4][0])%(2*pi))) 204 | print xy_estimate 205 | 206 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 207 | # in this order for grading purposes. 208 | return xy_estimate, OTHER 209 | 210 | 211 | # A helper function you may find useful. 212 | def distance_between(point1, point2): 213 | """Computes distance between point1 and point2. Points are (x, y) pairs.""" 214 | x1, y1 = point1 215 | x2, y2 = point2 216 | return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) 217 | 218 | # This is here to give you a sense for how we will be running and grading 219 | # your code. Note that the OTHER variable allows you to store any 220 | # information that you want. 221 | def demo_grading(estimate_next_pos_fcn, target_bot, OTHER = None): 222 | localized = False 223 | distance_tolerance = 0.01 * target_bot.distance 224 | ctr = 0 225 | # if you haven't localized the target bot, make a guess about the next 226 | # position, then we move the bot and compare your guess to the true 227 | # next position. When you are close enough, we stop checking. 228 | while not localized and ctr <= 1000: 229 | ctr += 1 230 | measurement = target_bot.sense() 231 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 232 | target_bot.move_in_circle() 233 | true_position = (target_bot.x, target_bot.y) 234 | error = distance_between(position_guess, true_position) 235 | if error <= distance_tolerance: 236 | print "You got it right! It took you ", ctr, " steps to localize." 237 | localized = True 238 | if ctr == 1000: 239 | print "Sorry, it took you too many steps to localize the target." 240 | return localized 241 | 242 | # This is a demo for what a strategy could look like. This one isn't very good. 243 | def naive_next_pos(measurement, OTHER = None): 244 | """This strategy records the first reported position of the target and 245 | assumes that eventually the target bot will eventually return to that 246 | position, so it always guesses that the first position will be the next.""" 247 | if not OTHER: # this is the first measurement 248 | OTHER = measurement 249 | xy_estimate = OTHER 250 | return xy_estimate, OTHER 251 | 252 | def demo_grading_visual(estimate_next_pos_fcn, target_bot, OTHER = None): 253 | localized = False 254 | distance_tolerance = 0.01 * target_bot.distance 255 | ctr = 0 256 | # if you haven't localized the target bot, make a guess about the next 257 | # position, then we move the bot and compare your guess to the true 258 | # next position. When you are close enough, we stop checking. 259 | #For Visualization 260 | import turtle #You need to run this locally to use the turtle module 261 | window = turtle.Screen() 262 | window.bgcolor('white') 263 | size_multiplier= 25.0 #change Size of animation 264 | broken_robot = turtle.Turtle() 265 | broken_robot.shape('turtle') 266 | broken_robot.color('green') 267 | broken_robot.resizemode('user') 268 | broken_robot.shapesize(0.1, 0.1, 0.1) 269 | measured_broken_robot = turtle.Turtle() 270 | measured_broken_robot.shape('circle') 271 | measured_broken_robot.color('red') 272 | measured_broken_robot.resizemode('user') 273 | measured_broken_robot.shapesize(0.1, 0.1, 0.1) 274 | prediction = turtle.Turtle() 275 | prediction.shape('arrow') 276 | prediction.color('blue') 277 | prediction.resizemode('user') 278 | prediction.shapesize(0.1, 0.1, 0.1) 279 | prediction.penup() 280 | broken_robot.penup() 281 | measured_broken_robot.penup() 282 | #End of Visualization 283 | while not localized and ctr <= 1000: 284 | ctr += 1 285 | measurement = target_bot.sense() 286 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 287 | target_bot.move_in_circle() 288 | true_position = (target_bot.x, target_bot.y) 289 | error = distance_between(position_guess, true_position) 290 | if error <= distance_tolerance: 291 | print "You got it right! It took you ", ctr, " steps to localize." 292 | localized = True 293 | if ctr == 1000: 294 | print "Sorry, it took you too many steps to localize the target." 295 | #More Visualization 296 | measured_broken_robot.setheading(target_bot.heading*180/pi) 297 | measured_broken_robot.goto(measurement[0]*size_multiplier, measurement[1]*size_multiplier-200) 298 | measured_broken_robot.stamp() 299 | broken_robot.setheading(target_bot.heading*180/pi) 300 | broken_robot.goto(target_bot.x*size_multiplier, target_bot.y*size_multiplier-200) 301 | broken_robot.stamp() 302 | prediction.setheading(target_bot.heading*180/pi) 303 | prediction.goto(position_guess[0]*size_multiplier, position_guess[1]*size_multiplier-200) 304 | prediction.stamp() 305 | #End of Visualization 306 | return localized 307 | 308 | 309 | # This is how we create a target bot. Check the robot.py file to understand 310 | # How the robot class behaves. 311 | test_target = robot(2.1, 4.3, 0.5, 2*pi / 34.0, 1.5) 312 | measurement_noise = 0.05 * test_target.distance 313 | test_target.set_noise(0.0, 0.0, measurement_noise) 314 | 315 | demo_grading_visual(estimate_next_pos, test_target) 316 | 317 | 318 | 319 | 320 | -------------------------------------------------------------------------------- /Project - Runaway Robot/studentMain2C-addingNoise(my solution).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 | 34 | # This is the function you have to write. Note that measurement is a 35 | # single (x, y) point. This function will have to be called multiple 36 | # times before you have enough information to accurately predict the 37 | # next position. The OTHER variable that your function returns will be 38 | # passed back to your function the next time it is called. You can use 39 | # this to keep track of important information over time. 40 | ######################### CARTESIAN ################################### 41 | def estimate_next_pos2(measurement, OTHER = None): 42 | """Estimate the next (x, y) position of the wandering Traxbot 43 | based on noisy (x, y) measurements.""" 44 | 45 | #print "meas:", measurement 46 | 47 | x1 = measurement[0] 48 | y1 = measurement[1] 49 | 50 | if not OTHER: 51 | OTHER = [[],[]] 52 | # inital state matrix (polar location and angular velocity) 53 | x = matrix([[0.], [0.], [0.], [0.], [0.], [0.]]) 54 | # initial uncertainty: x, y, vx, vy, ax, ay 55 | P = matrix([[1000.,0.,0.,0.,0.,0.], 56 | [0.,1000.,0.,0.,0.,0.], 57 | [0.,0.,1000.,0.,0.,0.], 58 | [0.,0.,0.,1000.,0.,0.], 59 | [0.,0.,0.,0.,1000.,0.], 60 | [0.,0.,0.,0.,0.,1000.]]) 61 | else: 62 | # pull state variables (x) and uncertainty (P) from OTHER 63 | x = OTHER[0] 64 | P = OTHER[1] 65 | 66 | # assume dt = 1 67 | dt = 1.5 68 | # external motion 69 | u = matrix([[0.], [0.], [0.], [0.], [0.], [0.]]) 70 | # next state function: 71 | F = matrix([[1.,0.,dt,0.,0.,0.], 72 | [0.,1.,0.,dt,0.,0.], 73 | [0.,0.,1.,0.,dt,0.], 74 | [0.,0.,0.,1.,0.,dt], 75 | [0.,0.,0.,0.,1.,0.], # constant acceleration? 76 | [0.,0.,0.,0.,0.,1.]]) 77 | # measurement function: 78 | H = matrix([[1.,0.,0.,0.,0.,0.], 79 | [0.,1.,0.,0.,0.,0.]]) 80 | # measurement uncertainty: 81 | R = matrix([[.015,0.], 82 | [0.,.015]]) 83 | # 5d identity matrix 84 | I = matrix([[]]) 85 | I.identity(6) 86 | 87 | 88 | # prediction 89 | x = (F * x) + u 90 | P = F * P * F.transpose() 91 | 92 | # measurement update 93 | Z = matrix([[x1,y1]]) 94 | y = Z.transpose() - (H * x) 95 | S = H * P * H.transpose() + R 96 | K = P * H.transpose() * S.inverse() 97 | x = x + (K * y) 98 | P = (I - (K * H)) * P 99 | 100 | OTHER[0] = x 101 | OTHER[1] = P 102 | 103 | #print "x:" 104 | #x.show() 105 | #print "P:" 106 | #P.show() 107 | 108 | xy_estimate = (x.value[0][0], x.value[1][0]) 109 | print xy_estimate 110 | 111 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 112 | # in this order for grading purposes. 113 | return xy_estimate, OTHER 114 | 115 | ######################### POLAR ################################### 116 | def estimate_next_pos(measurement, OTHER = None): 117 | """Estimate the next (x, y) position of the wandering Traxbot 118 | based on noisy (x, y) measurements.""" 119 | 120 | print "meas:", measurement 121 | 122 | x1 = measurement[0] 123 | y1 = measurement[1] 124 | 125 | if not OTHER: 126 | OTHER = [[],[],[]] 127 | # inital guesses: 128 | x0 = 0. 129 | y0 = 0. 130 | dist0 = 0. 131 | theta0 = 0. 132 | dtheta0 = 0. 133 | # initial uncertainty: 134 | P = matrix([[1000.,0.,0.,0.,0.], 135 | [0.,1000.,0.,0.,0.], 136 | [0.,0.,1000.,0.,0.], 137 | [0.,0.,0.,1000.,0.], 138 | [0.,0.,0.,0.,1000.]]) 139 | else: 140 | # pull previous measurement, state variables (x), and uncertainty (P) from OTHER 141 | x0 = OTHER[0].value[0][0] 142 | y0 = OTHER[0].value[1][0] 143 | dist0 = OTHER[0].value[2][0] 144 | theta0 = OTHER[0].value[3][0] % (2*pi) 145 | dtheta0 = OTHER[0].value[4][0] 146 | P = OTHER[1] 147 | 148 | # time step 149 | dt = 1. 150 | 151 | # state matrix (polar location and angular velocity) 152 | x = matrix([[x0],[y0],[dist0],[theta0],[dtheta0]]) 153 | # external motion 154 | u = matrix([[0.], [0.], [0.], [0.], [0.]]) 155 | 156 | # measurement function: 157 | # for the EKF this should be the Jacobian of H, but in this case it turns out to be the same (?) 158 | H = matrix([[1.,0.,0.,0.,0.], 159 | [0.,1.,0.,0.,0.]]) 160 | # measurement uncertainty: 161 | R = matrix([[measurement_noise,0.], 162 | [0.,measurement_noise]]) 163 | # 5d identity matrix 164 | I = matrix([[]]) 165 | I.identity(5) 166 | 167 | 168 | # measurement update 169 | Z = matrix([[x1,y1]]) 170 | y = Z.transpose() - (H * x) 171 | S = H * P * H.transpose() + R 172 | K = P * H.transpose() * S.inverse() 173 | x = x + (K * y) 174 | P = (I - (K * H)) * P 175 | 176 | # pull out current estimates based on measurement 177 | # this was a big part of what was hainging me up (I was using older estimates before) 178 | x0 = x.value[0][0] 179 | y0 = x.value[1][0] 180 | dist0 = x.value[2][0] 181 | theta0 = x.value[3][0] 182 | dtheta0 = x.value[4][0] 183 | 184 | # next state function: 185 | # this is now the Jacobian of the transition matrix (F) from the regular Kalman Filter 186 | A = matrix([[1.,0.,cos(theta0+dtheta0),-dist0*sin(theta0+dtheta0),-dist0*sin(theta0+dtheta0)], 187 | [0.,1.,sin(theta0+dtheta0),dist0*cos(theta0+dtheta0),dist0*cos(theta0+dtheta0)], 188 | [0.,0.,1.,0.,0.], 189 | [0.,0.,0.,1.,dt], 190 | [0.,0.,0.,0.,1.]]) 191 | 192 | # calculate new estimate 193 | # it's NOT simply the matrix multiplication of transition matrix and estimated state vector 194 | # for the EKF just use the state transition formulas the transition matrix was built from 195 | x = matrix([[x0 + dist0 * cos(theta0 + dtheta0)], 196 | [y0 + dist0 * sin(theta0 + dtheta0)], 197 | [dist0], 198 | [theta0 + dtheta0], 199 | [dtheta0]]) 200 | 201 | # prediction 202 | # x = (F * x) + u 203 | P = A * P * A.transpose() 204 | 205 | OTHER[0] = x 206 | OTHER[1] = P 207 | 208 | #print "x:" 209 | #x.show() 210 | #print "P:" 211 | #P.show() 212 | 213 | xy_estimate = (x.value[0][0], x.value[1][0]) 214 | #xy_estimate = (x1+x.value[0][0]*cos((x.value[1][0])), 215 | # y1+x.value[0][0]*sin((x.value[1][0]))) 216 | print xy_estimate 217 | 218 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 219 | # in this order for grading purposes. 220 | return xy_estimate, OTHER 221 | 222 | 223 | # A helper function you may find useful. 224 | def distance_between(point1, point2): 225 | """Computes distance between point1 and point2. Points are (x, y) pairs.""" 226 | x1, y1 = point1 227 | x2, y2 = point2 228 | return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) 229 | 230 | # This is here to give you a sense for how we will be running and grading 231 | # your code. Note that the OTHER variable allows you to store any 232 | # information that you want. 233 | def demo_grading(estimate_next_pos_fcn, target_bot, OTHER = None): 234 | localized = False 235 | distance_tolerance = 0.01 * target_bot.distance 236 | ctr = 0 237 | # if you haven't localized the target bot, make a guess about the next 238 | # position, then we move the bot and compare your guess to the true 239 | # next position. When you are close enough, we stop checking. 240 | while not localized and ctr <= 1000: 241 | ctr += 1 242 | measurement = target_bot.sense() 243 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 244 | target_bot.move_in_circle() 245 | true_position = (target_bot.x, target_bot.y) 246 | error = distance_between(position_guess, true_position) 247 | if error <= distance_tolerance: 248 | print "You got it right! It took you ", ctr, " steps to localize." 249 | localized = True 250 | if ctr == 1000: 251 | print "Sorry, it took you too many steps to localize the target." 252 | return localized 253 | 254 | # This is a demo for what a strategy could look like. This one isn't very good. 255 | def naive_next_pos(measurement, OTHER = None): 256 | """This strategy records the first reported position of the target and 257 | assumes that eventually the target bot will eventually return to that 258 | position, so it always guesses that the first position will be the next.""" 259 | if not OTHER: # this is the first measurement 260 | OTHER = measurement 261 | xy_estimate = OTHER 262 | return xy_estimate, OTHER 263 | 264 | def demo_grading_visual(estimate_next_pos_fcn, target_bot, OTHER = None): 265 | localized = False 266 | distance_tolerance = 0.01 * target_bot.distance 267 | ctr = 0 268 | # if you haven't localized the target bot, make a guess about the next 269 | # position, then we move the bot and compare your guess to the true 270 | # next position. When you are close enough, we stop checking. 271 | #For Visualization 272 | import turtle #You need to run this locally to use the turtle module 273 | window = turtle.Screen() 274 | window.bgcolor('white') 275 | size_multiplier= 25.0 #change Size of animation 276 | broken_robot = turtle.Turtle() 277 | broken_robot.shape('turtle') 278 | broken_robot.color('green') 279 | broken_robot.resizemode('user') 280 | broken_robot.shapesize(0.1, 0.1, 0.1) 281 | measured_broken_robot = turtle.Turtle() 282 | measured_broken_robot.shape('circle') 283 | measured_broken_robot.color('red') 284 | measured_broken_robot.resizemode('user') 285 | measured_broken_robot.shapesize(0.1, 0.1, 0.1) 286 | prediction = turtle.Turtle() 287 | prediction.shape('arrow') 288 | prediction.color('blue') 289 | prediction.resizemode('user') 290 | prediction.shapesize(0.1, 0.1, 0.1) 291 | prediction.penup() 292 | broken_robot.penup() 293 | measured_broken_robot.penup() 294 | #End of Visualization 295 | while not localized and ctr <= 1000: 296 | ctr += 1 297 | measurement = target_bot.sense() 298 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 299 | target_bot.move_in_circle() 300 | true_position = (target_bot.x, target_bot.y) 301 | error = distance_between(position_guess, true_position) 302 | if error <= distance_tolerance: 303 | print "You got it right! It took you ", ctr, " steps to localize." 304 | localized = True 305 | if ctr == 1000: 306 | print "Sorry, it took you too many steps to localize the target." 307 | #More Visualization 308 | measured_broken_robot.setheading(target_bot.heading*180/pi) 309 | measured_broken_robot.goto(measurement[0]*size_multiplier, measurement[1]*size_multiplier-200) 310 | measured_broken_robot.stamp() 311 | broken_robot.setheading(target_bot.heading*180/pi) 312 | broken_robot.goto(target_bot.x*size_multiplier, target_bot.y*size_multiplier-200) 313 | broken_robot.stamp() 314 | prediction.setheading(target_bot.heading*180/pi) 315 | prediction.goto(position_guess[0]*size_multiplier, position_guess[1]*size_multiplier-200) 316 | prediction.stamp() 317 | #End of Visualization 318 | return localized 319 | 320 | 321 | # This is how we create a target bot. Check the robot.py file to understand 322 | # How the robot class behaves. 323 | test_target = robot(2.1, 4.3, 0.5, 2*pi / 34.0, 1.5) 324 | measurement_noise = 2.0 * test_target.distance 325 | test_target.set_noise(0.0, 0.0, measurement_noise) 326 | print "measurement_noise:" , measurement_noise 327 | demo_grading(estimate_next_pos, test_target) 328 | 329 | 330 | 331 | 332 | -------------------------------------------------------------------------------- /Project - Runaway Robot/studentMainExample.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 matrix import * # Check the matrix.py tab to see how this works. 31 | 32 | 33 | # This is the function you have to write. Note that measurement is a 34 | # single (x, y) point. This function will have to be called multiple 35 | # times before you have enough information to accurately predict the 36 | # next position. The OTHER variable that your function returns will be 37 | # passed back to your function the next time it is called. You can use 38 | # this to keep track of important information over time. 39 | 40 | """ WORKING COPY OF THE SUBMITTED ANSWER WITH 5 STATE VARIABLEE """ 41 | 42 | def estimate_next_pos(measurement, OTHER=None): 43 | """Estimate the next (x, y) position of the wandering Traxbot 44 | based on noisy (x, y) measurements.""" 45 | 46 | # identity matrix 47 | I = matrix([[1., 0., 0., 0., 0.], 48 | [0., 1., 0., 0., 0.], 49 | [0., 0., 1., 0., 0.], 50 | [0., 0., 0., 1., 0.], 51 | [0., 0., 0., 0., 1.]]) 52 | 53 | #motion update matrix 54 | H = matrix([[1., 0., 0., 0., 0.], 55 | [0., 1., 0., 0., 0.]]) 56 | 57 | #measurement noise 58 | R = matrix([[measurement_noise, 0.], 59 | [0., measurement_noise]]) 60 | 61 | z = matrix([[measurement[0]], 62 | [measurement[1]]]) 63 | 64 | u = matrix([[0.], 65 | [0.], 66 | [0.], 67 | [0.], 68 | [0.]]) 69 | 70 | if OTHER is not None and 'X' not in OTHER: 71 | last_measurement = OTHER['last_measurement'] 72 | 73 | angle = atan2(measurement[0] - last_measurement[0], measurement[1] - last_measurement[1]) 74 | print 'angle: %.2f' %(angle * 180 / pi) 75 | if 'last_angle' not in OTHER: 76 | OTHER['last_angle'] = angle 77 | xy_estimate = [1., 1.] 78 | OTHER['last_measurement'] = measurement 79 | print 'here@' 80 | return xy_estimate, OTHER 81 | else: 82 | print 'here!' 83 | turning_angle = angle - OTHER['last_angle'] 84 | 85 | elif OTHER is None: 86 | print 'here!!!!!!' 87 | OTHER = {'last_measurement': measurement} 88 | return [1.,1.], OTHER 89 | 90 | 91 | if 'X' in OTHER: 92 | X = OTHER['X'] 93 | P = OTHER['P'] 94 | 95 | else: 96 | print 'here!!' 97 | X = matrix([[measurement[0]], 98 | [measurement[1]], 99 | [1.], 100 | [turning_angle], 101 | [1.]]) 102 | #convariance matrix 103 | P = matrix([[1000, 0., 0., 0., 0.], 104 | [0., 1000., 0., 0., 0.], 105 | [0., 0., 1000., 0., 0.], 106 | [0., 0., 0., 1000., 0.], 107 | [0., 0., 0., 0., 1000.] 108 | ]) 109 | 110 | #measurement update 111 | Y = z - (H * X) 112 | S = H * P * H.transpose() + R 113 | K = P * H.transpose() * S.inverse() 114 | 115 | X = X + (K * Y) 116 | P = (I - (K * H)) * P 117 | 118 | #Prediction 119 | x = X.value[0][0] 120 | y = X.value[1][0] 121 | angle = X.value[2][0] 122 | turning_angle = X.value[3][0] 123 | distance = X.value[4][0] 124 | 125 | 126 | 127 | new_X = [ 128 | [x + distance * sin(angle+turning_angle)], 129 | [y + distance * cos(angle+turning_angle)], 130 | [angle+turning_angle], 131 | [turning_angle], 132 | [distance], 133 | ] 134 | 135 | update_row0 = [ 136 | 1., 137 | 0., 138 | distance * cos(angle+turning_angle), 139 | distance * cos(angle+turning_angle), 140 | sin(angle+turning_angle), 141 | ] 142 | 143 | update_row1 = [ 144 | 0., 145 | 1., 146 | -distance * sin(turning_angle+angle), 147 | -distance * sin(turning_angle+angle), 148 | cos(angle+turning_angle) 149 | ] 150 | 151 | updated_X = [ 152 | update_row0, 153 | update_row1, 154 | [0., 0., 1., 1., 0.], 155 | [0., 0., 0., 1., 0.], 156 | [0., 0., 0., 0., 1.], 157 | ] 158 | 159 | A = matrix(updated_X) 160 | 161 | P = A * P * A.transpose() 162 | 163 | X = matrix(new_X) 164 | 165 | xy_estimate = [X.value[0][0], X.value[1][0]] 166 | OTHER = {'X': X, 'P': P} 167 | 168 | # You must return xy_estimate (x, y), and OTHER (even if it is None) 169 | # in this order for grading purposes. 170 | X.show() 171 | 172 | print '----------------------------------------------' 173 | return xy_estimate, OTHER 174 | 175 | 176 | # A helper function you may find useful. 177 | def distance_between(point1, point2): 178 | """Computes distance between point1 and point2. Points are (x, y) pairs.""" 179 | x1, y1 = point1 180 | x2, y2 = point2 181 | return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) 182 | 183 | # This is here to give you a sense for how we will be running and grading 184 | # your code. Note that the OTHER variable allows you to store any 185 | # information that you want. 186 | 187 | def demo_grading(estimate_next_pos_fcn, target_bot, OTHER = None): 188 | localized = False 189 | distance_tolerance = 0.01 * target_bot.distance 190 | ctr = 0 191 | # if you haven't localized the target bot, make a guess about the next 192 | # position, then we move the bot and compare your guess to the true 193 | # next position. When you are close enough, we stop checking. 194 | while not localized and ctr <= 1000: 195 | ctr += 1 196 | measurement = target_bot.sense() 197 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 198 | target_bot.move_in_circle() 199 | true_position = (target_bot.x, target_bot.y) 200 | error = distance_between(position_guess, true_position) 201 | if error <= distance_tolerance: 202 | print "You got it right! It took you ", ctr, " steps to localize." 203 | localized = True 204 | if ctr == 1000: 205 | print "Sorry, it took you too many steps to localize the target." 206 | return localized 207 | 208 | 209 | 210 | def demo_grading_vis(estimate_next_pos_fcn, target_bot, OTHER=None): 211 | localized = False 212 | distance_tolerance = 0.01 * target_bot.distance 213 | ctr = 0 214 | # if you haven't localized the target bot, make a guess about the next 215 | # position, then we move the bot and compare your guess to the true 216 | # next position. When you are close enough, we stop checking. 217 | # For Visualization 218 | import turtle #You need to run this locally to use the turtle module 219 | 220 | window = turtle.Screen() 221 | window.bgcolor('white') 222 | size_multiplier = 25.0 #change Size of animation 223 | broken_robot = turtle.Turtle() 224 | broken_robot.shape('turtle') 225 | broken_robot.color('green') 226 | broken_robot.resizemode('user') 227 | broken_robot.shapesize(0.1, 0.1, 0.1) 228 | measured_broken_robot = turtle.Turtle() 229 | measured_broken_robot.shape('circle') 230 | measured_broken_robot.color('red') 231 | measured_broken_robot.resizemode('user') 232 | measured_broken_robot.shapesize(0.1, 0.1, 0.1) 233 | prediction = turtle.Turtle() 234 | prediction.shape('arrow') 235 | prediction.color('blue') 236 | prediction.resizemode('user') 237 | prediction.shapesize(0.1, 0.1, 0.1) 238 | prediction.penup() 239 | broken_robot.penup() 240 | measured_broken_robot.penup() 241 | #End of Visualization 242 | while not localized and ctr <= 1000: 243 | ctr += 1 244 | measurement = target_bot.sense() 245 | position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) 246 | target_bot.move_in_circle() 247 | true_position = (target_bot.x, target_bot.y) 248 | error = distance_between(position_guess, true_position) 249 | if error <= distance_tolerance: 250 | print "You got it right! It took you ", ctr, " steps to localize." 251 | localized = True 252 | if ctr == 1000: 253 | print "Sorry, it took you too many steps to localize the target." 254 | #More Visualization 255 | measured_broken_robot.setheading(target_bot.heading * 180 / pi) 256 | measured_broken_robot.goto(measurement[0] * size_multiplier, measurement[1] * size_multiplier - 200) 257 | measured_broken_robot.stamp() 258 | broken_robot.setheading(target_bot.heading * 180 / pi) 259 | broken_robot.goto(target_bot.x * size_multiplier, target_bot.y * size_multiplier - 200) 260 | broken_robot.stamp() 261 | prediction.setheading(target_bot.heading * 180 / pi) 262 | prediction.goto(position_guess[0] * size_multiplier, position_guess[1] * size_multiplier - 200) 263 | prediction.stamp() 264 | #End of Visualization 265 | return localized 266 | 267 | 268 | # This is a demo for what a strategy could look like. This one isn't very good. 269 | def naive_next_pos(measurement, OTHER=None): 270 | """This strategy records the first reported position of the target and 271 | assumes that eventually the target bot will eventually return to that 272 | position, so it always guesses that the first position will be the next.""" 273 | if not OTHER: # this is the first measurement 274 | OTHER = measurement 275 | xy_estimate = OTHER 276 | return xy_estimate, OTHER 277 | 278 | # This is how we create a target bot. Check the robot.py file to understand 279 | # How the robot class behaves. 280 | test_target = robot(2.1, 4.3, 0.5, 2 * pi / 34.0, 1.5) 281 | measurement_noise = .05 * test_target.distance 282 | 283 | test_target.set_noise(0.0, 0.0, measurement_noise) 284 | 285 | demo_grading(estimate_next_pos, test_target) 286 | 287 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Udacity-AI-for-Robotics 2 | My solutions to some quizzes, exercises, and projects in the Udacity Artificial Intelligence for Robotics course 3 | 4 | 1. Localization 5 | 2. Kalman Filters 6 | 3. Particle Filters 7 | 4. Search 8 | 5. PID Control 9 | 6. SLAM 10 | 11 | - Practice Exam 12 | - Project: Runaway Robot 13 | --------------------------------------------------------------------------------