├── .gitignore ├── README.md ├── beam_range_finder_model.m ├── drawGrid.m ├── drawRobot.m ├── grid_localization.m ├── ind2state.m ├── main.m ├── map.mat ├── motion_model_odometry.m ├── range_finder.m ├── showProbabilities.m ├── state2sub.m ├── sub2state.m ├── test_measurement_model.m └── test_motion_model.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.m~ 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Grid Localization 2 | ================= 3 | 4 | A localizer approach based on the histogram filter to represent the posterior belief (p. 237). 5 | 6 | Demo and explanation at: [https://www.youtube.com/watch?v=6a8NUnmbhxI](https://www.youtube.com/watch?v=6a8NUnmbhxI) 7 | -------------------------------------------------------------------------------- /beam_range_finder_model.m: -------------------------------------------------------------------------------- 1 | function q = beam_range_finder_model( zt, xt, m, Theta ) 2 | %BEAM_RANGE_FINDER_MODEL Measurement model for the range finder on Thrunbot 3 | % Probabilistic Robotics, Thrun et al., p 158, Table 6.1 4 | % Returns the probability of getting the noisy measurement zt given that 5 | % Thrunbot is currently at the hypothesized successor pose xt and the map 6 | 7 | % TODO: For some reason, probabilities are being returned that are 8 | % greater than 1. Is there a normalizing factor missing in this code, 9 | % or is it in the grid_localization routine? 10 | 11 | % -- Intrinsic Parameters --------------------------------------------- 12 | % These should be found using Table 6.2, p. 160: 13 | % They were empirically chosen. 14 | % Algorithm learn_intrinsic_parameters(Z,X,m) 15 | 16 | if (nargin < 4) 17 | Theta = [0.85 0.05 0.05 0.05 0.1 4 1]; 18 | end 19 | 20 | % mixing weights 21 | zhit = Theta(1); 22 | zshort = Theta(2); 23 | zmax = Theta(3); 24 | zrand = Theta(4); 25 | 26 | % std dev of noise of sensor and sensor's max range 27 | sigma_hit = Theta(5); 28 | zmax_range = Theta(6); 29 | 30 | % parameter (1/mean) of exponential distribution for short measurements 31 | lambda_short = Theta(7); 32 | % --------------------------------------------------------------------- 33 | 34 | K = 1; % there is only one scan associated with a measurement because 35 | % we are just using an ultrasonic range finder (i.e., what's 36 | % right in front of me...) 37 | 38 | % initialize the probability to 1 39 | q = 1; % line 2, Table 6.1 40 | 41 | for k = 1:K 42 | ztk = zt; % change this for K > 1 43 | 44 | % -- Ray Casting -------------------------------------------------- 45 | % compute ztk* for the measurement ztk using ray casting 46 | ztk_star = range_finder(xt,m); % use exact range_finder by not 47 | % passing in noise params 48 | 49 | % I don't really understand this. If we know the exact distance, 50 | % why are we doing any of this anyways? 51 | % I suppose 'ray casting' could just mean, use the hypothesized 52 | % pose and the map and decide what the scans should be. 53 | % ----------------------------------------------------------------- 54 | 55 | p = zhit * phit(ztk,xt,m, sigma_hit,ztk_star,zmax_range) +... 56 | zshort * pshort(ztk,xt,m, lambda_short, ztk_star) +... 57 | zmax * pmax(ztk,xt,m, zmax_range) +... 58 | zrand * prand(ztk,xt,m, zmax_range); 59 | 60 | q = q*p; % accrue the probability from each scan in zt 61 | end; 62 | 63 | end 64 | 65 | function p = phit(ztk, xt, m, sigma_hit, ztk_star, zmax) 66 | %PHIT Find the probability of being hit 67 | % 68 | if (0 <= ztk && ztk <= zmax) % eq (6.4) 69 | eta = normcdf([0 zmax], ztk_star, sigma_hit); % eq (6.6) 70 | eta = eta(2)-eta(1); 71 | p = eta*normpdf(ztk, ztk_star, sigma_hit); 72 | else 73 | p = 0; 74 | end; 75 | end 76 | 77 | function p = pshort(ztk, xt, m, lambda_short, ztk_star) 78 | %PSHORT Find the probability 79 | % 80 | if (0 <= ztk && ztk <= ztk_star) % eq (6.7) 81 | eta = 1/(1-exp(-lambda_short*ztk_star)); % eq (6.9) 82 | mu = (1/lambda_short); 83 | p = eta*exppdf(ztk,mu); 84 | else 85 | p = 0; 86 | end; 87 | end 88 | 89 | function p = pmax(ztk, xt, m, zmax) 90 | %PMAX Find the probability 91 | % 92 | if (ztk == zmax) % eq (6.10) 93 | p = 1; 94 | else 95 | p = 0; 96 | end; 97 | end 98 | 99 | function p = prand(ztk, xt, m, zmax) 100 | %PRAND Find the probability 101 | % 102 | if (0 <= ztk && ztk < zmax) % eq (6.11) 103 | p = 1/zmax; 104 | else 105 | p = 0; 106 | end; 107 | end -------------------------------------------------------------------------------- /drawGrid.m: -------------------------------------------------------------------------------- 1 | function handle = drawGrid( fig, map ) 2 | %DRAWGRID Setup the grid environment for the robot 3 | % inputs: 4 | % fig: figure number to draw on 5 | % map: logical square map of the environment. 1's are obstacles 6 | 7 | % Flip the map across the horizontal axis. This is necessary since the 8 | % map is a matrix with the origin at the top-left. The grid that is 9 | % plotted has its origin at the bottom-left, however. Therefore 10 | % flipping here is easiest. That way range_finder isn't too crazy. 11 | map = flip(map); 12 | 13 | figure(fig), clf; 14 | 15 | N = size(map,1); % num of rows 16 | M = size(map,2); % num of cols 17 | 18 | % Using meshgrid puts center of cell as (x, y) 19 | [X, Y] = meshgrid(-0.5:1:((N-1)+0.5)); 20 | 21 | % Expand the map slightly so it all shows up 22 | m = [map zeros(N,1); zeros(1,M+1)]; 23 | C = double(~m); 24 | 25 | % Draw the pseudocolor (checkerboard) plot in B&W 26 | h = pcolor(X,Y,C); 27 | colormap(gray(2)); 28 | set(h, 'EdgeColor', 'k'); 29 | set(h, 'LineStyle', ':'); 30 | axis square % make the aspect ratio square 31 | 32 | % Set the tick values so it is clearer to read 33 | set(gca, 'XTick', 0:1:M); 34 | set(gca, 'YTick', 0:1:N); 35 | 36 | % So that you don't have to do this elsewhere 37 | hold on; 38 | 39 | % return the handle just in case 40 | handle = h; 41 | end 42 | 43 | -------------------------------------------------------------------------------- /drawRobot.m: -------------------------------------------------------------------------------- 1 | function handle = drawRobot( xt, handle) 2 | %DRAWROBOT Draws a simple Thrunbot 3 | % inputs: 4 | % xt: input state vector (x, y, theta (deg)) 5 | % outputs: 6 | % handle: the figure handle for the robot 7 | 8 | if nargin == 1 || isempty(handle) 9 | handle_circle = []; 10 | handle_line = []; 11 | else 12 | % Unpack the handles 13 | handle_circle = handle(1); 14 | handle_line = handle(2); 15 | end 16 | 17 | r = 0.5; % this comes from how we did the meshgrid when plotting with 18 | % `pcolor`. Since we start in the cetner of the square cell of 19 | % width 1, radius must be 0.5 20 | 21 | % Unpack the robot's states 22 | x = xt(1); 23 | y = xt(2); 24 | theta = xt(3); 25 | 26 | handle_circle = circle(x, y, r, handle_circle); 27 | handle_line = heading(x, y, r, theta, handle_line); 28 | 29 | drawnow; % draw the entire robot together 30 | 31 | handle = [handle_circle handle_line]; 32 | end 33 | 34 | function handle = circle(x,y,r, handle) 35 | %x and y are the coordinates of the center of the circle 36 | %r is the radius of the circle 37 | %0.01 is the angle step, bigger values will draw the circle faster but 38 | %you might notice imperfections (not very smooth) 39 | ang = 0:0.01:2*pi; 40 | xp = r*cos(ang); 41 | yp = r*sin(ang); 42 | 43 | if isempty(handle), 44 | handle = plot(x+xp,y+yp); 45 | else 46 | set(handle,'XData',x+xp,'YData',y+yp); 47 | end 48 | end 49 | 50 | function handle = heading(x,y,r,theta, handle) 51 | x2 = x + r*cosd(theta); 52 | y2 = y + r*sind(theta); 53 | 54 | if isempty(handle), 55 | handle = plot([x x2],[y y2]); 56 | else 57 | set(handle,'XData',[x x2],'YData',[y y2]); 58 | end 59 | end -------------------------------------------------------------------------------- /grid_localization.m: -------------------------------------------------------------------------------- 1 | function [ pkt ] = grid_localization( pkt_d1, ut, zt, m, motion_params, measurement_params ) 2 | %GRID_LOCALIZATION A localizer using the discrete Bayes filter 3 | % Using stochastic motion and measurement models, localize the Thrunbot 4 | % based on a discrete Bayes filter. Here we use the Odometry Motion Model 5 | % (p. 134) and the Range Finder Measurement Model (p. 158). 6 | % 7 | % Remember that: 8 | % motion_model == p(xt | ut, xt_d1) 9 | % measurement_model = p(zt | xt, m) 10 | % 11 | % Probabilistic Robotics, Thrun et al., p 238, Table 8.1 12 | 13 | % What is the size of this map? 14 | N = size(m,1); % num of rows and columns (it's square) 15 | depth = 4; % theta = 0, 90, 180, 270 16 | count = N*N*depth; % how many cells are there? 17 | 18 | % Initialize variables 19 | pbarkt = zeros([size(m) depth]); 20 | pkt = zeros([size(m) depth]); 21 | 22 | % This variable is for debugging. It helped me know to not flip the 23 | % probability matrix inside showProbabilities(). 24 | p = zeros(count, 5); % each row: [p(zt|xt,m) zt [x y theta]] 25 | 26 | for k = 1:count % line 2 of Table 8.1 27 | 28 | % pick a new hypothesis of were the successor pose will be 29 | % (i.e., what's the probability of being at xt given that I 30 | % was initially at xt_d1 and my encoders said I went ut 31 | [rowk, colk, depthk] = ind2sub([N N depth], k); 32 | xt = ind2state([N N depth], k); 33 | 34 | % No need to check if the hypothesis is on an obstacle 35 | if (is_occupied(xt, m)), continue; end 36 | 37 | % -- Prediction --------------------------------------------------- 38 | for i = 1:count % line 3 of Table 8.1 39 | 40 | % pick a new initial pose to test 41 | [rowi, coli, depthi] = ind2sub([N N depth], i); 42 | xt_d1 = ind2state([N N depth], i); 43 | 44 | pbarkt(rowk,colk,depthk) = pbarkt(rowk,colk,depthk) + ... 45 | pkt_d1(rowi,coli,depthi)*... 46 | motion_model_with_map(xt,ut,xt_d1,m,motion_params); 47 | end 48 | % ----------------------------------------------------------------- 49 | 50 | % -- Measurement update ------------------------------------------- 51 | p(k,:)=[beam_range_finder_model(zt,xt,m,measurement_params) zt xt]; 52 | pkt(rowk,colk,depthk) = pbarkt(rowk,colk,depthk)*p(k,1);% line 4 53 | % note that there is no eta -- that normalization is done at the 54 | % end of this function 55 | % ----------------------------------------------------------------- 56 | 57 | end 58 | 59 | % Normalize probability to sum to 1. 60 | pkt = pkt/sum(pkt(:)); 61 | 62 | % Show the before (fig 3) and after (fig 4) motion models. i.e., these 63 | % two plots show the probability of being in different positions based 64 | % on where Thrunbot was before, and what the motion command was. 65 | showProbabilities(3,pkt_d1,ut(1:3)); % bel(xt_d1) 66 | showProbabilities(4,pbarkt,ut(4:6)); % shows the prediction step 67 | 68 | % restructure p to plot p(zt={zt}|xt,m) 69 | pzt = reshape(p(:,1),[N N depth]); 70 | pzt = pzt/sum(pzt(:)); % normalize 71 | showProbabilities(5,pzt,ut(4:6)); % shows the measurement probabilities 72 | end 73 | 74 | function flag = is_occupied(xt, m) 75 | %IS_OCCUPIED Is there an obstacle at this pose in the map? 76 | 77 | [i, j, ~] = state2sub(xt, m); 78 | flag = (m(i, j) == 1); 79 | end 80 | 81 | function p = motion_model_with_map( xt, ut, xt_d1, m, motion_params ) 82 | %MOTION_MODEL_WITH_MAP Use the map to prevent motion through walls 83 | % pmap = p(xt | m) and pmotion = p(xt | ut, xt_d1). 84 | % See Table 5.7, p. 141 85 | 86 | % Probability of being in a state, given the map of obstacles 87 | if is_occupied(xt, m) 88 | pmap = 0; 89 | else 90 | % how many spots in the map? 91 | count = size(m,1)*size(m,2); 92 | 93 | % how many obstacles in the map are there? 94 | obstacle_count = sum(m(:)); 95 | 96 | % how many spots could Thrunbot be? 97 | N = count - obstacle_count; 98 | 99 | % If it's equally likely for Thrunbot to be in a non-obstacle spot, 100 | % what's the probability of being in the given xt? 101 | pmap = 1/N; 102 | end 103 | 104 | pmotion = motion_model_odometry(xt,ut,xt_d1,motion_params); 105 | 106 | p = pmotion*pmap; % line 2 107 | 108 | % Probably need to do some normalization? 109 | % Or is that built in to the end of grid_localization? 110 | end -------------------------------------------------------------------------------- /ind2state.m: -------------------------------------------------------------------------------- 1 | function xt = ind2state( siz, index ) 2 | %IND2STATE Takes a MATLAB matrix index (one number) and returns the pose 3 | 4 | [i, j, k] = ind2sub(siz, index); 5 | 6 | if (length(siz) == 3) 7 | depth = siz(3); 8 | else 9 | depth = 4; 10 | end 11 | 12 | xt = sub2state(i, j, k, depth); 13 | 14 | end -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Grid Localization using a Discrete Bayes Filter for the Thrunbot. 3 | % 4 | % Assumptions: 5 | % the robot is like a taxicab in Manhattan (i.e., no diagonals) 6 | % Thrunbot's encoders are perfect...? 7 | % 8 | % See https://www.cs.princeton.edu/courses/archive/fall11/ 9 | % cos495/COS495-Lecture14-MarkovLocalization.pdf 10 | % for more information about stepping through the algorithm. 11 | % 12 | % Parker Lusk, BYU 13 | % 12 July 2016 14 | % 15 | % Probabilistic Robotics, Thrun et al. 16 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 17 | clear; clc; %close all; 18 | % === Robot Parameters ==================================================== 19 | 20 | % Ultrasonic sensor parameters 21 | range_max = 10; % how many empty cells can the range finder see ahead 22 | range_sigma = 0.01; % std dev of range finder 23 | ranger_params = [range_max range_sigma]; 24 | 25 | % Odometry motion model error params (p. 139, 135, or Figure 5.8) 26 | % tuned by hand in `test_motion_model`. Decrease to reduce variance 27 | % [rot1 trans1 trans2 rot2] 28 | motion_params = [0.18 0.4 0.18 0.0025]; 29 | %motion_params = [0.018 0.04 0.018 0.0025]; % locks quicker 30 | 31 | % Measurement model intrinsic parameters 32 | % tuned by hand in `test_measurement_model` 33 | % [zhit zshort zmax zrand sigma_hit zmax_range lambda_short] 34 | %measurement_params = [0.85 0.05 0.05 0.05 0.1 4 1]; % good 35 | measurement_params = [0.40 0.20 0.25 0.15 0.1 4 .5]; % typical 36 | %measurement_params = [0.15 0.20 0.225 0.125 0.2 4 .5]; % noisy 37 | % ========================================================================= 38 | 39 | % Load the 'map' variable 40 | load('map.mat'); 41 | 42 | % What is the size of this map? 43 | N = size(map,1); % num of rows and columns (it's square) 44 | depth = 4; % The depth of discretization. How many different headings can 45 | % the robot have? 0, 90, 180, 270 46 | 47 | % Create the grid 48 | grid_fig = 1; % Which figure to draw on 49 | grid = drawGrid(grid_fig, map); 50 | 51 | % Initialize the probability map to be equally likely, except for obstacles 52 | probs = (ones([N N depth]).*repmat(~flip(map),1,1,depth)); % zero out obstacles 53 | probs = probs/sum(probs(:)); % normalize 54 | 55 | % Create the surface plot for probabilities 56 | prob_fig = 2; % Which figure to draw on 57 | showProbabilities(prob_fig, probs); 58 | 59 | % Wait for the user to get windows positioned, etc 60 | pause; 61 | 62 | % ------------------------------------------------------------------------- 63 | % --- Robot Motion and Localization --------------------------------------- 64 | % ------------------------------------------------------------------------- 65 | 66 | % A list of moves to make 67 | moves = [... % x y thetad 68 | 5 5 90;... 69 | 5 6 90;... 70 | 5 7 90;... 71 | 4 7 90;... 72 | 3 7 90;... 73 | 3 8 90;... 74 | ]; 75 | 76 | moves = [... % x y thetad 77 | 1 1 0;... 78 | 2 1 0;... 79 | 3 1 0;... 80 | 4 1 0;... 81 | 5 1 0;... 82 | 6 1 0;... 83 | 7 1 0;... 84 | 8 1 0;... 85 | ]; 86 | 87 | moves = [... % x y thetad 88 | 1 1 90;... 89 | 1 2 90;... 90 | 1 3 90;... 91 | 1 4 90;... 92 | 1 5 90;... 93 | 1 6 90;... 94 | 1 7 90;... 95 | 1 8 90;... 96 | ]; 97 | 98 | moves = [... % x y thetad 99 | 0 0 0;... 100 | 0 0 90;... 101 | 0 1 90;... 102 | 0 1 0;... 103 | 1 1 0;... 104 | 2 1 0;... 105 | ]; 106 | 107 | moves = [... % x y thetad 108 | 0 0 0;... 109 | 1 0 0;... 110 | 2 0 0;... 111 | 2 0 90;... 112 | 2 1 90;... 113 | ]; 114 | 115 | moves = [... % x y thetad 116 | 0 0 0;... 117 | 0 0 90;... 118 | 0 1 90;... 119 | 0 2 90;... 120 | 0 2 0;... 121 | 1 2 0;... 122 | 2 2 0;... 123 | 2 2 90;... 124 | 2 3 90;... 125 | 2 3 0;... 126 | 3 3 0;... 127 | 4 3 0;... 128 | 4 3 270;... 129 | 4 2 270;... 130 | 4 1 270;... 131 | 4 0 270;... 132 | 4 0 180;... 133 | ]; 134 | 135 | % initialize xt 136 | xt = moves(1,:); 137 | 138 | % Initializes the robot starting point (x, y, theta (deg)) 139 | figure(grid_fig); % select the grid figure 140 | bot = drawRobot(xt, []); 141 | 142 | % Show the initial global belief of where the robot is 143 | showProbabilities(prob_fig, probs, xt); 144 | 145 | pause; 146 | 147 | for i = 2:size(moves,1) 148 | 149 | % Move the robot 150 | xt_d1 = xt; 151 | xt = moves(i,:); 152 | bot = drawRobot(xt, bot); 153 | 154 | % Get the odometry (the commanded ut vector), see p. 134 155 | ut = [xt_d1 xt]; 156 | 157 | % Get the range measurements (abs() since it shouldn't be negative) 158 | zt = abs(range_finder(xt, map, ranger_params)); 159 | 160 | % Show the change in motion and the resulting zt in the grid_fig 161 | dut = ut(4:6) - ut(1:3); 162 | figure(grid_fig); 163 | title(['{\Delta}u_t = ('... 164 | num2str(dut(1)) ' ' num2str(dut(2)) ' ' num2str(dut(3)) ')^T'... 165 | ', z_t = ' num2str(zt)]); 166 | 167 | % Do grid localization 168 | probs_d1 = probs; % for safekeeping 169 | probs = grid_localization(probs, ut, zt, map,... 170 | motion_params, measurement_params); 171 | 172 | % Update probability map 173 | showProbabilities(prob_fig, probs, xt); 174 | 175 | % Wait for the user to press a key 176 | pause; 177 | 178 | end 179 | 180 | % ------------------------------------------------------------------------- -------------------------------------------------------------------------------- /map.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/plusk01/grid-localization/243290bacd645caca26f059d39ea60f1fea2c9da/map.mat -------------------------------------------------------------------------------- /motion_model_odometry.m: -------------------------------------------------------------------------------- 1 | function p = motion_model_odometry( xt, ut, xt_d1, params ) 2 | %MOTION_MODEL Motion Model using Odometry for p(xt | ut, xt_d1) 3 | % Probabilistic Robotics, Thrun et al., p 134 4 | % inputs: 5 | % xt = (x_prime y_prime theta_prime): hypothesized successor pose 6 | % ut = (xbart_d1 xbart): motion information based on odometry 7 | % xt_d1 = (x y theta): initial robot pose 8 | % params = (a1 a2 a3 a4): noise parameters 9 | % output: 10 | % p: the probability of being at the hypothesized pose xt 11 | % 12 | % Information about ut = (xbart_d1 xbart): 13 | % this is a relative advance from xbart_d1 to xbart. Basically a 14 | % difference. (p. 133) 15 | % xbart_d1 = (xbar ybar thetabar) 16 | % xbart = (xbar_prime ybar_prime thetabar_prime) 17 | % 18 | % Information about params = (a1 a2 a3 a4): 19 | % "robot-specific paramerters that specify the noise in robot motion" 20 | % (p. 135 or p. 139). See Figure 5.8 in book for visualization of 21 | % different params. 22 | 23 | % -- Unpack values --------------- 24 | % hypothesized pose (xt) 25 | x_prime = xt(1); 26 | y_prime = xt(2); 27 | theta_prime = xt(3); 28 | % odometry motion information (ut) 29 | xbar = ut(1); 30 | ybar = ut(2); 31 | thetabar = ut(3); 32 | xbar_prime = ut(4); 33 | ybar_prime = ut(5); 34 | thetabar_prime = ut(6); 35 | % initial pose (xt_d1) 36 | x = xt_d1(1); 37 | y = xt_d1(2); 38 | theta = xt_d1(3); 39 | % noise params 40 | a1 = params(1); 41 | a2 = params(2); 42 | a3 = params(3); 43 | a4 = params(4); 44 | % -------------------------------- 45 | 46 | % Convert degrees to radians 47 | theta_prime = deg2rad(theta_prime); 48 | thetabar = deg2rad(thetabar); 49 | thetabar_prime = deg2rad(thetabar_prime); 50 | theta = deg2rad(theta); 51 | 52 | % Calculate the relative motion based on what our odometry told us 53 | delta_rot1 = atan2(ybar_prime - ybar, xbar_prime - xbar); 54 | delta_trans = sqrt((xbar - xbar_prime)^2 + (ybar - ybar_prime)^2); 55 | delta_rot2 = thetabar_prime - thetabar - delta_rot1; 56 | 57 | % Calculate what the relative motion would be if we were to end up at 58 | % the hypothesized successor pose, xt 59 | deltahat_rot1 = atan2(y_prime - y, x_prime - x); 60 | deltahat_trans = sqrt((x - x_prime)^2 + (y - y_prime)^2); 61 | deltahat_rot2 = theta_prime - theta - deltahat_rot1; 62 | 63 | % Use the rotational and translational deltas to sample a PDF and get 64 | % the correct posterior probability. This computes p(xt | ut, xt_d1). 65 | % prob(a, b^2) is defined on p. 135, 122. It is a zero-mean gaussian 66 | % with variance b^2, evaluated at a. 67 | a = constrain(delta_rot1 - deltahat_rot1); 68 | check(a); 69 | bb = a1*(deltahat_rot1)^2 + a2*(deltahat_trans)^2; 70 | p1 = prob(a, bb); 71 | 72 | a = delta_trans - deltahat_trans; 73 | bb = a3*(deltahat_trans)^2 + ... 74 | a4*(deltahat_rot1)^2 + a4*(deltahat_rot2)^2; 75 | p2 = prob(a, bb); 76 | 77 | a = constrain(delta_rot2 - deltahat_rot2); 78 | check(a); 79 | bb = a1*(deltahat_rot2)^2 + a2*(deltahat_trans)^2; 80 | p3 = prob(a, bb); 81 | 82 | p = p1*p2*p3; 83 | 84 | if isnan(p) 85 | % Because of the architecture of this MATLAB script, if we get a 86 | % NaN it is because we are hypothesizing that the robot is in the 87 | % same place as when it started. 88 | % We could say that this probability will be zero because in this 89 | % setup, `grid_localization` is only run after a robot movement, 90 | % but to be a little more robust, we give it a 1% chance of not 91 | % moving even if there was a command. There has to be a better way 92 | p = .01; 93 | 94 | % The probability that I end up in the same spot as I started when 95 | % the commanded ut is non-zero is low. 96 | % the probability that I end up in the same spot as I started when 97 | % the commanded ut is zero is high 98 | 99 | dut = ut(4:6) - ut(1:3); 100 | if any(dut) 101 | % commanded ut was non-zero 102 | p = 0.01; 103 | else 104 | p = 0.99; 105 | end 106 | 107 | end; 108 | end 109 | 110 | function p = prob(a, bb) 111 | % It would probably be more efficient to redo the math (p. 137) & pass 112 | % in only the std dev, b instead of the variance bb... but I didn't :) 113 | 114 | % zero-mean, Gaussian distribution with variance bb 115 | p = normpdf(a, 0, sqrt(bb)); 116 | end 117 | 118 | function angle = constrain(angle) 119 | %CONSTRAIN make the angle be between [-pi, pi]. 120 | % See Probabilistic Robotics, p. 135: 121 | % "the implementer must observe that all angular differences must lie in 122 | % [-pi, pi]." 123 | % 124 | % Code taken from: http://stackoverflow.com/a/2323034/2392520 125 | % Note that MATLAB's (and Python's) mod() function automatically makes 126 | % the angle positive, therefore, we comment this line out here. 127 | % 128 | % `angle` is expected in radians 129 | 130 | % reduce the angle 131 | angle = mod(angle, 2*pi); 132 | 133 | % force it to be the positive remainder, so that 0 <= angle < 360 134 | %angle = mod((angle + 2*pi), 2*pi); 135 | 136 | if (angle > pi) 137 | angle = angle - 2*pi; 138 | end; 139 | end 140 | 141 | function check(a) 142 | if (abs(a) > pi) 143 | disp(a); 144 | error('outside of pi'); 145 | end 146 | end 147 | -------------------------------------------------------------------------------- /range_finder.m: -------------------------------------------------------------------------------- 1 | function zt = range_finder( xt, map, params ) 2 | %RANGE_FINDER Use a zero-mean Gaussian noise model to return a range 3 | %measurement based on the current location, the map, and the intrinsic 4 | %range finder parameters 5 | % inputs: 6 | % xt: the robot's real current state (x, y, theta (deg)) 7 | % map: sqaure map of the environment. 1's are obstacles 8 | % params: the intrinsic range finder params, a vector with: 9 | % range_max: the maximum range of the sensor 10 | % sigma: the std dev of the noise 11 | % outputs: 12 | % zt: the noisy range measurement 13 | 14 | if (nargin == 2) 15 | % If `params` wasn't passed in, that means we want to do 16 | % 'ray tracing', i.e., we want the actual distance with no noise 17 | % and no max range, Rmax 18 | params = [ size(map,1) 0 ]; 19 | end 20 | 21 | % Unpack the params 22 | Rmax = params(1); 23 | sigma = params(2); 24 | 25 | % Where am I in the map (row, col)? --> note index swap below 26 | i = xt(2)+1; % Silly MATLAB indexing 27 | j = xt(1)+1; % for indexing the map 28 | 29 | % flip the row index (i) to match how the grid looks 30 | i = (size(map,1)+1) - i; % size+1 deals with MATLAB indexing 31 | 32 | % Which way should I look for obstacles? 33 | c = exp(1j*xt(3)*pi/180); 34 | dir = [-imag(c) real(c)]; % (row, col) with robot as origin 35 | % Again, notice that imag(c) is negated to match the fmap 36 | 37 | % Force dir to have integer components 38 | dir(find(abs(dir)<2*eps)) = 0; 39 | 40 | % Based on where I am in the map, is there an obstacle in front of me? 41 | looker = [i j]; % which index is being looked at right now? 42 | zt = -1; 43 | 44 | % If not walled in, don't allow the looker to go beyond matrix indices 45 | if any(dir<(-eps)) 46 | % Special treatment: if the looker is going in a negative direction 47 | % then the max_look needs to be flipped again because of the fmap 48 | max_look = looker(find(abs(dir)>eps)) - 1; 49 | else 50 | max_look = size(map,1) - looker(find(abs(dir)>eps)); 51 | end 52 | 53 | % to see where the robot is (uncomment below) 54 | tmp = map; 55 | tmp(looker(1),looker(2)) = 2; 56 | 57 | for k = 1:min((Rmax+1),max_look) 58 | % move looker along line of sight 59 | looker = looker + dir; 60 | 61 | % Uncomment to see step-by-step visualization 62 | % tmp(looker(1), looker(2)) = 99 63 | % pause; 64 | 65 | if map(looker(1), looker(2)) == 1 66 | % an obstacle was hit! 67 | zt = k-1; 68 | break; 69 | end 70 | end 71 | 72 | if max_look == 0 73 | % We were on the edge of the matrix looking out, therefore there is 74 | % nothing else to see! 75 | zt = 0; 76 | end 77 | 78 | if zt == -1 79 | if min((Rmax+1),max_look) == max_look 80 | % We were constrained by the matrix size, not the range of the 81 | % sensor, so zt should be max_look 82 | zt = max_look; 83 | else 84 | % there was no obstacle detected, so use the Rmax 85 | zt = Rmax; 86 | end 87 | end 88 | 89 | % Apply Gaussian noise to the measurement 90 | zt = normrnd(zt, sigma); 91 | end 92 | 93 | -------------------------------------------------------------------------------- /showProbabilities.m: -------------------------------------------------------------------------------- 1 | function showProbabilities( fig, probs, xt) 2 | %SHOWPROBABILITIES Show the grid of probabilities, i.e., the 2D PDF 3 | % Detailed explanation goes here 4 | 5 | if (nargin < 3) 6 | xt = []; 7 | end 8 | 9 | figure(fig), clf; 10 | 11 | % If probs is has a depth, we have to plot each layer 12 | K = size(probs,3); 13 | for i = 1:K 14 | drawLayer(probs,i,K,xt); 15 | end 16 | 17 | if ~isempty(xt) 18 | % Indicate when Thrunbot's position is locked on 19 | [M,I] = max(probs(:)); 20 | if all(probs(:) == M), return; end 21 | xhat = ind2state(size(probs),I); 22 | if all(xt == xhat) 23 | 24 | % Let's just make sure that this isn't when it's equally likely 25 | % everywhere... 26 | if sum(probs(:) == M) > size(probs,1)*size(probs,2),return; end 27 | 28 | % Thrunbot knows something! 29 | annotation('textbox', [0 0.9 1 0.1],... 30 | 'String', 'Position Locked',... 31 | 'EdgeColor', 'None',... 32 | 'HorizontalAlignment', 'Center',... 33 | 'FontSize', 14,... 34 | 'Color', 'r'); 35 | 36 | else 37 | % indicate where the highest probability is 38 | % Figure out how many subplots are needed 39 | n = ceil(sqrt(K)); 40 | layer = (xhat(3)/90)+1; 41 | subplot(n,n,layer), hold on; 42 | h = plot(xhat(1),xhat(2),'rx'); 43 | set(h, 'MarkerSize', 10); 44 | set(h, 'LineWidth', 2); 45 | end 46 | end 47 | end 48 | 49 | function drawLayer( all_probs, layer, total_layers, xt ) 50 | 51 | % access the correct layer 52 | probs = all_probs(:,:,layer)/sum(all_probs(:)); 53 | 54 | % probs doesn't need to be flipped like the map ... for some reason. 55 | probs = (probs); 56 | 57 | % Figure out how many subplots are needed 58 | n = ceil(sqrt(total_layers)); 59 | 60 | % Select the correct subplot 61 | subplot(n,n,layer); 62 | 63 | N = size(probs,1); % num of rows 64 | M = size(probs,2); % num of cols 65 | 66 | % Using meshgrid puts center of cell as (x, y) 67 | [X, Y] = meshgrid(-0.5:1:((N-1)+0.5)); 68 | 69 | % Expand the map slightly so it all shows up 70 | C = [probs zeros(N,1); zeros(1,M+1)]; 71 | 72 | % Draw the pseudocolor (checkerboard) plot 73 | h = pcolor(X,Y,C); 74 | colormap(flipud(gray)); 75 | colorbar; 76 | caxis([0 max(all_probs(:))]); 77 | 78 | % Make visually easy to see 79 | set(h, 'EdgeColor', 'k'); 80 | set(h, 'LineStyle', ':'); 81 | axis square % make the aspect ratio square 82 | 83 | if (total_layers == 4) 84 | titles = {'\theta = 0^{\circ}', '\theta = 90^{\circ}',... 85 | '\theta = 180^{\circ}', '\theta = 270^{\circ}'}; 86 | 87 | title(titles{layer}); 88 | end 89 | 90 | % Add true robot position (*) 91 | if (nargin == 4 && ~isempty(xt) && (xt(3)/90)+1 == layer) 92 | hold on; 93 | h = plot(xt(1),xt(2),'w*'); 94 | set(h, 'MarkerSize', 20); 95 | set(h, 'LineWidth', 4); 96 | end 97 | end 98 | -------------------------------------------------------------------------------- /state2sub.m: -------------------------------------------------------------------------------- 1 | function [ i, j, k ] = state2sub( xt, m, depth ) 2 | %STATE2SUB Takes a pose and turns into MATLAB matrix subscripts 3 | 4 | if (nargin < 3) 5 | depth = 4; 6 | end 7 | 8 | depth2deg = @(index)(index-1)*(360/depth); 9 | deg2depth = @(deg)(deg/(360/depth))+1; 10 | 11 | % Where am I in the map (row, col)? --> note index swap below 12 | i = xt(2)+1; % Silly MATLAB indexing 13 | j = xt(1)+1; % for indexing the map 14 | 15 | % flip the row index (i) to match how the grid looks 16 | i = (size(m,1)+1) - i; % size+1 deals with MATLAB indexing 17 | 18 | % get depth of discretization (the angle) 19 | if length(xt) == 3 20 | k = deg2depth(xt(3)); 21 | end 22 | end 23 | 24 | -------------------------------------------------------------------------------- /sub2state.m: -------------------------------------------------------------------------------- 1 | function xt = sub2state( i, j, k, depth ) 2 | %STATE2SUB Takes MATLAB matrix subscripts and returns the pose 3 | 4 | if (nargin < 4) 5 | depth = 4; 6 | end 7 | 8 | depth2deg = @(index)(index-1)*(360/depth); 9 | deg2depth = @(deg)(deg/(360/depth))+1; 10 | 11 | xt = [j-1 i-1 depth2deg(k)]; 12 | end 13 | -------------------------------------------------------------------------------- /test_measurement_model.m: -------------------------------------------------------------------------------- 1 | % Set defaults 2 | set(0,'DefaultAxesFontSize',18); 3 | set(0,'DefaultLineLineWidth',2); 4 | 5 | % This will give us ztk = 2 in the range finder model 6 | m = zeros(3); xt = [0 0 90]; 7 | ztkstar = 2; 8 | 9 | % Measurement paramters 10 | % [zhit zshort zmax zrand sigma_hit zmax_range lambda_short] 11 | params = [0.40 0.20 0.25 0.15 0.1 4 .5]; 12 | % params = [0.85 0.05 0.05 0.05 0.1 4 1]; 13 | % params = [0.15 0.20 0.225 0.125 0.2 4 .5]; 14 | 15 | % Span of PDF is from 0 <= z <= zmax 16 | zmax = params(6); 17 | 18 | count = 1000; % samples 19 | p = zeros(1,count); 20 | 21 | ztk = linspace(0,zmax+.5,count); 22 | 23 | 24 | for k = 1:count 25 | zt = ztk(k); 26 | p(k) = beam_range_finder_model(zt,xt,m,params); 27 | end 28 | 29 | figure(5), clf; 30 | plot(ztk,p/sum(p(:))) 31 | title(['p(zt | xt, m) with z_{max} = ' num2str(zmax) ' and z_t^{k*} = '... 32 | num2str(ztkstar)], 'FontSize', 20); -------------------------------------------------------------------------------- /test_motion_model.m: -------------------------------------------------------------------------------- 1 | m = zeros(4); 2 | 3 | 4 | % What is the size of this map? 5 | N = size(m,1); % num of rows and columns (it's square) 6 | count = N*N*4; % how many cells are there? 7 | 8 | % Initialize variables 9 | pbarkt = zeros([size(m) 4]); 10 | pkt = zeros([size(m) 4]); 11 | 12 | % identify the odometry motion (where I was --> where I'm going) 13 | ut = [ [0 0 0] [1 0 0] ]; 14 | 15 | % Set up the initial belief of where the thrunbot is. 16 | pkt_d1 = zeros(N,N,4); 17 | r = N-ut(1); 18 | c = ut(2)+1; 19 | d = (ut(3)/90)+1; 20 | pkt_d1(r,c,d) = 1; 21 | % pkt_d1 = ones(N,N,4)/(N*N*4); 22 | 23 | % motion params 24 | p = [.18 .4 .18 .0025]; 25 | 26 | 27 | for k = 1:count 28 | 29 | % pick a new hypothesis of were the successor pose will be 30 | % (i.e., what's the probability of being at xt given that I 31 | % was initially at xt_d1 and my encoders said I went ut 32 | [rowk, colk, depthk] = ind2sub([N N 4], k); 33 | xt = [colk-1, rowk-1, (depthk-1)*90]; % Silly MATLAB! 34 | rowk = N+1 - rowk; 35 | 36 | % If successor pose is an obstacle on the map, we should just skip this 37 | % iteration. 38 | 39 | for i = 1:count % line 3 of Table 8.1 40 | 41 | % pick a new initial pose to test 42 | [rowi, coli, depthi] = ind2sub([N N 4], i); 43 | xt_d1 = [coli-1, rowi-1, (depthi-1)*90]; % Silly MATLAB! 44 | rowi = N+1 - rowi; 45 | 46 | pbarkt(rowk,colk,depthk) = pbarkt(rowk,colk,depthk) + ... 47 | pkt_d1(rowi,coli,depthi)*... 48 | motion_model_odometry(xt,ut,xt_d1,p); 49 | end 50 | end 51 | 52 | pbarkt = pbarkt/sum(pbarkt(:)); 53 | 54 | showProbabilities(3,pkt_d1,ut(1:3)) 55 | showProbabilities(4,pbarkt,ut(4:6)) --------------------------------------------------------------------------------