Revision 1361
Made some small changes reading through the code, no mex file compiler at home so I might have broken it.. but probably not.
| prob_map.m | ||
|---|---|---|
| 47 | 47 |
|
| 48 | 48 |
%loop through each line of file |
| 49 | 49 |
i = 1; ir = 1; |
| 50 |
%for i=1:size(Data, 1)
|
|
| 50 |
for i=1:size(Data, 1) |
|
| 51 | 51 |
row = Data(i, :); |
| 52 | 52 |
x = row(1); |
| 53 | 53 |
y = row(2); |
| 54 | 54 |
theta = row(3); |
| 55 | 55 |
%look at each IR sensor reading |
| 56 |
%for ir=1:5
|
|
| 57 |
%if row(ir+3) ~= -1
|
|
| 56 |
for ir=1:5 |
|
| 57 |
if row(ir+3) ~= -1 |
|
| 58 | 58 |
%Find vector of object relative to center of bot |
| 59 | 59 |
distance = row(ir+3) * 1; %TODO!!!!!!!!! replace 1 with a conversion factor to cm |
| 60 | 60 |
ObjectVector = distance * Angle_IR(ir, :); |
| ... | ... | |
| 66 | 66 |
objTheta = objTheta + theta; |
| 67 | 67 |
|
| 68 | 68 |
%add absolute position and shift into image |
| 69 |
objX = (objR * cos(objTheta)) + x + 1500;
|
|
| 70 |
objY = (objR * sin(objTheta)) + y + 1500;
|
|
| 69 |
objX = (objR * cos(objTheta)) + x + size(X1,1);
|
|
| 70 |
objY = (objR * sin(objTheta)) + y + size(X2,2);
|
|
| 71 | 71 |
ObjectVector = [objX, objY]; |
| 72 | 72 |
|
| 73 |
%Calculate distribution for this point: |
|
| 73 |
%Calculate distribution for this data point:
|
|
| 74 | 74 |
|
| 75 | 75 |
%Range variance in y direction, rotated by the object angle. |
| 76 | 76 |
rotation = [cos(objTheta),sin(objTheta); -sin(objTheta),cos(objTheta)]; |
| ... | ... | |
| 80 | 80 |
%(TODO introduce error in position estimation perpendicular to the robot) |
| 81 | 81 |
rotation = [cos(theta),sin(theta); -sin(theta),cos(theta)]; |
| 82 | 82 |
position_error_cov = inv(rotation)*[0,0;0,pos_error]*rotation; |
| 83 |
|
|
| 83 |
|
|
| 84 | 84 |
covar = position_error_cov + range_error_cov; |
| 85 | 85 |
|
| 86 |
dist = reshape(mvnpdf(X,[x,y],covar), size(X1,1),size(X2,2)) |
|
| 87 |
surf(X1,X2,dist); |
|
| 88 |
colormap default; |
|
| 89 |
return; |
|
| 86 |
dist = reshape(mvnpdf(X,[x,y],covar), size(X1,1),size(X2,2)); |
|
| 90 | 87 |
|
| 91 |
%Using the values of the pdf as an approximation for the prob. density map |
|
| 88 |
%Using the values of the pdf as an approximation for the prob. density map NOT SAFE
|
|
| 92 | 89 |
%(needs to change) update as: |
| 93 | 90 |
prob_map = prob_map.*(1-dist) + dist; |
| 94 |
%end
|
|
| 95 |
%end
|
|
| 96 |
%end
|
|
| 91 |
end |
|
| 92 |
end |
|
| 93 |
end |
|
| 97 | 94 |
|
| 98 | 95 |
%graph the probability map. |
| 99 | 96 |
surf(X1,X2,prob_map); |
| 97 |
colormap default; |
|
| 100 | 98 |
|
| 101 | 99 |
|
Also available in: Unified diff