Colony

%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