
%Define sensor model.

%Definitions:
sensor_model_width = 100;
sensor_model_height = 100;
sensor_bounds_x = 30;
sensor_bounds_y = 100;
sensor_width = 8; %cm
empty_weight = 0.1;
wall_weight = 2.0;
outside_weight = 1;

%Space creation:
Sx = linspace(-sensor_bounds_x, sensor_bounds_x, sensor_model_width);
Sy = linspace(-sensor_bounds_y, sensor_bounds_y, sensor_model_height);
sensor_model = zeros(size(Sx,1), size(Sy,1), 71);

%k ranges over distances (cm)
for k=10:80
	for j = 1:sensor_model_height;
		for i = 1:sensor_model_width;
			slope = k / sensor_width;	
			if abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k - 2) && Sy(j) > 0
				sensor_model(i,j,k) = empty_weight;
			elseif abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k + 2) && Sy(j) > 0
				sensor_model(i,j,k) = wall_weight / abs((k - Sy(j)));	
			else
				sensor_model(i,j,k) = outside_weight;
			end
		end
	end
end

%Define the sensor model for when no measurement is made (-1)
for j = 1:sensor_model_height;
	for i = 1:sensor_model_width;
		slope = 80 / sensor_width;	
		if abs(Sx(i)) < Sy(j) / slope && Sy(j) < 80 && Sy(j) > 0
			sensor_model(i,j,k) = empty_weight;
		else
			sensor_model(i,j,k) = outside_weight;
		end
	end
end



