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)];