Colony

sensor_model_width = 100;

sensor_model_height = 100;

map_width = 800;

map_height = 800;

arena = ones(map_width,map_height);

sample_sensor = sensor_model(:,:, 60);

%sample data:

x = 200;

y = 100;

dx = x;

dy = y;

objTheta = pi;

%objTheta = 0;

translation_mat = [1,   0, 0;

				   0,   1, 0;

				   dx, dy, 1];

rotation_mat = [cos(objTheta), sin(objTheta), 0;

			   -sin(objTheta), cos(objTheta), 0

			   			    0,			   0, 1];

udata = [-1, 1] * sensor_model_width/2;

vdata = [-1, 1] * sensor_model_height/2;

xdata = [-1, 1] * map_width/2;

ydata = [-1, 1] * map_height/2;

transformation_mat = rotation_mat * translation_mat;

translation = maketform('affine', transformation_mat);

sensor = imtransform(sample_sensor, translation,... 

		'UData', udata, 'Vdata', vdata,...

		'XData', xdata, 'YData', ydata,...

		'Size', [map_width, map_height], 'FillValues', 1);

size(arena)

size(sensor)

arena = arena.*sensor;

image(arena.*20);

sensor_model_width = 60;

sensor_model_height = 100;

map_width = 600;

map_height = 600;

%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