robotics专项课Estimation and Learning assignment 3: Occupancy Grid Mapping

% Robotics: Estimation and Learning 
% WEEK 3
% 
% Complete this function following the instruction. 
function myMap = occGridMapping(ranges, scanAngles, pose, param)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
% Parameters 
% 
% the number of grids for 1 meter.
myResol = param.resol;
% the initial map size in pixels
myMap = zeros(param.size);
% the origin of the map in pixels
myorigin = param.origin; 

% 4. Log-odd parameters 
lo_occ = param.lo_occ;
lo_free = param.lo_free; 
lo_max = param.lo_max;
lo_min = param.lo_min;

N = size(pose,2);
M = size(scanAngles);
for j = 1:N % for each time,
    % Find grids hit by the rays (in the gird map coordinate) 
      iStart = [ceil(pose(1,j)*myResol);ceil(pose(2,j)*myResol)] + myorigin;
     
      for i = 1:M
        beta = pose(3,j) +scanAngles(i); 
        d = ranges(i,j);
        pOcc = [d*cos(beta)+pose(1,j); -d*sin(beta)+pose(2,j)];