Statistics
| Revision:

## root / trunk / code / projects / mapping / matlab / prob_map.m @ 1134

 1 ```%Function which creates a heat map of wall location probabilities. ``` ```%Input comes from 'filename' where each line contains 8 space-separated values ``` ```%Each line contains: x, y, theta, IR1, IR2, IR3, IR4, IR5 in that order ``` ```%Example line of input file: ``` ```%17 23 .45 -1 42 -1 -1 -1 ``` ```%x and y are centimeters, theta is in radians ``` ```%IR values are the values seen by the robot, where -1 means that nothing is seen ``` ```%Output: probability density map. ``` ```%All output measurements are in centimeters ``` ```%TODO ``` ```%find conversion factor between IR sensor values and centimeters (currently 1 is being used) ``` ```function map(filename) ``` ```%Vectors from center of bot to IR sensors ``` ```Bot_IR1 = [4.5,3.5]; ``` ```Bot_IR2 = [7.5,0]; ``` ```Bot_IR3 = [4.5,-3.5]; ``` ```Bot_IR4 = [-4,-3.5]; ``` ```Bot_IR5 = [-4,3.5]; ``` ```Bot_IR = [Bot_IR1; Bot_IR2; Bot_IR3; Bot_IR4; Bot_IR5]; ``` ```%Unit vectors with the same angle that the IR sensors are tilted at ``` ```Angle_IR1 = [.78,.63]; ``` ```Angle_IR2 = [1,0]; ``` ```Angle_IR3 = [.78,-.63]; ``` ```Angle_IR4 = [0,-1]; ``` ```Angle_IR5 = [0,1]; ``` ```Angle_IR = [Angle_IR1; Angle_IR2; Angle_IR3; Angle_IR4; Angle_IR5]; ``` ```%For now, read input in from a file ``` ```Data = dlmread(filename, ' '); ``` ```points = []; ``` ```%Map area, defined in centimeters: ``` ```[X,Y] = meshgrid(-100:2:100,-100:2:100); ``` ```%V = [X1(:),X2(:)]; ``` ```prob_map = zeros(size(X,1),size(Y,2)); ``` ```%Set position and range error constant for testing FIXME! ``` ```range_error = 250; ``` ```pos_error = 500; ``` ```%loop through each line of file ``` ```i = 1; ir = 1; ``` ```%for i=1:size(Data, 1) ``` ``` row = Data(i, :); ``` ``` x = row(1); ``` ``` y = row(2); ``` ``` theta = row(3); ``` ``` %look at each IR sensor reading ``` ``` %for ir=1:5 ``` ``` %if row(ir+3) ~= -1 ``` ``` %Find vector of object relative to center of bot ``` ``` distance = row(ir+3) * 1; %TODO!!!!!!!!! replace 1 with a conversion factor to cm ``` ``` ObjectVector = distance * Angle_IR(ir, :); ``` ``` ObjectVector = ObjectVector + Bot_IR(ir, :); ``` ``` ``` ``` %rotate by theta ``` ``` objR = sqrt(ObjectVector(1)^2 + ObjectVector(2)^2); ``` ``` objTheta = atan2(ObjectVector(2), ObjectVector(1)); ``` ``` objTheta = objTheta + theta; ``` ``` ``` ``` %add absolute position and shift into image ``` ``` objX = (objR * cos(objTheta)) + x + 1500; ``` ``` objY = (objR * sin(objTheta)) + y + 1500; ``` ``` ObjectVector = [objX, objY]; ``` ``` ``` ``` %Calculate distribution for this point: ``` ``` ``` ``` %Range variance in y direction, rotated by the object angle. ``` ``` rotation = [cos(objTheta),sin(objTheta); -sin(objTheta),cos(objTheta)]; ``` ``` range_error_cov = inv(rotation)*[0,0;0,range_error]*rotation; ``` ``` %Position variance in the robot direction ``` ``` %(TODO introduce error in position estimation perpendicular to the robot) ``` ``` rotation = [cos(theta),sin(theta); -sin(theta),cos(theta)]; ``` ``` position_error_cov = inv(rotation)*[0,0;0,pos_error]*rotation; ``` ``` covar = position_error_cov + range_error_cov; ``` ``` ``` ``` dist = reshape(mvnpdf(X,[x,y],covar), size(X1,1),size(X2,2)) ``` ``` surf(X1,X2,dist); ``` ``` colormap default; ``` ``` return; ``` ``` %Using the values of the pdf as an approximation for the prob. density map ``` ``` %(needs to change) update as: ``` ``` prob_map = prob_map.*(1-dist) + dist; ``` ``` %end ``` ``` %end ``` ```%end ``` ```%graph the probability map. ``` ```surf(X1,X2,prob_map); ```