├── Final Exam ├── Final Exam Theory.pdf └── final.py ├── Homework 01 └── hw1.py ├── Homework 02 └── hw2.py ├── Homework 03 ├── hw3_4.py ├── hw3_5.py └── hw3_6.py ├── Homework 04 └── hw4_5.py ├── Homework 05 ├── hw5_2.py ├── hw5_3.py └── hw5_4.py ├── Homework 06 └── hw6.py ├── README.md ├── Unit 02 └── kalman_matrices.py ├── Unit 03 └── myrobot.py └── Unit 04 └── twiddle.py /Final Exam/Final Exam Theory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rieder91/CS373-Programming-a-Robotic-Car/18519e6ae3db154220249ff27ca684f598f0c83c/Final Exam/Final Exam Theory.pdf -------------------------------------------------------------------------------- /Final Exam/final.py: -------------------------------------------------------------------------------- 1 | # final programming exercise done with dp 2 | # Thomas Rieder 3 | # 4 | 5 | # ------------------- 6 | # Background Information 7 | # 8 | # In this problem, you will build a planner that makes a robot 9 | # car's lane decisions. On a highway, the left lane (in the US) 10 | # generally has a higher traffic speed than the right line. 11 | # 12 | # In this problem, a 2 lane highway of length 5 could be 13 | # represented as: 14 | # 15 | # road = [[80, 80, 80, 80, 80], 16 | # [60, 60, 60, 60, 60]] 17 | # 18 | # In this case, the left lane has an average speed of 80 km/h and 19 | # the right lane has a speed of 60 km/h. We can use a 0 to indicate 20 | # an obstacle in the road. 21 | # 22 | # To get to a location as quickly as possible, we usually 23 | # want to be in the left lane. But there is a cost associated 24 | # with changing lanes. This means that for short trips, it is 25 | # sometimes optimal to stay in the right lane. 26 | # 27 | # ------------------- 28 | # User Instructions 29 | # 30 | # Design a planner (any kind you like, so long as it works). 31 | # This planner should be a function named plan() that takes 32 | # as input four parameters: road, lane_change_cost, init, and 33 | # goal. See parameter info below. 34 | # 35 | # Your function should RETURN the final cost to reach the 36 | # goal from the start point (which should match with our answer). 37 | # You may include print statements to show the optimum policy, 38 | # though this is not necessary for grading. 39 | # 40 | # Your solution must work for a variety of roads and lane 41 | # change costs. 42 | # 43 | # Add your code at line 92. 44 | # 45 | # -------------------- 46 | # Parameter Info 47 | # 48 | # road - A grid of values. Each value represents the speed associated 49 | # with that cell. A value of 0 means the cell in non-navigable. 50 | # The cost for traveling in a cell must be (1.0 / speed). 51 | # 52 | # lane_change_cost - The cost associated with changing lanes. 53 | # 54 | # init - The starting point for your car. This will always be somewhere 55 | # in the right (bottom) lane to simulate a highway on-ramp. 56 | # 57 | # goal - The destination. This will always be in the right lane to 58 | # simulate a highway exit-ramp. 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 | # You may also use the build_road function to build 67 | # your own roads to test your function with. 68 | 69 | import random 70 | 71 | # ------------------------------------------ 72 | # build_road - Makes a road according to your specified length and 73 | # lane_speeds. lane_speeds is a list of speeds for the lanes (listed 74 | # from left lane to right). You can also include random obstacles. 75 | # 76 | def build_road(length, lane_speeds, print_flag = False, obstacles = False, obstacle_prob = 0.05): 77 | num_lanes = len(lane_speeds) 78 | road = [[lane_speeds[i] for dist in range(length)] for i in range(len(lane_speeds))] 79 | if obstacles: 80 | for x in range(len(road)): 81 | for y in range(len(road[0])): 82 | if random.random() < obstacle_prob: 83 | road[x][y] = 0 84 | if print_flag: 85 | for lane in road: 86 | print '[' + ', '.join('%5.3f' % speed for speed in lane) + ']' 87 | return road 88 | 89 | # ------------------------------------------ 90 | # plan - Returns cost to get from init to goal on road given a 91 | # lane_change_cost. 92 | # 93 | def plan(road, lane_change_cost, init, goal): # Don't change the name of this function! 94 | # 10 is unreachable 95 | value = [[10 for place in lane] for lane in road] 96 | policy = [['--' for place in lane] for lane in road] 97 | 98 | x_action = [-1, 0, 1] 99 | y_action = [-1, 0, 1] 100 | action_name = ['||', '->', '||'] 101 | 102 | change = True 103 | 104 | while change: 105 | change = False 106 | for x in range(len(road)): 107 | for y in range(len(road[0])): 108 | if goal[0] == x and goal[1] == y: 109 | if value[x][y] > 0: 110 | # goal reached! 111 | value[x][y] = 0 112 | policy[x][y] = 'G!' 113 | change = True 114 | if road[x][y] != 0: 115 | # if not goal 116 | for k in range(len(x_action)): 117 | # iterate through every action for every field 118 | x2 = x + x_action[k] 119 | y2 = y + 1 # the car cant move back 120 | # validity check 121 | if (x2 >= 0 and x2 < len(road) and y2 >= 0 and y2 < len(road[0]) and road[x2][y2] != 0): 122 | if x_action[k] == 0: 123 | # no lane change 124 | v2 = 1. / road[x][y] + value[x2][y2] 125 | else: 126 | # lane change 127 | v2 = 1. / road[x][y] + value[x2][y2] + lane_change_cost 128 | if v2 < value[x][y]: 129 | # update value if new cost is lower/better 130 | change = True 131 | value[x][y] = v2 132 | policy[x][y] = action_name[k] 133 | 134 | # testing output 135 | # print the policy map 136 | for i in range(len(road)): 137 | print policy[i] 138 | # print the value map 139 | for i in range(len(road)): 140 | print value[i] 141 | 142 | # return the value of starting position (bottom right corner) 143 | return value[len(value) - 1][0] 144 | 145 | ################# TESTING ################## 146 | 147 | # ------------------------------------------ 148 | # solution check - Checks your path function using 149 | # data from list called test[]. Uncomment the call 150 | # to solution_check at the bottom to test your code. 151 | # 152 | def solution_check(test, epsilon = 0.00001): 153 | answer_list = [] 154 | for i in range(len(test[0])): 155 | user_cost = plan(test[0][i], test[1][i], test[2][i], test[3][i]) 156 | true_cost = test[4][i] 157 | if abs(user_cost - true_cost) < epsilon: 158 | answer_list.append(1) 159 | else: 160 | answer_list.append(0) 161 | correct_answers = 0 162 | print 163 | for i in range(len(answer_list)): 164 | if answer_list[i] == 1: 165 | print 'Test case', i+1, 'passed!' 166 | correct_answers += 1 167 | else: 168 | print 'Test case', i+1, 'failed.' 169 | if correct_answers == len(answer_list): 170 | print "\nYou passed all test cases!" 171 | return True 172 | else: 173 | print "\nYou passed", correct_answers, "of", len(answer_list), "test cases. Try to get them all!" 174 | return False 175 | 176 | # Test Case 1 (FAST left lane) 177 | test_road1 = build_road(8, [100, 10, 1]) 178 | lane_change_cost1 = 1.0 / 1000.0 179 | test_init1 = [len(test_road1) - 1, 0] 180 | test_goal1 = [len(test_road1) - 1, len(test_road1[0]) - 1] 181 | true_cost1 = 1.244 182 | 183 | # Test Case 2 (more realistic road) 184 | test_road2 = build_road(14, [80, 60, 40, 20]) 185 | lane_change_cost2 = 1.0 / 100.0 186 | test_init2 = [len(test_road2) - 1, 0] 187 | test_goal2 = [len(test_road2) - 1, len(test_road2[0]) - 1] 188 | true_cost2 = 0.293333333333 189 | 190 | # Test Case 3 (Obstacles included) 191 | test_road3 = [[50, 50, 50, 50, 50, 40, 0, 40, 50, 50, 50, 50, 50, 50, 50], # left lane: 50 km/h 192 | [40, 40, 40, 40, 40, 30, 20, 30, 40, 40, 40, 40, 40, 40, 40], 193 | [30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30]] # right lane: 30 km/h 194 | lane_change_cost3 = 1.0 / 500.0 195 | test_init3 = [len(test_road3) - 1, 0] 196 | test_goal3 = [len(test_road3) - 1, len(test_road3[0]) - 1] 197 | true_cost3 = 0.355333333333 198 | 199 | # Test Case 4 (Slalom) 200 | test_road4 = [[50, 50, 50, 50, 50, 40, 0, 40, 50, 50, 0, 50, 50, 50, 50], # left lane: 50 km/h 201 | [40, 40, 40, 40, 0, 30, 20, 30, 0, 40, 40, 40, 40, 40, 40], 202 | [30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30]] # right lane: 30 km/h 203 | lane_change_cost4 = 1.0 / 65.0 204 | test_init4 = [len(test_road4) - 1, 0] 205 | test_goal4 = [len(test_road4) - 1, len(test_road4[0]) - 1] 206 | true_cost4 = 0.450641025641 207 | 208 | 209 | testing_suite = [[test_road1, test_road2, test_road3, test_road4], 210 | [lane_change_cost1, lane_change_cost2, lane_change_cost3, lane_change_cost4], 211 | [test_init1, test_init2, test_init3, test_init4], 212 | [test_goal1, test_goal2, test_goal3, test_goal4], 213 | [true_cost1, true_cost2, true_cost3, true_cost4]] 214 | 215 | solution_check(testing_suite) #UNCOMMENT THIS LINE TO TEST YOUR CODE 216 | 217 | 218 | 219 | -------------------------------------------------------------------------------- /Homework 01/hw1.py: -------------------------------------------------------------------------------- 1 | # Histogramm based localization in a 2d world (like colored flatland) 2 | # Thomas Rieder 3 | # 4 | 5 | 6 | colors = [['red', 'green', 'green', 'red' , 'red'], 7 | ['red', 'red', 'green', 'red', 'red'], 8 | ['red', 'red', 'green', 'green', 'red'], 9 | ['red', 'red', 'red', 'red', 'red']] 10 | 11 | measurements = ['green', 'green', 'green' ,'green', 'green'] 12 | 13 | 14 | motions = [[0,0],[0,1],[1,0],[1,0],[0,1]] 15 | 16 | sensor_right = 0.7 17 | 18 | p_move = 0.8 19 | 20 | def show(p): 21 | for i in range(len(p)): 22 | print p[i] 23 | 24 | #DO NOT USE IMPORT 25 | #ENTER CODE BELOW HERE 26 | #ANY CODE ABOVE WILL CAUSE 27 | #HOMEWORK TO BE GRADED 28 | #INCORRECT 29 | 30 | size_h = len(colors[0]) 31 | size_v = len(colors) 32 | 33 | start_p = 1.0 / (size_h * size_v) 34 | 35 | # uniform distribution 36 | p = [[start_p for column in range(size_h)] for row in range(size_v)] 37 | 38 | # sense function 39 | def sense(p, Z): 40 | # initialize array 41 | q = [[0 for column in range(size_h)] for row in range(size_v)] 42 | # calculate probabilities 43 | for i in range(size_v): 44 | for j in range(size_h): 45 | hit = colors[i][j] == Z 46 | q[i][j] = p[i][j] * (hit * sensor_right + (1 - hit) * (1 - sensor_right)) 47 | # normalize 48 | pSum = sum(sum(i) for i in q) 49 | for i in range(size_v): 50 | for j in range(size_h): 51 | q[i][j] = q[i][j] / pSum 52 | return q 53 | 54 | # move function 55 | def move(p, U): 56 | # initialize array 57 | q = [[0 for column in range(size_h)] for row in range(size_v)] 58 | # execute move 59 | for i in range(size_v): 60 | for j in range(size_h): 61 | s = p_move * p[(i - U[0]) % size_v][(j - U[1]) % size_h] 62 | s += (1 - p_move) * p[i][j] 63 | q[i][j] = s 64 | return q 65 | 66 | 67 | # move/sense-loop 68 | for i in range(len(measurements)): 69 | p = move(p, motions[i]) 70 | p = sense(p, measurements[i]) 71 | 72 | #Your probability array must be printed 73 | #with the following code. 74 | 75 | show(p) 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /Homework 02/hw2.py: -------------------------------------------------------------------------------- 1 | # 4 dimensional kalman filter 2 | # Thomas Rieder 3 | # 4 | 5 | # Fill in the matrices P, F, H, R and I at the bottom 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 | # equations are from the definition of the kalman filter itself 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 | ### fill this in: ### 183 | P = matrix([[0., 0., 0., 0.], 184 | [0., 0., 0., 0.], 185 | [0., 0., 1000., 0.], # uncertainty for both x & y velocity is 1000. 186 | [0., 0., 0., 1000.]]) # initial uncertainty 187 | 188 | F = matrix([[1., 0., dt, 0.], # x and x velocity 189 | [0., 1., 0., dt], # y and y velocity 190 | [0., 0., 1., 0.], 191 | [0., 0., 0., 1.]]) # next state function 192 | 193 | H = matrix([[1., 0., 0., 0.], # simple 4d to 2d projection 194 | [0., 1., 0., 0.]]) # measurement function 195 | 196 | R = matrix([[0.1, 0.], # 197 | [0., 0.1]]) # measurement uncertainty 198 | 199 | I = matrix([[1., 0., 0., 0.], 200 | [0., 1., 0., 0.], 201 | [0., 0., 1., 0.], 202 | [0., 0., 0., 1.]]) # identity matrix 203 | 204 | filter(x, P) 205 | -------------------------------------------------------------------------------- /Homework 03/hw3_4.py: -------------------------------------------------------------------------------- 1 | 2 | # -------------- 3 | # USER INSTRUCTIONS 4 | # 5 | # Write a function in the class robot called move() 6 | # 7 | # that takes self and a motion vector (this 8 | # motion vector contains a steering* angle and a 9 | # distance) as input and returns an instance of the class 10 | # robot with the appropriate x, y, and orientation 11 | # for the given motion. 12 | # 13 | # *steering is defined in the video 14 | # which accompanies this problem. 15 | # 16 | # For now, please do NOT add noise to your move function. 17 | # 18 | # Please do not modify anything except where indicated 19 | # below. 20 | # 21 | # There are test cases which you are free to use at the 22 | # bottom. If you uncomment them for testing, make sure you 23 | # re-comment them before you submit. 24 | 25 | from math import * 26 | import random 27 | # -------- 28 | # 29 | # the "world" has 4 landmarks. 30 | # the robot's initial coordinates are somewhere in the square 31 | # represented by the landmarks. 32 | # 33 | # NOTE: Landmark coordinates are given in (y, x) form and NOT 34 | # in the traditional (x, y) format! 35 | 36 | landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks 37 | world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds" 38 | max_steering_angle = pi/4 # You don't need to use this value, but it is good to keep in mind the limitations of a real car. 39 | 40 | # ------------------------------------------------ 41 | # 42 | # this is the robot class 43 | # 44 | 45 | class robot: 46 | 47 | # -------- 48 | 49 | # init: 50 | # creates robot and initializes location/orientation 51 | # 52 | 53 | def __init__(self, length = 10.0): 54 | self.x = random.random() * world_size # initial x position 55 | self.y = random.random() * world_size # initial y position 56 | self.orientation = random.random() * 2.0 * pi # initial orientation 57 | self.length = length # length of robot 58 | self.bearing_noise = 0.0 # initialize bearing noise to zero 59 | self.steering_noise = 0.0 # initialize steering noise to zero 60 | self.distance_noise = 0.0 # initialize distance noise to zero 61 | 62 | def __repr__(self): 63 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation)) 64 | # -------- 65 | # set: 66 | # sets a robot coordinate 67 | # 68 | 69 | def set(self, new_x, new_y, new_orientation): 70 | 71 | if new_orientation < 0 or new_orientation >= 2 * pi: 72 | raise ValueError, 'Orientation must be in [0..2pi]' 73 | self.x = float(new_x) 74 | self.y = float(new_y) 75 | self.orientation = float(new_orientation) 76 | 77 | 78 | # -------- 79 | # set_noise: 80 | # sets the noise parameters 81 | # 82 | 83 | def set_noise(self, new_b_noise, new_s_noise, new_d_noise): 84 | # makes it possible to change the noise parameters 85 | # this is often useful in particle filters 86 | self.bearing_noise = float(new_b_noise) 87 | self.steering_noise = float(new_s_noise) 88 | self.distance_noise = float(new_d_noise) 89 | 90 | ############# ONLY ADD/MODIFY CODE BELOW HERE ################### 91 | 92 | # -------- 93 | # move: 94 | # move along a section of a circular path according to motion 95 | # 96 | 97 | def move(self, motion): # Do not change the name of this function 98 | result = robot(self.length) 99 | result.set_noise(self.bearing_noise, self.steering_noise, self.distance_noise) 100 | 101 | # basically a straight implementation of the equations for bicycle-based movement 102 | alpha = motion[0] 103 | d = motion[1] 104 | beta = d * tan(alpha) / self.length 105 | if abs(beta) < 0.001: 106 | # no movement 107 | result.x = self.x + d * cos(self.orientation) 108 | result.y = self.y + d * sin(self.orientation) 109 | else: 110 | R = d / beta 111 | cx = self.x - sin(self.orientation) * R 112 | cy = self.y + cos(self.orientation) * R 113 | result.x = cx + sin(self.orientation + beta) * R 114 | result.y = cy - cos(self.orientation + beta) * R 115 | result.orientation = (self.orientation + beta) % (2*pi) 116 | 117 | return result # make sure your move function returns an instance 118 | # of the robot class with the correct coordinates. 119 | 120 | ############## ONLY ADD/MODIFY CODE ABOVE HERE #################### 121 | 122 | 123 | ## IMPORTANT: You may uncomment the test cases below to test your code. 124 | ## But when you submit this code, your test cases MUST be commented 125 | ## out. Our testing program provides its own code for testing your 126 | ## move function with randomized motion data. 127 | 128 | ## -------- 129 | ## TEST CASE: 130 | ## 131 | ## 1) The following code should print: 132 | ## Robot: [x=0.0 y=0.0 orient=0.0] 133 | ## Robot: [x=10.0 y=0.0 orient=0.0] 134 | ## Robot: [x=19.861 y=1.4333 orient=0.2886] 135 | ## Robot: [x=39.034 y=7.1270 orient=0.2886] 136 | ## 137 | ## 138 | length = 20. 139 | bearing_noise = 0.0 140 | steering_noise = 0.0 141 | distance_noise = 0.0 142 | 143 | myrobot = robot(length) 144 | myrobot.set(0.0, 0.0, 0.0) 145 | myrobot.set_noise(bearing_noise, steering_noise, distance_noise) 146 | 147 | motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]] 148 | 149 | T = len(motions) 150 | 151 | print 'Robot: ', myrobot 152 | for t in range(T): 153 | myrobot = myrobot.move(motions[t]) 154 | print 'Robot: ', myrobot 155 | 156 | 157 | 158 | ## IMPORTANT: You may uncomment the test cases below to test your code. 159 | ## But when you submit this code, your test cases MUST be commented 160 | ## out. Our testing program provides its own code for testing your 161 | ## move function with randomized motion data. 162 | 163 | 164 | ## 2) The following code should print: 165 | ## Robot: [x=0.0 y=0.0 orient=0.0] 166 | ## Robot: [x=9.9828 y=0.5063 orient=0.1013] 167 | ## Robot: [x=19.863 y=2.0201 orient=0.2027] 168 | ## Robot: [x=29.539 y=4.5259 orient=0.3040] 169 | ## Robot: [x=38.913 y=7.9979 orient=0.4054] 170 | ## Robot: [x=47.887 y=12.400 orient=0.5067] 171 | ## Robot: [x=56.369 y=17.688 orient=0.6081] 172 | ## Robot: [x=64.273 y=23.807 orient=0.7094] 173 | ## Robot: [x=71.517 y=30.695 orient=0.8108] 174 | ## Robot: [x=78.027 y=38.280 orient=0.9121] 175 | ## Robot: [x=83.736 y=46.485 orient=1.0135] 176 | ## 177 | ## 178 | length = 20. 179 | bearing_noise = 0.0 180 | steering_noise = 0.0 181 | distance_noise = 0.0 182 | 183 | myrobot = robot(length) 184 | myrobot.set(0.0, 0.0, 0.0) 185 | myrobot.set_noise(bearing_noise, steering_noise, distance_noise) 186 | 187 | motions = [[0.2, 10.] for row in range(10)] 188 | 189 | T = len(motions) 190 | 191 | print 'Robot: ', myrobot 192 | for t in range(T): 193 | myrobot = myrobot.move(motions[t]) 194 | print 'Robot: ', myrobot 195 | 196 | ## IMPORTANT: You may uncomment the test cases below to test your code. 197 | ## But when you submit this code, your test cases MUST be commented 198 | ## out. Our testing program provides its own code for testing your 199 | ## move function with randomized motion data. 200 | 201 | 202 | -------------------------------------------------------------------------------- /Homework 03/hw3_5.py: -------------------------------------------------------------------------------- 1 | # -------------- 2 | # USER INSTRUCTIONS 3 | # 4 | # Write a function in the class robot called sense() 5 | # that takes self as input 6 | # and returns a list, Z, of the four bearings* to the 4 7 | # different landmarks. you will have to use the robot's 8 | # x and y position, as well as its orientation, to 9 | # compute this. 10 | # 11 | # *bearing is defined in the video 12 | # which accompanies this problem. 13 | # 14 | # For now, please do NOT add noise to your sense function. 15 | # 16 | # Please do not modify anything except where indicated 17 | # below. 18 | # 19 | # There are test cases provided at the bottom which you are 20 | # free to use. If you uncomment any of these cases for testing 21 | # make sure that you re-comment it before you submit. 22 | 23 | from math import * 24 | import random 25 | 26 | # -------- 27 | # 28 | # the "world" has 4 landmarks. 29 | # the robot's initial coordinates are somewhere in the square 30 | # represented by the landmarks. 31 | # 32 | # NOTE: Landmark coordinates are given in (y, x) form and NOT 33 | # in the traditional (x, y) format! 34 | 35 | landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) form. 36 | world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds" 37 | 38 | # ------------------------------------------------ 39 | # 40 | # this is the robot class 41 | # 42 | 43 | class robot: 44 | 45 | # -------- 46 | # init: 47 | # creates robot and initializes location/orientation 48 | # 49 | 50 | def __init__(self, length = 10.0): 51 | self.x = random.random() * world_size # initial x position 52 | self.y = random.random() * world_size # initial y position 53 | self.orientation = random.random() * 2.0 * pi # initial orientation 54 | self.length = length # length of robot 55 | self.bearing_noise = 0.0 # initialize bearing noise to zero 56 | self.steering_noise = 0.0 # initialize steering noise to zero 57 | self.distance_noise = 0.0 # initialize distance noise to zero 58 | 59 | 60 | 61 | def __repr__(self): 62 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), 63 | str(self.orientation)) 64 | 65 | 66 | # -------- 67 | # set: 68 | # sets a robot coordinate 69 | # 70 | 71 | def set(self, new_x, new_y, new_orientation): 72 | if new_orientation < 0 or new_orientation >= 2 * pi: 73 | raise ValueError, 'Orientation must be in [0..2pi]' 74 | self.x = float(new_x) 75 | self.y = float(new_y) 76 | self.orientation = float(new_orientation) 77 | 78 | # -------- 79 | # set_noise: 80 | # sets the noise parameters 81 | # 82 | 83 | def set_noise(self, new_b_noise, new_s_noise, new_d_noise): 84 | # makes it possible to change the noise parameters 85 | # this is often useful in particle filters 86 | self.bearing_noise = float(new_b_noise) 87 | self.steering_noise = float(new_s_noise) 88 | self.distance_noise = float(new_d_noise) 89 | 90 | ############# ONLY ADD/MODIFY CODE BELOW HERE ################### 91 | 92 | # -------- 93 | # sense: 94 | # obtains bearings from positions 95 | # 96 | 97 | def sense(self): #do not change the name of this function 98 | Z = [] 99 | 100 | for l in landmarks: 101 | direction = atan2(l[0] - self.y, l[1] - self.x) 102 | bearing = direction - self.orientation 103 | Z.append(bearing % (2 * pi)) 104 | 105 | return Z #Leave this line here. Return vector Z of 4 bearings. 106 | 107 | ############## ONLY ADD/MODIFY CODE ABOVE HERE #################### 108 | 109 | 110 | ## IMPORTANT: You may uncomment the test cases below to test your code. 111 | ## But when you submit this code, your test cases MUST be commented 112 | ## out. Our testing program provides its own code for testing your 113 | ## sense function with randomized initial robot coordinates. 114 | 115 | ## -------- 116 | ## TEST CASES: 117 | 118 | 119 | 120 | ## 121 | ## 1) The following code should print the list [6.004885648174475, 3.7295952571373605, 1.9295669970654687, 0.8519663271732721] 122 | ## 123 | ## 124 | length = 20. 125 | bearing_noise = 0.0 126 | steering_noise = 0.0 127 | distance_noise = 0.0 128 | 129 | myrobot = robot(length) 130 | myrobot.set(30.0, 20.0, 0.0) 131 | myrobot.set_noise(bearing_noise, steering_noise, distance_noise) 132 | 133 | print 'Robot: ', myrobot 134 | print 'Measurements: ', myrobot.sense() 135 | 136 | 137 | ## IMPORTANT: You may uncomment the test cases below to test your code. 138 | ## But when you submit this code, your test cases MUST be commented 139 | ## out. Our testing program provides its own code for testing your 140 | ## sense function with randomized initial robot coordinates. 141 | 142 | 143 | ## 144 | ## 2) The following code should print the list [5.376567117456516, 3.101276726419402, 1.3012484663475101, 0.22364779645531352] 145 | ## 146 | ## 147 | length = 20. 148 | bearing_noise = 0.0 149 | steering_noise = 0.0 150 | distance_noise = 0.0 151 | 152 | myrobot = robot(length) 153 | myrobot.set(30.0, 20.0, pi / 5.0) 154 | myrobot.set_noise(bearing_noise, steering_noise, distance_noise) 155 | 156 | print 'Robot: ', myrobot 157 | print 'Measurements: ', myrobot.sense() 158 | 159 | 160 | 161 | ## IMPORTANT: You may uncomment the test cases below to test your code. 162 | ## But when you submit this code, your test cases MUST be commented 163 | ## out. Our testing program provides its own code for testing your 164 | ## sense function with randomized initial robot coordinates. 165 | 166 | -------------------------------------------------------------------------------- /Homework 03/hw3_6.py: -------------------------------------------------------------------------------- 1 | # complete particle filter 2 | # Thomas Rieder 3 | # 4 | 5 | # -------------- 6 | # USER INSTRUCTIONS 7 | # 8 | # Now you will put everything together. 9 | # 10 | # First make sure that your sense and move functions 11 | # work as expected for the test cases provided at the 12 | # bottom of the previous two programming assignments. 13 | # Once you are satisfied, copy your sense and move 14 | # definitions into the robot class on this page, BUT 15 | # now include noise. 16 | # 17 | # A good way to include noise in the sense step is to 18 | # add Gaussian noise, centered at zero with variance 19 | # of self.bearing_noise to each bearing. You can do this 20 | # with the command random.gauss(0, self.bearing_noise) 21 | # 22 | # In the move step, you should make sure that your 23 | # actual steering angle is chosen from a Gaussian 24 | # distribution of steering angles. This distribution 25 | # should be centered at the intended steering angle 26 | # with variance of self.steering_noise. 27 | # 28 | # Feel free to use the included set_noise function. 29 | # 30 | # Please do not modify anything except where indicated 31 | # below. 32 | 33 | from math import * 34 | import random 35 | 36 | # -------- 37 | # 38 | # some top level parameters 39 | # 40 | 41 | max_steering_angle = pi / 4.0 # You do not need to use this value, but keep in mind the limitations of a real car. 42 | bearing_noise = 0.1 # Noise parameter: should be included in sense function. 43 | steering_noise = 0.1 # Noise parameter: should be included in move function. 44 | distance_noise = 5.0 # Noise parameter: should be included in move function. 45 | 46 | tolerance_xy = 15.0 # Tolerance for localization in the x and y directions. 47 | tolerance_orientation = 0.25 # Tolerance for orientation. 48 | 49 | 50 | # -------- 51 | # 52 | # the "world" has 4 landmarks. 53 | # the robot's initial coordinates are somewhere in the square 54 | # represented by the landmarks. 55 | # 56 | # NOTE: Landmark coordinates are given in (y, x) form and NOT 57 | # in the traditional (x, y) format! 58 | 59 | landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) format. 60 | world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds" 61 | 62 | # ------------------------------------------------ 63 | # 64 | # this is the robot class 65 | # 66 | 67 | class robot: 68 | 69 | # -------- 70 | # init: 71 | # creates robot and initializes location/orientation 72 | # 73 | 74 | def __init__(self, length = 20.0): 75 | self.x = random.random() * world_size # initial x position 76 | self.y = random.random() * world_size # initial y position 77 | self.orientation = random.random() * 2.0 * pi # initial orientation 78 | self.length = length # length of robot 79 | self.bearing_noise = 0.0 # initialize bearing noise to zero 80 | self.steering_noise = 0.0 # initialize steering noise to zero 81 | self.distance_noise = 0.0 # initialize distance noise to zero 82 | 83 | # -------- 84 | # set: 85 | # sets a robot coordinate 86 | # 87 | 88 | def set(self, new_x, new_y, new_orientation): 89 | 90 | if new_orientation < 0 or new_orientation >= 2 * pi: 91 | raise ValueError, 'Orientation must be in [0..2pi]' 92 | self.x = float(new_x) 93 | self.y = float(new_y) 94 | self.orientation = float(new_orientation) 95 | 96 | # -------- 97 | # set_noise: 98 | # sets the noise parameters 99 | # 100 | def set_noise(self, new_b_noise, new_s_noise, new_d_noise): 101 | # makes it possible to change the noise parameters 102 | # this is often useful in particle filters 103 | self.bearing_noise = float(new_b_noise) 104 | self.steering_noise = float(new_s_noise) 105 | self.distance_noise = float(new_d_noise) 106 | 107 | # -------- 108 | # measurement_prob 109 | # computes the probability of a measurement 110 | # 111 | 112 | def measurement_prob(self, measurements): 113 | 114 | # calculate the correct measurement 115 | predicted_measurements = self.sense(0) # Our sense function took 0 as an argument to switch off noise. 116 | 117 | 118 | # compute errors 119 | error = 1.0 120 | for i in range(len(measurements)): 121 | error_bearing = abs(measurements[i] - predicted_measurements[i]) 122 | error_bearing = (error_bearing + pi) % (2.0 * pi) - pi # truncate 123 | 124 | 125 | # update Gaussian 126 | error *= (exp(- (error_bearing ** 2) / (self.bearing_noise ** 2) / 2.0) / 127 | sqrt(2.0 * pi * (self.bearing_noise ** 2))) 128 | 129 | return error 130 | 131 | def move(self, motion): # Do not change the name of this function 132 | result = robot(self.length) 133 | result.set_noise(self.bearing_noise, self.steering_noise, self.distance_noise) 134 | 135 | # basically a straight implementation of the equations for bicycle-based movement 136 | # added gaussians for hw3_6 137 | alpha = random.gauss(motion[0], self.steering_noise) 138 | d = random.gauss(motion[1], self.distance_noise) 139 | beta = d * tan(alpha) / self.length 140 | 141 | if abs(beta) < 0.001: 142 | # no movement 143 | result.x = self.x + d * cos(self.orientation) 144 | result.y = self.y + d * sin(self.orientation) 145 | else: 146 | R = d / beta 147 | cx = self.x - sin(self.orientation) * R 148 | cy = self.y + cos(self.orientation) * R 149 | result.x = cx + sin(self.orientation + beta) * R 150 | result.y = cy - cos(self.orientation + beta) * R 151 | 152 | result.orientation = (self.orientation + beta) % (2*pi) 153 | 154 | return result # make sure your move function returns an instance 155 | # of the robot class with the correct coordinates. 156 | 157 | # noise is enabled by default 158 | def sense(self, noise = 1): #do not change the name of this function 159 | Z = [] 160 | 161 | for l in landmarks: 162 | direction = atan2(l[0] - self.y, l[1] - self.x) 163 | bearing = direction - self.orientation 164 | if noise == 1: 165 | bearing += random.gauss(0, self.bearing_noise) 166 | Z.append(bearing % (2 * pi)) 167 | 168 | return Z #Leave this line here. Return vector Z of 4 bearings. 169 | 170 | 171 | 172 | 173 | def __repr__(self): #allows us to print robot attributes. 174 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), 175 | str(self.orientation)) 176 | 177 | ############# ONLY ADD/MODIFY CODE BELOW HERE ################### 178 | 179 | # -------- 180 | # move: 181 | # 182 | 183 | # copy your code from the previous exercise 184 | # and modify it so that it simulates motion noise 185 | # according to the noise parameters 186 | # self.steering_noise 187 | # self.distance_noise 188 | 189 | # -------- 190 | # sense: 191 | # 192 | 193 | # copy your code from the previous exercise 194 | # and modify it so that it simulates bearing noise 195 | # according to 196 | # self.bearing_noise 197 | 198 | ############## ONLY ADD/MODIFY CODE ABOVE HERE #################### 199 | 200 | # -------- 201 | # 202 | # extract position from a particle set 203 | # 204 | 205 | def get_position(p): 206 | x = 0.0 207 | y = 0.0 208 | orientation = 0.0 209 | for i in range(len(p)): 210 | x += p[i].x 211 | y += p[i].y 212 | # orientation is tricky because it is cyclic. By normalizing 213 | # around the first particle we are somewhat more robust to 214 | # the 0=2pi problem 215 | orientation += (((p[i].orientation - p[0].orientation + pi) % (2.0 * pi)) 216 | + p[0].orientation - pi) 217 | return [x / len(p), y / len(p), orientation / len(p)] 218 | 219 | # -------- 220 | # 221 | # The following code generates the measurements vector 222 | # You can use it to develop your solution. 223 | # 224 | 225 | 226 | def generate_ground_truth(motions): 227 | 228 | myrobot = robot() 229 | myrobot.set_noise(bearing_noise, steering_noise, distance_noise) 230 | 231 | Z = [] 232 | T = len(motions) 233 | 234 | for t in range(T): 235 | myrobot = myrobot.move(motions[t]) 236 | Z.append(myrobot.sense()) 237 | #print 'Robot: ', myrobot 238 | return [myrobot, Z] 239 | 240 | # -------- 241 | # 242 | # The following code prints the measurements associated 243 | # with generate_ground_truth 244 | # 245 | 246 | def print_measurements(Z): 247 | 248 | T = len(Z) 249 | 250 | print 'measurements = [[%.8s, %.8s, %.8s, %.8s],' % \ 251 | (str(Z[0][0]), str(Z[0][1]), str(Z[0][2]), str(Z[0][3])) 252 | for t in range(1,T-1): 253 | print ' [%.8s, %.8s, %.8s, %.8s],' % \ 254 | (str(Z[t][0]), str(Z[t][1]), str(Z[t][2]), str(Z[t][3])) 255 | print ' [%.8s, %.8s, %.8s, %.8s]]' % \ 256 | (str(Z[T-1][0]), str(Z[T-1][1]), str(Z[T-1][2]), str(Z[T-1][3])) 257 | 258 | # -------- 259 | # 260 | # The following code checks to see if your particle filter 261 | # localizes the robot to within the desired tolerances 262 | # of the true position. The tolerances are defined at the top. 263 | # 264 | 265 | def check_output(final_robot, estimated_position): 266 | 267 | error_x = abs(final_robot.x - estimated_position[0]) 268 | error_y = abs(final_robot.y - estimated_position[1]) 269 | error_orientation = abs(final_robot.orientation - estimated_position[2]) 270 | error_orientation = (error_orientation + pi) % (2.0 * pi) - pi 271 | correct = error_x < tolerance_xy and error_y < tolerance_xy \ 272 | and error_orientation < tolerance_orientation 273 | return correct 274 | 275 | 276 | 277 | def particle_filter(motions, measurements, N=500): # I know it's tempting, but don't change N! 278 | # -------- 279 | # 280 | # Make particles 281 | # 282 | 283 | p = [] 284 | for i in range(N): 285 | r = robot() 286 | r.set_noise(bearing_noise, steering_noise, distance_noise) 287 | p.append(r) 288 | 289 | # -------- 290 | # 291 | # Update particles 292 | # 293 | 294 | for t in range(len(motions)): 295 | 296 | # motion update (prediction) 297 | p2 = [] 298 | for i in range(N): 299 | p2.append(p[i].move(motions[t])) 300 | p = p2 301 | 302 | # measurement update 303 | w = [] 304 | for i in range(N): 305 | w.append(p[i].measurement_prob(measurements[t])) 306 | 307 | # resampling 308 | p3 = [] 309 | index = int(random.random() * N) 310 | beta = 0.0 311 | mw = max(w) 312 | for i in range(N): 313 | beta += random.random() * 2.0 * mw 314 | while beta > w[index]: 315 | beta -= w[index] 316 | index = (index + 1) % N 317 | p3.append(p[index]) 318 | p = p3 319 | 320 | return get_position(p) 321 | 322 | ## IMPORTANT: You may uncomment the test cases below to test your code. 323 | ## But when you submit this code, your test cases MUST be commented 324 | ## out. 325 | ## 326 | ## You can test whether your particle filter works using the 327 | ## function check_output (see test case 2). We will be using a similar 328 | ## function. Note: Even for a well-implemented particle filter this 329 | ## function occasionally returns False. This is because a particle 330 | ## filter is a randomized algorithm. We will be testing your code 331 | ## multiple times. Make sure check_output returns True at least 80% 332 | ## of the time. 333 | 334 | 335 | 336 | ## -------- 337 | ## TEST CASES: 338 | ## 339 | ##1) Calling the particle_filter function with the following 340 | ## motions and measurements should return a [x,y,orientation] 341 | ## vector near [x=93.476 y=75.186 orient=5.2664], that is, the 342 | ## robot's true location. 343 | ## 344 | motions = [[2. * pi / 10, 20.] for row in range(8)] 345 | measurements = [[4.746936, 3.859782, 3.045217, 2.045506], 346 | [3.510067, 2.916300, 2.146394, 1.598332], 347 | [2.972469, 2.407489, 1.588474, 1.611094], 348 | [1.906178, 1.193329, 0.619356, 0.807930], 349 | [1.352825, 0.662233, 0.144927, 0.799090], 350 | [0.856150, 0.214590, 5.651497, 1.062401], 351 | [0.194460, 5.660382, 4.761072, 2.471682], 352 | [5.717342, 4.736780, 3.909599, 2.342536]] 353 | 354 | print particle_filter(motions, measurements) 355 | 356 | ## 2) You can generate your own test cases by generating 357 | ## measurements using the generate_ground_truth function. 358 | ## It will print the robot's last location when calling it. 359 | ## 360 | ## 361 | ##number_of_iterations = 6 362 | ##motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)] 363 | ## 364 | ##x = generate_ground_truth(motions) 365 | ##final_robot = x[0] 366 | ##measurements = x[1] 367 | ##estimated_position = particle_filter(motions, measurements) 368 | ##print_measurements(measurements) 369 | ##print 'Ground truth: ', final_robot 370 | ##print 'Particle filter: ', estimated_position 371 | ##print 'Code check: ', check_output(final_robot, estimated_position) 372 | 373 | 374 | 375 | -------------------------------------------------------------------------------- /Homework 04/hw4_5.py: -------------------------------------------------------------------------------- 1 | # dynamic programming exercise 2 | # Thomas Rieder 3 | # 4 | 5 | # -------------- 6 | # USER INSTRUCTIONS 7 | # 8 | # Write a function called stochastic_value that 9 | # takes no input and RETURNS two grids. The 10 | # first grid, value, should contain the computed 11 | # value of each cell as shown in the video. The 12 | # second grid, policy, should contain the optimum 13 | # policy for each cell. 14 | # 15 | # Stay tuned for a homework help video! This should 16 | # be available by Thursday and will be visible 17 | # in the course content tab. 18 | # 19 | # Good luck! Keep learning! 20 | # 21 | # -------------- 22 | # GRADING NOTES 23 | # 24 | # We will be calling your stochastic_value function 25 | # with several different grids and different values 26 | # of success_prob, collision_cost, and cost_step. 27 | # In order to be marked correct, your function must 28 | # RETURN (it does not have to print) two grids, 29 | # value and policy. 30 | # 31 | # When grading your value grid, we will compare the 32 | # value of each cell with the true value according 33 | # to this model. If your answer for each cell 34 | # is sufficiently close to the correct answer 35 | # (within 0.001), you will be marked as correct. 36 | # 37 | # NOTE: Please do not modify the values of grid, 38 | # success_prob, collision_cost, or cost_step inside 39 | # your function. Doing so could result in your 40 | # submission being inappropriately marked as incorrect. 41 | 42 | # ------------- 43 | # GLOBAL VARIABLES 44 | # 45 | # You may modify these variables for testing 46 | # purposes, but you should only modify them here. 47 | # Do NOT modify them inside your stochastic_value 48 | # function. 49 | 50 | grid = [[0, 0, 0, 0], 51 | [0, 0, 0, 0], 52 | [0, 0, 0, 0], 53 | [0, 1, 1, 0]] 54 | 55 | goal = [0, len(grid[0])-1] # Goal is in top right corner 56 | 57 | 58 | delta = [[-1, 0 ], # go up 59 | [ 0, -1], # go left 60 | [ 1, 0 ], # go down 61 | [ 0, 1 ]] # go right 62 | 63 | delta_name = ['^', '<', 'v', '>'] # Use these when creating your policy grid. 64 | 65 | success_prob = 0.5 66 | failure_prob = (1.0 - success_prob)/2.0 # Probability(stepping left) = prob(stepping right) = failure_prob 67 | collision_cost = 100 68 | cost_step = 1 69 | 70 | 71 | ############## INSERT/MODIFY YOUR CODE BELOW ################## 72 | # 73 | # You may modify the code below if you want, but remember that 74 | # your function must... 75 | # 76 | # 1) ...be called stochastic_value(). 77 | # 2) ...NOT take any arguments. 78 | # 3) ...return two grids: FIRST value and THEN policy. 79 | 80 | def stochastic_value(): 81 | value = [[1000 for row in range(len(grid[0]))] for col in range(len(grid))] 82 | policy = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))] 83 | 84 | progress = True 85 | while progress: 86 | progress = False 87 | 88 | # columns 89 | for x in range(len(grid)): 90 | # rows 91 | for y in range(len(grid[0])): 92 | # check if goal reached 93 | if goal[0] == x and goal[1] == y: 94 | # check if obstacle 95 | if value[x][y] > 0: 96 | value[x][y] = 0 97 | policy[x][y] = '*' 98 | progress = True 99 | # no obstacle 100 | elif grid[x][y] == 0: 101 | # iterate through all movements 102 | for i in range(len(delta)): 103 | xNew_1 = x + delta[i][0] 104 | yNew_1 = y + delta[i][1] 105 | 106 | xNew_2 = x + delta[i - 1][0] 107 | yNew_2 = y + delta[i - 1][1] 108 | 109 | xNew_3 = x + delta[(i + 1) % len(delta)][0] 110 | yNew_3 = y + delta[(i + 1) % len(delta)][1] 111 | 112 | valueNew = 0 113 | 114 | # check if valid for 1st 115 | valid = xNew_1 >= 0 and xNew_1 < len(grid) and yNew_1 >= 0 and yNew_1 < len(grid[0]) 116 | if not valid: 117 | valueNew = collision_cost * success_prob 118 | else: 119 | if grid[xNew_1][yNew_1] == 0: 120 | valueNew = value[xNew_1][yNew_1] * success_prob 121 | else: 122 | valueNew = collision_cost * success_prob 123 | 124 | # check if valid for 2nd 125 | valid = xNew_2 >= 0 and xNew_2 < len(grid) and yNew_2 >= 0 and yNew_2 < len(grid[0]) 126 | if not valid: 127 | valueNew += collision_cost * failure_prob 128 | else: 129 | if grid[xNew_2][yNew_2] == 0: 130 | valueNew += value[xNew_2][yNew_2] * failure_prob 131 | else: 132 | valueNew += collision_cost * failure_prob 133 | 134 | # check if valid for 3rd 135 | valid = xNew_3 >= 0 and xNew_3 < len(grid) and yNew_3 >= 0 and yNew_3 < len(grid[0]) 136 | if not valid: 137 | valueNew += collision_cost * failure_prob 138 | else: 139 | if grid[xNew_3][yNew_3] == 0: 140 | valueNew += value[xNew_3][yNew_3] * failure_prob 141 | else: 142 | valueNew += collision_cost * failure_prob 143 | 144 | valueNew += cost_step 145 | # check if progress 146 | if valueNew < value[x][y]: 147 | progress = True 148 | value[x][y] = valueNew 149 | policy[x][y] = delta_name[i] 150 | return value, policy 151 | 152 | r1, r2 = stochastic_value() 153 | print r2 -------------------------------------------------------------------------------- /Homework 05/hw5_2.py: -------------------------------------------------------------------------------- 1 | # cyclic smoothing exercise 2 | # Thomas Rieder 3 | # 4 | 5 | # ------------- 6 | # User Instructions 7 | # 8 | # Here you will be implementing a cyclic smoothing 9 | # algorithm. This algorithm should not fix the end 10 | # points (as you did in the unit quizzes). You 11 | # should use the gradient descent equations that 12 | # you used previously. 13 | # 14 | # Your function should return the newpath that it 15 | # calculates.. 16 | # 17 | # Feel free to use the provided solution_check function 18 | # to test your code. You can find it at the bottom. 19 | # 20 | # -------------- 21 | # Testing Instructions 22 | # 23 | # To test your code, call the solution_check function with 24 | # two arguments. The first argument should be the result of your 25 | # smooth function. The second should be the corresponding answer. 26 | # For example, calling 27 | # 28 | # solution_check(smooth(testpath1), answer1) 29 | # 30 | # should return True if your answer is correct and False if 31 | # it is not. 32 | 33 | from math import * 34 | 35 | # Do not modify path inside your function. 36 | path=[[0, 0], 37 | [1, 0], 38 | [2, 0], 39 | [3, 0], 40 | [4, 0], 41 | [5, 0], 42 | [6, 0], 43 | [6, 1], 44 | [6, 2], 45 | [6, 3], 46 | [5, 3], 47 | [4, 3], 48 | [3, 3], 49 | [2, 3], 50 | [1, 3], 51 | [0, 3], 52 | [0, 2], 53 | [0, 1]] 54 | 55 | ############# ONLY ENTER CODE BELOW THIS LINE ########## 56 | 57 | # ------------------------------------------------ 58 | # smooth coordinates 59 | # If your code is timing out, make the tolerance parameter 60 | # larger to decrease run time. 61 | # 62 | 63 | def smooth(path, weight_data = 0.1, weight_smooth = 0.1, tolerance = 0.00001): 64 | # deep copy 65 | newpath = [[0 for col in range(len(path[0]))] for row in range(len(path))] 66 | for i in range(len(path)): 67 | for j in range(len(path[0])): 68 | newpath[i][j] = path[i][j] 69 | 70 | change = 1 71 | while change > tolerance: 72 | change = 0.0 73 | for i in range(len(path)): 74 | for j in range(len(path[0])): 75 | aux = newpath[i][j] 76 | newpath[i][j] += weight_data * (path[i][j] - newpath[i][j]) 77 | newpath[i][j] += weight_smooth * (newpath[(i-1) % len(path)][j] + newpath[(i+1) % len(path)][j] - (2.0 * newpath[i][j])) 78 | change += abs(aux-newpath[i][j]) 79 | return newpath 80 | 81 | # thank you - EnTerr - for posting this on our discussion forum 82 | 83 | #newpath = smooth(path) 84 | #for i in range(len(path)): 85 | # print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']' 86 | 87 | 88 | ##### TESTING ###### 89 | 90 | # -------------------------------------------------- 91 | # check if two numbers are 'close enough,'used in 92 | # solution_check function. 93 | # 94 | def close_enough(user_answer, true_answer, epsilon = 0.001): 95 | if abs(user_answer - true_answer) > epsilon: 96 | return False 97 | return True 98 | 99 | # -------------------------------------------------- 100 | # check your solution against our reference solution for 101 | # a variety of test cases (given below) 102 | # 103 | def solution_check(newpath, answer): 104 | if type(newpath) != type(answer): 105 | print "Error. You do not return a list." 106 | return False 107 | if len(newpath) != len(answer): 108 | print 'Error. Your newpath is not the correct length.' 109 | return False 110 | if len(newpath[0]) != len(answer[0]): 111 | print 'Error. Your entries do not contain an (x, y) coordinate pair.' 112 | return False 113 | for i in range(len(newpath)): 114 | for j in range(len(newpath[0])): 115 | if not close_enough(newpath[i][j], answer[i][j]): 116 | print 'Error, at least one of your entries is not correct.' 117 | return False 118 | print "Test case correct!" 119 | return True 120 | 121 | # -------------- 122 | # Testing Instructions 123 | # 124 | # To test your code, call the solution_check function with 125 | # two arguments. The first argument should be the result of your 126 | # smooth function. The second should be the corresponding answer. 127 | # For example, calling 128 | # 129 | # solution_check(smooth(testpath1), answer1) 130 | # 131 | # should return True if your answer is correct and False if 132 | # it is not. 133 | 134 | testpath1 = [[0, 0], 135 | [1, 0], 136 | [2, 0], 137 | [3, 0], 138 | [4, 0], 139 | [5, 0], 140 | [6, 0], 141 | [6, 1], 142 | [6, 2], 143 | [6, 3], 144 | [5, 3], 145 | [4, 3], 146 | [3, 3], 147 | [2, 3], 148 | [1, 3], 149 | [0, 3], 150 | [0, 2], 151 | [0, 1]] 152 | 153 | answer1 = [[0.5449300156668018, 0.47485226780102946], 154 | [1.2230705677535505, 0.2046277687200752], 155 | [2.079668890615267, 0.09810778721159963], 156 | [3.0000020176660755, 0.07007646364781912], 157 | [3.9203348821839112, 0.09810853832382399], 158 | [4.7769324511170455, 0.20462917195702085], 159 | [5.455071854686622, 0.4748541381544533], 160 | [5.697264197153936, 1.1249625336275617], 161 | [5.697263485026567, 1.8750401628534337], 162 | [5.455069810373743, 2.5251482916876378], 163 | [4.776929339068159, 2.795372759575895], 164 | [3.92033110541304, 2.9018927284871063], 165 | [2.999998066091118, 2.929924058932193], 166 | [2.0796652780381826, 2.90189200881968], 167 | [1.2230677654766597, 2.7953714133566603], 168 | [0.544928391271399, 2.5251464933327794], 169 | [0.3027360471605494, 1.875038145804603], 170 | [0.302736726373967, 1.1249605602741133]] 171 | 172 | ## [0.000, 0.000] -> [0.545, 0.475] 173 | ## [1.000, 0.000] -> [1.223, 0.205] 174 | ## [2.000, 0.000] -> [2.080, 0.098] 175 | ## [3.000, 0.000] -> [3.000, 0.070] 176 | ## [4.000, 0.000] -> [3.920, 0.098] 177 | ## [5.000, 0.000] -> [4.777, 0.205] 178 | ## [6.000, 0.000] -> [5.455, 0.475] 179 | ## [6.000, 1.000] -> [5.697, 1.125] 180 | ## [6.000, 2.000] -> [5.697, 1.875] 181 | ## [6.000, 3.000] -> [5.455, 2.525] 182 | ## [5.000, 3.000] -> [4.777, 2.795] 183 | ## [4.000, 3.000] -> [3.920, 2.902] 184 | ## [3.000, 3.000] -> [3.000, 2.930] 185 | ## [2.000, 3.000] -> [2.080, 2.902] 186 | ## [1.000, 3.000] -> [1.223, 2.795] 187 | ## [0.000, 3.000] -> [0.545, 2.525] 188 | ## [0.000, 2.000] -> [0.303, 1.875] 189 | ## [0.000, 1.000] -> [0.303, 1.125] 190 | 191 | 192 | testpath2 = [[1, 0], # Move in the shape of a plus sign 193 | [2, 0], 194 | [2, 1], 195 | [3, 1], 196 | [3, 2], 197 | [2, 2], 198 | [2, 3], 199 | [1, 3], 200 | [1, 2], 201 | [0, 2], 202 | [0, 1], 203 | [1, 1]] 204 | 205 | answer2 = [[1.239080543767428, 0.5047204351187283], 206 | [1.7609243903912781, 0.5047216452560908], 207 | [2.0915039821562416, 0.9085017167753027], 208 | [2.495281862032503, 1.2390825203587184], 209 | [2.4952805300504783, 1.7609262468826048], 210 | [2.0915003641706296, 2.0915058211575475], 211 | [1.7609195135622062, 2.4952837841027695], 212 | [1.2390757942466555, 2.4952826072236918], 213 | [0.9084962737918979, 2.091502621431358], 214 | [0.5047183914625598, 1.7609219230352355], 215 | [0.504719649257698, 1.2390782835562297], 216 | [0.9084996902674257, 0.9084987462432871]] 217 | 218 | ## [1.000, 0.000] -> [1.239, 0.505] 219 | ## [2.000, 0.000] -> [1.761, 0.505] 220 | ## [2.000, 1.000] -> [2.092, 0.909] 221 | ## [3.000, 1.000] -> [2.495, 1.239] 222 | ## [3.000, 2.000] -> [2.495, 1.761] 223 | ## [2.000, 2.000] -> [2.092, 2.092] 224 | ## [2.000, 3.000] -> [1.761, 2.495] 225 | ## [1.000, 3.000] -> [1.239, 2.495] 226 | ## [1.000, 2.000] -> [0.908, 2.092] 227 | ## [0.000, 2.000] -> [0.505, 1.761] 228 | ## [0.000, 1.000] -> [0.505, 1.239] 229 | ## [1.000, 1.000] -> [0.908, 0.908] 230 | 231 | print '1st test:' 232 | solution_check(smooth(testpath1), answer1) 233 | 234 | print '2nd test:' 235 | solution_check(smooth(testpath2), answer2) 236 | 237 | 238 | 239 | 240 | -------------------------------------------------------------------------------- /Homework 05/hw5_3.py: -------------------------------------------------------------------------------- 1 | # cyclic smoothing (with fix points) exercise 2 | # Thomas Rieder 3 | # 4 | 5 | # ------------- 6 | # User Instructions 7 | # 8 | # Now you will be incorporating fixed points into 9 | # your smoother. 10 | # 11 | # You will need to use the equations from gradient 12 | # descent AND the new equations presented in the 13 | # previous lecture to implement smoothing with 14 | # fixed points. 15 | # 16 | # Your function should return the newpath that it 17 | # calculates. 18 | # 19 | # Feel free to use the provided solution_check function 20 | # to test your code. You can find it at the bottom. 21 | # 22 | # -------------- 23 | # Testing Instructions 24 | # 25 | # To test your code, call the solution_check function with 26 | # two arguments. The first argument should be the result of your 27 | # smooth function. The second should be the corresponding answer. 28 | # For example, calling 29 | # 30 | # solution_check(smooth(testpath1), answer1) 31 | # 32 | # should return True if your answer is correct and False if 33 | # it is not. 34 | 35 | from math import * 36 | 37 | # Do not modify path inside your function. 38 | path=[[0, 0], #fix 39 | [1, 0], 40 | [2, 0], 41 | [3, 0], 42 | [4, 0], 43 | [5, 0], 44 | [6, 0], #fix 45 | [6, 1], 46 | [6, 2], 47 | [6, 3], #fix 48 | [5, 3], 49 | [4, 3], 50 | [3, 3], 51 | [2, 3], 52 | [1, 3], 53 | [0, 3], #fix 54 | [0, 2], 55 | [0, 1]] 56 | 57 | # Do not modify fix inside your function 58 | fix = [1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0] 59 | 60 | ######################## ENTER CODE BELOW HERE ######################### 61 | 62 | def smooth(path, fix, weight_data = 0.0, weight_smooth = 0.1, tolerance = 0.00001): 63 | # deep copy 64 | newpath = [[0 for col in range(len(path[0]))] for row in range(len(path))] 65 | for i in range(len(path)): 66 | for j in range(len(path[0])): 67 | newpath[i][j] = path[i][j] 68 | 69 | change = 1 70 | while change > tolerance: 71 | change = 0.0 72 | for i in range(len(path)): 73 | if not fix[i]: 74 | for j in range(len(path[0])): 75 | aux = newpath[i][j] 76 | newpath[i][j] += weight_data * (path[i][j] - newpath[i][j]) 77 | newpath[i][j] += weight_smooth * (newpath[(i-1) % len(path)][j] + newpath[(i+1) % len(path)][j] - 2.0 * newpath[i][j]) 78 | newpath[i][j] += 0.5 * weight_smooth * (2.0 * newpath[(i-1) % len(path)][j] - newpath[(i-2) % len(path)][j] - newpath[i][j]) 79 | newpath[i][j] += 0.5 * weight_smooth * (2.0 * newpath[(i+1) % len(path)][j] - newpath[(i+2) % len(path)][j] - newpath[i][j]) 80 | change += abs(aux-newpath[i][j]) 81 | return newpath 82 | 83 | 84 | 85 | #thank you - EnTerr - for posting this on our discussion forum 86 | 87 | ##newpath = smooth(path) 88 | ##for i in range(len(path)): 89 | ## print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']' 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.03): 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], #fix 136 | [1, 0], 137 | [2, 0], 138 | [3, 0], 139 | [4, 0], 140 | [5, 0], 141 | [6, 0], #fix 142 | [6, 1], 143 | [6, 2], 144 | [6, 3], #fix 145 | [5, 3], 146 | [4, 3], 147 | [3, 3], 148 | [2, 3], 149 | [1, 3], 150 | [0, 3], #fix 151 | [0, 2], 152 | [0, 1]] 153 | testfix1 = [1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0] 154 | answer1 = [[0, 0], 155 | [0.7938620981547201, -0.8311168821106101], 156 | [1.8579052986461084, -1.3834788165869276], 157 | [3.053905318597796, -1.5745863173084], 158 | [4.23141390533387, -1.3784271816058231], 159 | [5.250184859723701, -0.8264215958231558], 160 | [6, 0], 161 | [6.415150091996651, 0.9836951698796843], 162 | [6.41942442687092, 2.019512290770163], 163 | [6, 3], 164 | [5.206131365604606, 3.831104483245191], 165 | [4.142082497497067, 4.383455704596517], 166 | [2.9460804122779813, 4.5745592975708105], 167 | [1.768574219397359, 4.378404668718541], 168 | [0.7498089205417316, 3.826409771585794], 169 | [0, 3], 170 | [-0.4151464728194156, 2.016311854977891], 171 | [-0.4194207879552198, 0.9804948340550833]] 172 | 173 | testpath2 = [[0, 0], # fix 174 | [2, 0], 175 | [4, 0], # fix 176 | [4, 2], 177 | [4, 4], # fix 178 | [2, 4], 179 | [0, 4], # fix 180 | [0, 2]] 181 | testfix2 = [1, 0, 1, 0, 1, 0, 1, 0] 182 | answer2 = [[0, 0], 183 | [2.0116767115496095, -0.7015439080661671], 184 | [4, 0], 185 | [4.701543905420104, 2.0116768147460418], 186 | [4, 4], 187 | [1.9883231877640861, 4.701543807525115], 188 | [0, 4], 189 | [-0.7015438099112995, 1.9883232808252207]] 190 | 191 | 192 | print '1st test:' 193 | solution_check(smooth(testpath1, testfix1), answer1) 194 | print '2nd test:' 195 | solution_check(smooth(testpath2, testfix2), answer2) 196 | 197 | -------------------------------------------------------------------------------- /Homework 05/hw5_4.py: -------------------------------------------------------------------------------- 1 | # cte-function for pid-control 2 | # Thomas Rieder 3 | # 4 | 5 | # -------------- 6 | # User Instructions 7 | # 8 | # Define a function cte in the robot class that will 9 | # compute the crosstrack error for a robot on a 10 | # racetrack with a shape as described in the video. 11 | # 12 | # You will need to base your error calculation on 13 | # the robot's location on the track. Remember that 14 | # the robot will be traveling to the right on the 15 | # upper straight segment and to the left on the lower 16 | # straight segment. 17 | # 18 | # -------------- 19 | # Grading Notes 20 | # 21 | # We will be testing your cte function directly by 22 | # calling it with different robot locations and making 23 | # sure that it returns the correct crosstrack error. 24 | 25 | from math import * 26 | import random 27 | 28 | 29 | # ------------------------------------------------ 30 | # 31 | # this is the robot class 32 | # 33 | 34 | class robot: 35 | 36 | # -------- 37 | # init: 38 | # creates robot and initializes location/orientation to 0, 0, 0 39 | # 40 | 41 | def __init__(self, length = 20.0): 42 | self.x = 0.0 43 | self.y = 0.0 44 | self.orientation = 0.0 45 | self.length = length 46 | self.steering_noise = 0.0 47 | self.distance_noise = 0.0 48 | self.steering_drift = 0.0 49 | 50 | # -------- 51 | # set: 52 | # sets a robot coordinate 53 | # 54 | 55 | def set(self, new_x, new_y, new_orientation): 56 | 57 | self.x = float(new_x) 58 | self.y = float(new_y) 59 | self.orientation = float(new_orientation) % (2.0 * pi) 60 | 61 | 62 | # -------- 63 | # set_noise: 64 | # sets the noise parameters 65 | # 66 | 67 | def set_noise(self, new_s_noise, new_d_noise): 68 | # makes it possible to change the noise parameters 69 | # this is often useful in particle filters 70 | self.steering_noise = float(new_s_noise) 71 | self.distance_noise = float(new_d_noise) 72 | 73 | # -------- 74 | # set_steering_drift: 75 | # sets the systematical steering drift parameter 76 | # 77 | 78 | def set_steering_drift(self, drift): 79 | self.steering_drift = drift 80 | 81 | # -------- 82 | # move: 83 | # steering = front wheel steering angle, limited by max_steering_angle 84 | # distance = total distance driven, most be non-negative 85 | 86 | def move(self, steering, distance, 87 | tolerance = 0.001, max_steering_angle = pi / 4.0): 88 | 89 | if steering > max_steering_angle: 90 | steering = max_steering_angle 91 | if steering < -max_steering_angle: 92 | steering = -max_steering_angle 93 | if distance < 0.0: 94 | distance = 0.0 95 | 96 | 97 | # make a new copy 98 | res = robot() 99 | res.length = self.length 100 | res.steering_noise = self.steering_noise 101 | res.distance_noise = self.distance_noise 102 | res.steering_drift = self.steering_drift 103 | 104 | # apply noise 105 | steering2 = random.gauss(steering, self.steering_noise) 106 | distance2 = random.gauss(distance, self.distance_noise) 107 | 108 | # apply steering drift 109 | steering2 += self.steering_drift 110 | 111 | # Execute motion 112 | turn = tan(steering2) * distance2 / res.length 113 | 114 | if abs(turn) < tolerance: 115 | 116 | # approximate by straight line motion 117 | 118 | res.x = self.x + (distance2 * cos(self.orientation)) 119 | res.y = self.y + (distance2 * sin(self.orientation)) 120 | res.orientation = (self.orientation + turn) % (2.0 * pi) 121 | 122 | else: 123 | 124 | # approximate bicycle model for motion 125 | 126 | radius = distance2 / turn 127 | cx = self.x - (sin(self.orientation) * radius) 128 | cy = self.y + (cos(self.orientation) * radius) 129 | res.orientation = (self.orientation + turn) % (2.0 * pi) 130 | res.x = cx + (sin(res.orientation) * radius) 131 | res.y = cy - (cos(res.orientation) * radius) 132 | 133 | return res 134 | 135 | 136 | 137 | 138 | def __repr__(self): 139 | return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) 140 | 141 | 142 | ############## ONLY ADD / MODIFY CODE BELOW THIS LINE #################### 143 | 144 | def cte(self, radius): 145 | x = self.x 146 | y = self.y 147 | if x >= radius and x <= 3 * radius: 148 | # robot is near the straight lines 149 | if y >= radius: 150 | # upper line 151 | cte = y - 2 * radius 152 | else: 153 | # lower line 154 | cte = y * -1 155 | else: 156 | # robot is near one of the circles 157 | if x <= radius: 158 | # left circle 159 | # caculate the distance between the position of the robot and the center of the circle 160 | cte = sqrt((radius - x )**2 + (radius - y)**2) - radius 161 | else: 162 | # right circle 163 | cte = sqrt(((3 * radius) - x)**2 + (radius - y)**2) - radius 164 | return cte 165 | 166 | ############## ONLY ADD / MODIFY CODE ABOVE THIS LINE #################### 167 | 168 | 169 | 170 | 171 | # ------------------------------------------------------------------------ 172 | # 173 | # run - does a single control run. 174 | 175 | 176 | def run(params, radius, printflag = True): 177 | myrobot = robot() 178 | myrobot.set(0.0, radius, pi / 2.0) 179 | speed = 1.0 # motion distance is equal to speed (we assume time = 1) 180 | err = 0.0 181 | int_crosstrack_error = 0.0 182 | N = 200 183 | crosstrack_error = myrobot.cte(radius) # You need to define the cte function! 184 | 185 | for i in range(N*2): 186 | diff_crosstrack_error = - crosstrack_error 187 | crosstrack_error = myrobot.cte(radius) 188 | diff_crosstrack_error += crosstrack_error 189 | int_crosstrack_error += crosstrack_error 190 | steer = - params[0] * crosstrack_error \ 191 | - params[1] * diff_crosstrack_error \ 192 | - params[2] * int_crosstrack_error 193 | myrobot = myrobot.move(steer, speed) 194 | if i >= N: 195 | err += crosstrack_error ** 2 196 | if printflag: 197 | print myrobot 198 | return err / float(N) 199 | 200 | radius = 25.0 201 | params = [10.0, 15.0, 0] 202 | err = run(params, radius, True) 203 | print '\nFinal paramaeters: ', params, '\n ->', err 204 | -------------------------------------------------------------------------------- /Homework 06/hw6.py: -------------------------------------------------------------------------------- 1 | # online slam exercise 2 | # Thomas Rieder 3 | # 4 | 5 | # ------------ 6 | # User Instructions 7 | # 8 | # In this problem you will implement a more manageable 9 | # version of graph SLAM in 2 dimensions. 10 | # 11 | # Define a function, online_slam, that takes 5 inputs: 12 | # data, N, num_landmarks, motion_noise, and 13 | # measurement_noise--just as was done in the last 14 | # programming assignment of unit 6. This function 15 | # must return TWO matrices, mu and the final Omega. 16 | # 17 | # Just as with the quiz, your matrices should have x 18 | # and y interlaced, so if there were two poses and 2 19 | # landmarks, mu would look like: 20 | # 21 | # mu = matrix([[Px0], 22 | # [Py0], 23 | # [Px1], 24 | # [Py1], 25 | # [Lx0], 26 | # [Ly0], 27 | # [Lx1], 28 | # [Ly1]]) 29 | # 30 | # Enter your code at line 566. 31 | 32 | # ----------- 33 | # Testing 34 | # 35 | # You have two methods for testing your code. 36 | # 37 | # 1) You can make your own data with the make_data 38 | # function. Then you can run it through the 39 | # provided slam routine and check to see that your 40 | # online_slam function gives the same estimated 41 | # final robot pose and landmark positions. 42 | # 2) You can use the solution_check function at the 43 | # bottom of this document to check your code 44 | # for the two provided test cases. The grading 45 | # will be almost identical to this function, so 46 | # if you pass both test cases, you should be 47 | # marked correct on the homework. 48 | 49 | from math import * 50 | import random 51 | 52 | 53 | # ------------------------------------------------ 54 | # 55 | # this is the matrix class 56 | # we use it because it makes it easier to collect constraints in GraphSLAM 57 | # and to calculate solutions (albeit inefficiently) 58 | # 59 | 60 | class matrix: 61 | 62 | # implements basic operations of a matrix class 63 | 64 | # ------------ 65 | # 66 | # initialization - can be called with an initial matrix 67 | # 68 | 69 | def __init__(self, value = [[]]): 70 | self.value = value 71 | self.dimx = len(value) 72 | self.dimy = len(value[0]) 73 | if value == [[]]: 74 | self.dimx = 0 75 | 76 | # ----------- 77 | # 78 | # defines matrix equality - returns true if corresponding elements 79 | # in two matrices are within epsilon of each other. 80 | # 81 | 82 | def __eq__(self, other): 83 | epsilon = 0.01 84 | if self.dimx != other.dimx or self.dimy != other.dimy: 85 | return False 86 | for i in range(self.dimx): 87 | for j in range(self.dimy): 88 | if abs(self.value[i][j] - other.value[i][j]) > epsilon: 89 | return False 90 | return True 91 | 92 | def __ne__(self, other): 93 | return not (self == other) 94 | 95 | # ------------ 96 | # 97 | # makes matrix of a certain size and sets each element to zero 98 | # 99 | 100 | def zero(self, dimx, dimy): 101 | if dimy == 0: 102 | dimy = dimx 103 | # check if valid dimensions 104 | if dimx < 1 or dimy < 1: 105 | raise ValueError, "Invalid size of matrix" 106 | else: 107 | self.dimx = dimx 108 | self.dimy = dimy 109 | self.value = [[0.0 for row in range(dimy)] for col in range(dimx)] 110 | 111 | # ------------ 112 | # 113 | # makes matrix of a certain (square) size and turns matrix into identity matrix 114 | # 115 | 116 | def identity(self, dim): 117 | # check if valid dimension 118 | if dim < 1: 119 | raise ValueError, "Invalid size of matrix" 120 | else: 121 | self.dimx = dim 122 | self.dimy = dim 123 | self.value = [[0.0 for row in range(dim)] for col in range(dim)] 124 | for i in range(dim): 125 | self.value[i][i] = 1.0 126 | 127 | # ------------ 128 | # 129 | # prints out values of matrix 130 | # 131 | 132 | def show(self, txt = ''): 133 | for i in range(len(self.value)): 134 | print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']' 135 | print ' ' 136 | 137 | # ------------ 138 | # 139 | # defines elmement-wise matrix addition. Both matrices must be of equal dimensions 140 | # 141 | 142 | def __add__(self, other): 143 | # check if correct dimensions 144 | if self.dimx != other.dimx or self.dimx != other.dimx: 145 | raise ValueError, "Matrices must be of equal dimension to add" 146 | else: 147 | # add if correct dimensions 148 | res = matrix() 149 | res.zero(self.dimx, self.dimy) 150 | for i in range(self.dimx): 151 | for j in range(self.dimy): 152 | res.value[i][j] = self.value[i][j] + other.value[i][j] 153 | return res 154 | 155 | # ------------ 156 | # 157 | # defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions 158 | # 159 | 160 | def __sub__(self, other): 161 | # check if correct dimensions 162 | if self.dimx != other.dimx or self.dimx != other.dimx: 163 | raise ValueError, "Matrices must be of equal dimension to subtract" 164 | else: 165 | # subtract if correct dimensions 166 | res = matrix() 167 | res.zero(self.dimx, self.dimy) 168 | for i in range(self.dimx): 169 | for j in range(self.dimy): 170 | res.value[i][j] = self.value[i][j] - other.value[i][j] 171 | return res 172 | 173 | # ------------ 174 | # 175 | # defines multiplication. Both matrices must be of fitting dimensions 176 | # 177 | 178 | def __mul__(self, other): 179 | # check if correct dimensions 180 | if self.dimy != other.dimx: 181 | raise ValueError, "Matrices must be m*n and n*p to multiply" 182 | else: 183 | # multiply if correct dimensions 184 | res = matrix() 185 | res.zero(self.dimx, other.dimy) 186 | for i in range(self.dimx): 187 | for j in range(other.dimy): 188 | for k in range(self.dimy): 189 | res.value[i][j] += self.value[i][k] * other.value[k][j] 190 | return res 191 | 192 | # ------------ 193 | # 194 | # returns a matrix transpose 195 | # 196 | 197 | def transpose(self): 198 | # compute transpose 199 | res = matrix() 200 | res.zero(self.dimy, self.dimx) 201 | for i in range(self.dimx): 202 | for j in range(self.dimy): 203 | res.value[j][i] = self.value[i][j] 204 | return res 205 | 206 | # ------------ 207 | # 208 | # creates a new matrix from the existing matrix elements. 209 | # 210 | # Example: 211 | # l = matrix([[ 1, 2, 3, 4, 5], 212 | # [ 6, 7, 8, 9, 10], 213 | # [11, 12, 13, 14, 15]]) 214 | # 215 | # l.take([0, 2], [0, 2, 3]) 216 | # 217 | # results in: 218 | # 219 | # [[1, 3, 4], 220 | # [11, 13, 14]] 221 | # 222 | # 223 | # take is used to remove rows and columns from existing matrices 224 | # list1/list2 define a sequence of rows/columns that shall be taken 225 | # is no list2 is provided, then list2 is set to list1 (good for symmetric matrices) 226 | # 227 | 228 | def take(self, list1, list2 = []): 229 | if list2 == []: 230 | list2 = list1 231 | if len(list1) > self.dimx or len(list2) > self.dimy: 232 | raise ValueError, "list invalid in take()" 233 | 234 | res = matrix() 235 | res.zero(len(list1), len(list2)) 236 | for i in range(len(list1)): 237 | for j in range(len(list2)): 238 | res.value[i][j] = self.value[list1[i]][list2[j]] 239 | return res 240 | 241 | # ------------ 242 | # 243 | # creates a new matrix from the existing matrix elements. 244 | # 245 | # Example: 246 | # l = matrix([[1, 2, 3], 247 | # [4, 5, 6]]) 248 | # 249 | # l.expand(3, 5, [0, 2], [0, 2, 3]) 250 | # 251 | # results in: 252 | # 253 | # [[1, 0, 2, 3, 0], 254 | # [0, 0, 0, 0, 0], 255 | # [4, 0, 5, 6, 0]] 256 | # 257 | # expand is used to introduce new rows and columns into an existing matrix 258 | # list1/list2 are the new indexes of row/columns in which the matrix 259 | # elements are being mapped. Elements for rows and columns 260 | # that are not listed in list1/list2 261 | # will be initialized by 0.0. 262 | # 263 | 264 | def expand(self, dimx, dimy, list1, list2 = []): 265 | if list2 == []: 266 | list2 = list1 267 | if len(list1) > self.dimx or len(list2) > self.dimy: 268 | raise ValueError, "list invalid in expand()" 269 | 270 | res = matrix() 271 | res.zero(dimx, dimy) 272 | for i in range(len(list1)): 273 | for j in range(len(list2)): 274 | res.value[list1[i]][list2[j]] = self.value[i][j] 275 | return res 276 | 277 | # ------------ 278 | # 279 | # Computes the upper triangular Cholesky factorization of 280 | # a positive definite matrix. 281 | # This code is based on http://adorio-research.org/wordpress/?p=4560 282 | 283 | def Cholesky(self, ztol= 1.0e-5): 284 | 285 | res = matrix() 286 | res.zero(self.dimx, self.dimx) 287 | 288 | for i in range(self.dimx): 289 | S = sum([(res.value[k][i])**2 for k in range(i)]) 290 | d = self.value[i][i] - S 291 | if abs(d) < ztol: 292 | res.value[i][i] = 0.0 293 | else: 294 | if d < 0.0: 295 | raise ValueError, "Matrix not positive-definite" 296 | res.value[i][i] = sqrt(d) 297 | for j in range(i+1, self.dimx): 298 | S = sum([res.value[k][i] * res.value[k][j] for k in range(i)]) 299 | if abs(S) < ztol: 300 | S = 0.0 301 | res.value[i][j] = (self.value[i][j] - S)/res.value[i][i] 302 | return res 303 | 304 | # ------------ 305 | # 306 | # Computes inverse of matrix given its Cholesky upper Triangular 307 | # decomposition of matrix. 308 | # This code is based on http://adorio-research.org/wordpress/?p=4560 309 | 310 | def CholeskyInverse(self): 311 | # Computes inverse of matrix given its Cholesky upper Triangular 312 | # decomposition of matrix. 313 | # This code is based on http://adorio-research.org/wordpress/?p=4560 314 | 315 | res = matrix() 316 | res.zero(self.dimx, self.dimx) 317 | 318 | # Backward step for inverse. 319 | for j in reversed(range(self.dimx)): 320 | tjj = self.value[j][j] 321 | S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)]) 322 | res.value[j][j] = 1.0/ tjj**2 - S/ tjj 323 | for i in reversed(range(j)): 324 | res.value[j][i] = res.value[i][j] = \ 325 | -sum([self.value[i][k]*res.value[k][j] for k in \ 326 | range(i+1,self.dimx)])/self.value[i][i] 327 | return res 328 | 329 | # ------------ 330 | # 331 | # comutes and returns the inverse of a square matrix 332 | # 333 | 334 | def inverse(self): 335 | aux = self.Cholesky() 336 | res = aux.CholeskyInverse() 337 | return res 338 | 339 | # ------------ 340 | # 341 | # prints matrix (needs work!) 342 | # 343 | 344 | def __repr__(self): 345 | return repr(self.value) 346 | 347 | # ###################################################################### 348 | 349 | # ------------------------------------------------ 350 | # 351 | # this is the robot class 352 | # 353 | # our robot lives in x-y space, and its motion is 354 | # pointed in a random direction. It moves on a straight line 355 | # until is comes close to a wall at which point it turns 356 | # away from the wall and continues to move. 357 | # 358 | # For measurements, it simply senses the x- and y-distance 359 | # to landmarks. This is different from range and bearing as 360 | # commonly studies in the literature, but this makes it much 361 | # easier to implement the essentials of SLAM without 362 | # cluttered math 363 | # 364 | 365 | class robot: 366 | 367 | # -------- 368 | # init: 369 | # creates robot and initializes location to 0, 0 370 | # 371 | 372 | def __init__(self, world_size = 100.0, measurement_range = 30.0, 373 | motion_noise = 1.0, measurement_noise = 1.0): 374 | self.measurement_noise = 0.0 375 | self.world_size = world_size 376 | self.measurement_range = measurement_range 377 | self.x = world_size / 2.0 378 | self.y = world_size / 2.0 379 | self.motion_noise = motion_noise 380 | self.measurement_noise = measurement_noise 381 | self.landmarks = [] 382 | self.num_landmarks = 0 383 | 384 | 385 | def rand(self): 386 | return random.random() * 2.0 - 1.0 387 | 388 | # -------- 389 | # 390 | # make random landmarks located in the world 391 | # 392 | 393 | def make_landmarks(self, num_landmarks): 394 | self.landmarks = [] 395 | for i in range(num_landmarks): 396 | self.landmarks.append([round(random.random() * self.world_size), 397 | round(random.random() * self.world_size)]) 398 | self.num_landmarks = num_landmarks 399 | 400 | # -------- 401 | # 402 | # move: attempts to move robot by dx, dy. If outside world 403 | # boundary, then the move does nothing and instead returns failure 404 | # 405 | 406 | def move(self, dx, dy): 407 | 408 | x = self.x + dx + self.rand() * self.motion_noise 409 | y = self.y + dy + self.rand() * self.motion_noise 410 | 411 | if x < 0.0 or x > self.world_size or y < 0.0 or y > self.world_size: 412 | return False 413 | else: 414 | self.x = x 415 | self.y = y 416 | return True 417 | 418 | # -------- 419 | # 420 | # sense: returns x- and y- distances to landmarks within visibility range 421 | # because not all landmarks may be in this range, the list of measurements 422 | # is of variable length. Set measurement_range to -1 if you want all 423 | # landmarks to be visible at all times 424 | # 425 | 426 | def sense(self): 427 | Z = [] 428 | for i in range(self.num_landmarks): 429 | dx = self.landmarks[i][0] - self.x + self.rand() * self.measurement_noise 430 | dy = self.landmarks[i][1] - self.y + self.rand() * self.measurement_noise 431 | if self.measurement_range < 0.0 or abs(dx) + abs(dy) <= self.measurement_range: 432 | Z.append([i, dx, dy]) 433 | return Z 434 | 435 | # -------- 436 | # 437 | # print robot location 438 | # 439 | 440 | def __repr__(self): 441 | return 'Robot: [x=%.5f y=%.5f]' % (self.x, self.y) 442 | 443 | 444 | # ###################################################################### 445 | 446 | # -------- 447 | # this routine makes the robot data 448 | # 449 | 450 | def make_data(N, num_landmarks, world_size, measurement_range, motion_noise, 451 | measurement_noise, distance): 452 | 453 | complete = False 454 | 455 | while not complete: 456 | 457 | data = [] 458 | 459 | # make robot and landmarks 460 | r = robot(world_size, measurement_range, motion_noise, measurement_noise) 461 | r.make_landmarks(num_landmarks) 462 | seen = [False for row in range(num_landmarks)] 463 | 464 | # guess an initial motion 465 | orientation = random.random() * 2.0 * pi 466 | dx = cos(orientation) * distance 467 | dy = sin(orientation) * distance 468 | 469 | for k in range(N-1): 470 | 471 | # sense 472 | Z = r.sense() 473 | 474 | # check off all landmarks that were observed 475 | for i in range(len(Z)): 476 | seen[Z[i][0]] = True 477 | 478 | # move 479 | while not r.move(dx, dy): 480 | # if we'd be leaving the robot world, pick instead a new direction 481 | orientation = random.random() * 2.0 * pi 482 | dx = cos(orientation) * distance 483 | dy = sin(orientation) * distance 484 | 485 | # memorize data 486 | data.append([Z, [dx, dy]]) 487 | 488 | # we are done when all landmarks were observed; otherwise re-run 489 | complete = (sum(seen) == num_landmarks) 490 | 491 | print ' ' 492 | print 'Landmarks: ', r.landmarks 493 | print r 494 | 495 | return data 496 | 497 | # ###################################################################### 498 | 499 | # -------------------------------- 500 | # 501 | # full_slam - retains entire path and all landmarks 502 | # Feel free to use this for comparison. 503 | # 504 | 505 | def slam(data, N, num_landmarks, motion_noise, measurement_noise): 506 | 507 | # Set the dimension of the filter 508 | dim = 2 * (N + num_landmarks) 509 | 510 | # make the constraint information matrix and vector 511 | Omega = matrix() 512 | Omega.zero(dim, dim) 513 | Omega.value[0][0] = 1.0 514 | Omega.value[1][1] = 1.0 515 | 516 | Xi = matrix() 517 | Xi.zero(dim, 1) 518 | Xi.value[0][0] = world_size / 2.0 519 | Xi.value[1][0] = world_size / 2.0 520 | 521 | # process the data 522 | 523 | for k in range(len(data)): 524 | 525 | # n is the index of the robot pose in the matrix/vector 526 | n = k * 2 527 | 528 | measurement = data[k][0] 529 | motion = data[k][1] 530 | 531 | # integrate the measurements 532 | for i in range(len(measurement)): 533 | 534 | # m is the index of the landmark coordinate in the matrix/vector 535 | m = 2 * (N + measurement[i][0]) 536 | 537 | # update the information maxtrix/vector based on the measurement 538 | for b in range(2): 539 | Omega.value[n+b][n+b] += 1.0 / measurement_noise 540 | Omega.value[m+b][m+b] += 1.0 / measurement_noise 541 | Omega.value[n+b][m+b] += -1.0 / measurement_noise 542 | Omega.value[m+b][n+b] += -1.0 / measurement_noise 543 | Xi.value[n+b][0] += -measurement[i][1+b] / measurement_noise 544 | Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise 545 | 546 | 547 | # update the information maxtrix/vector based on the robot motion 548 | for b in range(4): 549 | Omega.value[n+b][n+b] += 1.0 / motion_noise 550 | for b in range(2): 551 | Omega.value[n+b ][n+b+2] += -1.0 / motion_noise 552 | Omega.value[n+b+2][n+b ] += -1.0 / motion_noise 553 | Xi.value[n+b ][0] += -motion[b] / motion_noise 554 | Xi.value[n+b+2][0] += motion[b] / motion_noise 555 | 556 | # compute best estimate 557 | mu = Omega.inverse() * Xi 558 | 559 | # return the result 560 | return mu 561 | 562 | # -------------------------------- 563 | # 564 | # online_slam - retains all landmarks but only most recent robot pose 565 | # 566 | 567 | def online_slam(data, N, num_landmarks, motion_noise, measurement_noise): 568 | # number of dimensions of the filter 569 | # enter your code here 570 | 571 | dim = 2 * (1 + num_landmarks) 572 | 573 | # initalize matrices 574 | Omega = matrix() 575 | Omega.zero(dim, dim) 576 | Omega.value[0][0] = 1. 577 | Omega.value[1][1] = 1. 578 | 579 | Xi = matrix() 580 | Xi.zero(dim, 1) 581 | Xi.value[0][0] = world_size / 2. 582 | Xi.value[1][0] = world_size / 2. 583 | 584 | # process the data 585 | list1 = [0, 1] 586 | list2 = [0, 1] 587 | list3 = [] 588 | 589 | for i in range(4, dim + 2): 590 | list1.append(i) 591 | list2.append(i) 592 | for i in range(2, dim + 2): 593 | list3.append(i) 594 | 595 | for k in range(len(data)): 596 | # n is the robot position in the matrix 597 | n = 0 598 | Omega = Omega.expand(dim + 2, dim + 2, list1, list2) 599 | Xi = Xi.expand(dim + 2, 1, list1, [0]) 600 | measurement = data[k][0] 601 | motion = data[k][1] 602 | 603 | # measurement update 604 | for i in range(len(measurement)): 605 | 606 | # m is the landmark index 607 | m = 2 * (2 + measurement[i][0]) 608 | 609 | # matrix/vector update 610 | for j in range(2): 611 | Omega.value[n + j][n + j] += 1. / measurement_noise 612 | Omega.value[m + j][m + j] += 1. / measurement_noise 613 | Omega.value[n + j][m + j] += -1. / measurement_noise 614 | Omega.value[m + j][n + j] += -1. / measurement_noise 615 | Xi.value[n + j][0] += -measurement[i][1+j] / measurement_noise 616 | Xi.value[m + j][0] += measurement[i][1+j] / measurement_noise 617 | 618 | 619 | # motion update 620 | for i in range(4): 621 | Omega.value[n + i][n + i] += 1. / motion_noise 622 | for i in range(2): 623 | Omega.value[n + i][n + i + 2] += -1. / motion_noise 624 | Omega.value[n + i + 2][n + i] += -1. / motion_noise 625 | Xi.value[n + i][0] += -motion[i] / motion_noise 626 | Xi.value[n + i + 2][0] += motion[i] / motion_noise 627 | 628 | 629 | # get temporary matrices 630 | A = Omega.take([0, 1], list3) 631 | B = Omega.take([0, 1], [0, 1]) 632 | C = Xi.take([0, 1], [0]) 633 | Omega1 = Omega.take(list3, list3) 634 | Xi1 = Xi.take(list3, [0]) 635 | 636 | # compute new matrices for next round 637 | Omega = Omega1 - ((A.transpose()) * (B.inverse()) * (A)) 638 | Xi = Xi1 - ((A.transpose()) * (B.inverse()) * (C)) 639 | 640 | 641 | # best estimate 642 | mu = Omega.inverse() * Xi 643 | 644 | return mu, Omega # make sure you return both of these matrices to be marked correct. 645 | 646 | # -------------------------------- 647 | # 648 | # print the result of SLAM, the robot pose(s) and the landmarks 649 | # 650 | 651 | def print_result(N, num_landmarks, result): 652 | print 653 | print 'Estimated Pose(s):' 654 | for i in range(N): 655 | print ' ['+ ', '.join('%.3f'%x for x in result.value[2*i]) + ', ' \ 656 | + ', '.join('%.3f'%x for x in result.value[2*i+1]) +']' 657 | print 658 | print 'Estimated Landmarks:' 659 | for i in range(num_landmarks): 660 | print ' ['+ ', '.join('%.3f'%x for x in result.value[2*(N+i)]) + ', ' \ 661 | + ', '.join('%.3f'%x for x in result.value[2*(N+i)+1]) +']' 662 | 663 | # ------------------------------------------------------------------------ 664 | # 665 | # Main routines 666 | # 667 | 668 | num_landmarks = 5 # number of landmarks 669 | N = 20 # time steps 670 | world_size = 100.0 # size of world 671 | measurement_range = 50.0 # range at which we can sense landmarks 672 | motion_noise = 2.0 # noise in robot motion 673 | measurement_noise = 2.0 # noise in the measurements 674 | distance = 20.0 # distance by which robot (intends to) move each iteratation 675 | 676 | 677 | # Uncomment the following three lines to run the full slam routine. 678 | 679 | print 'Running full slam routine' 680 | data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance) 681 | result = slam(data, N, num_landmarks, motion_noise, measurement_noise) 682 | print_result(N, num_landmarks, result) 683 | print '' 684 | 685 | # Uncomment the following three lines to run the online_slam routine. 686 | 687 | print 'Running online slam routine' 688 | data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance) 689 | result = online_slam(data, N, num_landmarks, motion_noise, measurement_noise) 690 | print_result(1, num_landmarks, result[0]) 691 | print '' 692 | 693 | ########################################################## 694 | 695 | # ------------ 696 | # TESTING 697 | # 698 | # Uncomment one of the test cases below to check that your 699 | # online_slam function works as expected. 700 | 701 | def solution_check(result, answer_mu, answer_omega): 702 | 703 | if len(result) != 2: 704 | print "Your function must return TWO matrices, mu and Omega" 705 | return False 706 | 707 | user_mu = result[0] 708 | user_omega = result[1] 709 | 710 | if user_mu.dimx == answer_omega.dimx and user_mu.dimy == answer_omega.dimy: 711 | print "It looks like you returned your results in the wrong order. Make sure to return mu then Omega." 712 | return False 713 | 714 | if user_mu.dimx != answer_mu.dimx or user_mu.dimy != answer_mu.dimy: 715 | print "Your mu matrix doesn't have the correct dimensions. Mu should be a", answer_mu.dimx, " x ", answer_mu.dimy, "matrix." 716 | return False 717 | else: 718 | print "Mu has correct dimensions." 719 | 720 | if user_omega.dimx != answer_omega.dimx or user_omega.dimy != answer_omega.dimy: 721 | print "Your Omega matrix doesn't have the correct dimensions. Omega should be a", answer_omega.dimx, " x ", answer_omega.dimy, "matrix." 722 | return False 723 | else: 724 | print "Omega has correct dimensions." 725 | 726 | if user_mu != answer_mu: 727 | print "Mu has incorrect entries." 728 | return False 729 | else: 730 | print "Mu correct." 731 | 732 | if user_omega != answer_omega: 733 | print "Omega has incorrect entries." 734 | return False 735 | else: 736 | print "Omega correct." 737 | 738 | print "Test case passed!" 739 | return True 740 | 741 | # ----------- 742 | # Test Case 1 743 | 744 | testdata1 = [[[[1, 21.796713239511305, 25.32184135169971], [2, 15.067410969755826, -27.599928007267906]], [16.4522379034509, -11.372065246394495]], 745 | [[[1, 6.1286996178786755, 35.70844618389858], [2, -0.7470113490937167, -17.709326161950294]], [16.4522379034509, -11.372065246394495]], 746 | [[[0, 16.305692184072235, -11.72765549112342], [2, -17.49244296888888, -5.371360408288514]], [16.4522379034509, -11.372065246394495]], 747 | [[[0, -0.6443452578030207, -2.542378369361001], [2, -32.17857547483552, 6.778675958806988]], [-16.66697847355152, 11.054945886894709]]] 748 | 749 | answer_mu1 = matrix([[81.63549976607898], 750 | [27.175270706192254], 751 | [98.09737507003692], 752 | [14.556272940621195], 753 | [71.97926631050574], 754 | [75.07644206765099], 755 | [65.30397603859097], 756 | [22.150809430682695]]) 757 | 758 | answer_omega1 = matrix([[0.36603773584905663, 0.0, -0.169811320754717, 0.0, -0.011320754716981133, 0.0, -0.1811320754716981, 0.0], 759 | [0.0, 0.36603773584905663, 0.0, -0.169811320754717, 0.0, -0.011320754716981133, 0.0, -0.1811320754716981], 760 | [-0.169811320754717, 0.0, 0.6509433962264151, 0.0, -0.05660377358490567, 0.0, -0.40566037735849064, 0.0], 761 | [0.0, -0.169811320754717, 0.0, 0.6509433962264151, 0.0, -0.05660377358490567, 0.0, -0.40566037735849064], 762 | [-0.011320754716981133, 0.0, -0.05660377358490567, 0.0, 0.6962264150943396, 0.0, -0.360377358490566, 0.0], 763 | [0.0, -0.011320754716981133, 0.0, -0.05660377358490567, 0.0, 0.6962264150943396, 0.0, -0.360377358490566], 764 | [-0.1811320754716981, 0.0, -0.4056603773584906, 0.0, -0.360377358490566, 0.0, 1.2339622641509433, 0.0], 765 | [0.0, -0.1811320754716981, 0.0, -0.4056603773584906, 0.0, -0.360377358490566, 0.0, 1.2339622641509433]]) 766 | 767 | print '' 768 | print 'Test Case #1' 769 | result = online_slam(testdata1, 5, 3, 2.0, 2.0) 770 | solution_check(result, answer_mu1, answer_omega1) 771 | 772 | 773 | # ----------- 774 | # Test Case 2 775 | 776 | testdata2 = [[[[0, 12.637647070797396, 17.45189715769647], [1, 10.432982633935133, -25.49437383412288]], [17.232472057089492, 10.150955955063045]], 777 | [[[0, -4.104607680013634, 11.41471295488775], [1, -2.6421937245699176, -30.500310738397154]], [17.232472057089492, 10.150955955063045]], 778 | [[[0, -27.157759429499166, -1.9907376178358271], [1, -23.19841267128686, -43.2248146183254]], [-17.10510363812527, 10.364141523975523]], 779 | [[[0, -2.7880265859173763, -16.41914969572965], [1, -3.6771540967943794, -54.29943770172535]], [-17.10510363812527, 10.364141523975523]], 780 | [[[0, 10.844236516370763, -27.19190207903398], [1, 14.728670653019343, -63.53743222490458]], [14.192077112147086, -14.09201714598981]]] 781 | 782 | answer_mu2 = matrix([[63.37479912250136], 783 | [78.17644539069596], 784 | [61.33207502170053], 785 | [67.10699675357239], 786 | [62.57455560221361], 787 | [27.042758786080363]]) 788 | 789 | answer_omega2 = matrix([[0.22871751620895048, 0.0, -0.11351536555795691, 0.0, -0.11351536555795691, 0.0], 790 | [0.0, 0.22871751620895048, 0.0, -0.11351536555795691, 0.0, -0.11351536555795691], 791 | [-0.11351536555795691, 0.0, 0.7867205207948973, 0.0, -0.46327947920510265, 0.0], 792 | [0.0, -0.11351536555795691, 0.0, 0.7867205207948973, 0.0, -0.46327947920510265], 793 | [-0.11351536555795691, 0.0, -0.46327947920510265, 0.0, 0.7867205207948973, 0.0], 794 | [0.0, -0.11351536555795691, 0.0, -0.46327947920510265, 0.0, 0.7867205207948973]]) 795 | print '' 796 | print 'Test Case #2' 797 | result = online_slam(testdata2, 6, 2, 3.0, 4.0) 798 | solution_check(result, answer_mu2, answer_omega2) 799 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Programming a Robotic Car CS373 2 | _by Thomas Rieder_ 3 | 4 | ### Done: 5 | * Localization: 6 | * Homework 01 - Histogramm Filters 7 | * Homework 02 - Kalman Filters 8 | * Homework 03 - Particle Filters 9 | * Planning: 10 | * Homework 04 - A*-Search and Dynamic Programming 11 | * Control: 12 | * Homework 05 - Smoothing and PID-Control 13 | * SLAM 14 | * Homework 06 - Online Graph SLAM 15 | * Final Exam 16 | * Programming Exercise done with Dynamic Programming 17 | * Bonus: Answers for Questions 1 through 19 (PDF) 18 | 19 | ### Pending: 20 | * Nothing! :-) 21 | 22 | * * * 23 | ### Udacity Class: 24 | [Udacity CS373 Programming a Robotic Car 2012](http://www.udacity.com/overview/Course/cs373/CourseRev/feb2012 "Udacity Class") -------------------------------------------------------------------------------- /Unit 02/kalman_matrices.py: -------------------------------------------------------------------------------- 1 | # Write a function '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 filter(x, P): 144 | for n in range(len(measurements)): 145 | z = matrix([[measurements[n]]]) 146 | 147 | # measurement update 148 | y = z - (H * x) 149 | S = H * P * H.transpose() + R 150 | K = P * H.transpose() * S.inverse() 151 | x = x + (K * y) 152 | P = (I - K * H) * P 153 | 154 | # prediction 155 | x = (F * x) + u 156 | P = F * P * F.transpose() 157 | 158 | print 'x= ' 159 | x.show() 160 | print 'P= ' 161 | P.show() 162 | 163 | 164 | 165 | ######################################## 166 | 167 | measurements = [1, 2, 3] 168 | 169 | x = matrix([[0.], [0.]]) # initial state (location and velocity) 170 | P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty 171 | u = matrix([[0.], [0.]]) # external motion 172 | F = matrix([[1., 1.], [0, 1.]]) # next state function 173 | H = matrix([[1., 0.]]) # measurement function 174 | R = matrix([[1.]]) # measurement uncertainty 175 | I = matrix([[1., 0.], [0., 1.]]) # identity matrix 176 | 177 | filter(x, P) 178 | -------------------------------------------------------------------------------- /Unit 03/myrobot.py: -------------------------------------------------------------------------------- 1 | # In this exercise, please run your previous code twice. 2 | # Please only modify the indicated area below! 3 | 4 | from math import * 5 | import random 6 | 7 | 8 | 9 | landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]] 10 | world_size = 100.0 11 | 12 | class robot: 13 | def __init__(self): 14 | self.x = random.random() * world_size 15 | self.y = random.random() * world_size 16 | self.orientation = random.random() * 2.0 * pi 17 | self.forward_noise = 0.0; 18 | self.turn_noise = 0.0; 19 | self.sense_noise = 0.0; 20 | 21 | def set(self, new_x, new_y, new_orientation): 22 | if new_x < 0 or new_x >= world_size: 23 | raise ValueError, 'X coordinate out of bound' 24 | if new_y < 0 or new_y >= world_size: 25 | raise ValueError, 'Y coordinate out of bound' 26 | if new_orientation < 0 or new_orientation >= 2 * pi: 27 | raise ValueError, 'Orientation must be in [0..2pi]' 28 | self.x = float(new_x) 29 | self.y = float(new_y) 30 | self.orientation = float(new_orientation) 31 | 32 | 33 | def set_noise(self, new_f_noise, new_t_noise, new_s_noise): 34 | # makes it possible to change the noise parameters 35 | # this is often useful in particle filters 36 | self.forward_noise = float(new_f_noise); 37 | self.turn_noise = float(new_t_noise); 38 | self.sense_noise = float(new_s_noise); 39 | 40 | 41 | def sense(self): 42 | Z = [] 43 | for i in range(len(landmarks)): 44 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 45 | dist += random.gauss(0.0, self.sense_noise) 46 | Z.append(dist) 47 | return Z 48 | 49 | 50 | def move(self, turn, forward): 51 | if forward < 0: 52 | raise ValueError, 'Robot cant move backwards' 53 | 54 | # turn, and add randomness to the turning command 55 | orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise) 56 | orientation %= 2 * pi 57 | 58 | # move, and add randomness to the motion command 59 | dist = float(forward) + random.gauss(0.0, self.forward_noise) 60 | x = self.x + (cos(orientation) * dist) 61 | y = self.y + (sin(orientation) * dist) 62 | x %= world_size # cyclic truncate 63 | y %= world_size 64 | 65 | # set particle 66 | res = robot() 67 | res.set(x, y, orientation) 68 | res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise) 69 | return res 70 | 71 | def Gaussian(self, mu, sigma, x): 72 | 73 | # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma 74 | return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) 75 | 76 | 77 | def measurement_prob(self, measurement): 78 | 79 | # calculates how likely a measurement should be 80 | 81 | prob = 1.0; 82 | for i in range(len(landmarks)): 83 | dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) 84 | prob *= self.Gaussian(dist, self.sense_noise, measurement[i]) 85 | return prob 86 | 87 | 88 | 89 | def __repr__(self): 90 | return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation)) 91 | 92 | 93 | 94 | def eval(r, p): 95 | sum = 0.0; 96 | for i in range(len(p)): # calculate mean error 97 | dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0) 98 | dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0) 99 | err = sqrt(dx * dx + dy * dy) 100 | sum += err 101 | return sum / float(len(p)) 102 | 103 | #myrobot = robot() 104 | #myrobot.set_noise(5.0, 0.1, 5.0) 105 | #myrobot.set(30.0, 50.0, pi/2) 106 | #myrobot = myrobot.move(-pi/2, 15.0) 107 | #print myrobot.sense() 108 | #myrobot = myrobot.move(-pi/2, 10.0) 109 | #print myrobot.sense() 110 | 111 | #### DON'T MODIFY ANYTHING ABOVE HERE! ENTER/MODIFY CODE BELOW #### 112 | myrobot = robot() 113 | myrobot = myrobot.move(0.1, 5.0) 114 | Z = myrobot.sense() 115 | N = 1000 116 | T = 10 #Leave this as 10 for grading purposes. 117 | 118 | p = [] 119 | for i in range(N): 120 | r = robot() 121 | r.set_noise(0.05, 0.05, 5.0) 122 | p.append(r) 123 | 124 | for t in range(T): 125 | myrobot = myrobot.move(0.1, 5.0) 126 | Z = myrobot.sense() 127 | 128 | p2 = [] 129 | for i in range(N): 130 | p2.append(p[i].move(0.1, 5.0)) 131 | p = p2 132 | 133 | w = [] 134 | for i in range(N): 135 | w.append(p[i].measurement_prob(Z)) 136 | 137 | p3 = [] 138 | index = int(random.random() * N) 139 | beta = 0.0 140 | mw = max(w) 141 | for i in range(N): 142 | beta += random.random() * 2.0 * mw 143 | while beta > w[index]: 144 | beta -= w[index] 145 | index = (index + 1) % N 146 | p3.append(p[index]) 147 | p = p3 148 | 149 | print eval(myrobot, p) 150 | 151 | #enter code here, make sure that you output 10 print statements. 152 | 153 | -------------------------------------------------------------------------------- /Unit 04/twiddle.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.001): #Make this tolerance bigger if you are timing out! 170 | n_params = 3 171 | dparams = [1.0 for row in range(n_params)] 172 | params = [0.0 for row in range(n_params)] 173 | 174 | best_error = run(params) 175 | n = 0 176 | # sum = integral of the cross track error 177 | while sum(dparams) > tol: 178 | for i in range(len(params)): 179 | params[i] += dparams[i] 180 | err = run(params) 181 | if err < best_error: 182 | # error is better than our best error 183 | best_error = err 184 | dparams[i] *= 1.1 185 | else: 186 | # error is not better than before 187 | # try other directions 188 | params[i] -= 2.0 * dparams[i] 189 | err = run(params) 190 | if err < best_error: 191 | best_error = err 192 | dparams[i] *= 1.1 193 | else: 194 | # did not succeed - decrease increments 195 | params[i] += dparams[i] 196 | dparams[i] *= 0.9 197 | n += 1 198 | #print 'Twiddle #', n, params, ' -> ', best_error 199 | #print ' ' 200 | return run(params) 201 | 202 | 203 | params = twiddle() 204 | 205 | #err = run(params) 206 | print '\nFinal parameters: ', params, '\n -> ', err --------------------------------------------------------------------------------