Colony

function [sensor_model, udata, vdata] = model_sensor

%Definitions:

sensor_model_width = 25;

sensor_model_height = 100;

sensor_bounds_x = 85; %cm

sensor_bounds_y = 15; %cm

sensor_width = 10; %cm

empty_weight = 0.4;

wall_weight = 2.0;

outside_weight = 1;

%Space creation:

Sx = linspace(0, 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 = double(k) / double(sensor_width);

			if abs(Sy(j)) < (Sx(i) / slope) && Sx(i) < (k - 2)

				sensor_model(j,i,k) = empty_weight;

			elseif abs(Sy(j)) < (Sx(i) / slope) && Sx(i) < (k + 2)

				sensor_model(j,i,k) = wall_weight;

			else

				sensor_model(j,i,k) = outside_weight;

			end

		end

	end

	map(k,1,1) = 100.0;

end

%Define the sensor model for when no measurement is made (-1)

slope = 80 / sensor_width;

for j = 1:sensor_model_height;

	for i = 1:sensor_model_width;

		if abs(Sy(j)) < Sx(i) / slope && Sx(i) < 80 && Sx(i) > 0

			sensor_model(j,i,71) = empty_weight;

		else

			sensor_model(j,i,71) = outside_weight;

		end

	end

end

image(sensor_model(:,:,70).*20);

udata = [0, 2] * sensor_bounds_x;

vdata = [-1, 1] * sensor_bounds_y;