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)];
    % Find occupied-measurement cells and free-measurement cells
   
        iOcc = ceil(pOcc*myResol)+myorigin;
        [freex, freey] = bresenham(iStart(1),iStart(2),iOcc(1),iOcc(2));
        occInd = sub2ind(param.size, iOcc(2), iOcc(1));
        freeInd = sub2ind(param.size, freey, freex);
    % Update the log-odds
        myMap(freeInd) = myMap(freeInd) - lo_free;
        myMap(occInd) = myMap(occInd) + lo_occ;
    % Saturate the log-odd values
      end

    % Visualize the map as needed
   

end

end