root / trunk / code / projects / mapping / matlab / sensor_test.m @ 1135
History | View | Annotate | Download (1.16 KB)
1 |
|
---|---|
2 |
%Define sensor model. |
3 |
|
4 |
%Definitions: |
5 |
sensor_model_width = 100; |
6 |
sensor_model_height = 100; |
7 |
sensor_bounds_x = 30; |
8 |
sensor_bounds_y = 100; |
9 |
sensor_width = 8; %cm |
10 |
empty_weight = 0.1; |
11 |
wall_weight = 2.0; |
12 |
outside_weight = 1; |
13 |
|
14 |
%Space creation: |
15 |
Sx = linspace(-sensor_bounds_x, sensor_bounds_x, sensor_model_width); |
16 |
Sy = linspace(-sensor_bounds_y, sensor_bounds_y, sensor_model_height); |
17 |
sensor_model = zeros(size(Sx,1), size(Sy,1), 71); |
18 |
|
19 |
%k ranges over distances (cm) |
20 |
for k=10:80 |
21 |
for j = 1:sensor_model_height; |
22 |
for i = 1:sensor_model_width; |
23 |
slope = k / sensor_width; |
24 |
if abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k - 2) && Sy(j) > 0 |
25 |
sensor_model(i,j,k) = empty_weight; |
26 |
elseif abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k + 2) && Sy(j) > 0 |
27 |
sensor_model(i,j,k) = wall_weight / abs((k - Sy(j))); |
28 |
else |
29 |
sensor_model(i,j,k) = outside_weight; |
30 |
end |
31 |
end |
32 |
end |
33 |
end |
34 |
|
35 |
%Define the sensor model for when no measurement is made (-1) |
36 |
for j = 1:sensor_model_height; |
37 |
for i = 1:sensor_model_width; |
38 |
slope = 80 / sensor_width; |
39 |
if abs(Sx(i)) < Sy(j) / slope && Sy(j) < 80 && Sy(j) > 0 |
40 |
sensor_model(i,j,k) = empty_weight; |
41 |
else |
42 |
sensor_model(i,j,k) = outside_weight; |
43 |
end |
44 |
end |
45 |
end |
46 |
|
47 |
|
48 |
|