RSSI_Localization_Simulator_matlab

clc; clear; close all; % Import Environment %========================================================================== % Environment Variables A = 40; n = 1.6; len = 5; % environment length (m) wid = 5; % environment width (m) % End Import Environment %-------------------------------------------------------------------------- % Generate Nodes %========================================================================== % Node Params numOfNodes = 4; nodeRes = 6; for ii = 1:numOfNodes % Types can be Outdoors, Hallway, Lab, Large Open Room, or Outdoors [node(ii),A(ii),n(ii)] = generateNode([rand*len,rand*wid],nodeRes, 3, 'Outdoors'); end % End Generate Nodes %-------------------------------------------------------------------------- % Generate Robot %========================================================================== % Robot Params robotPose = [1,1,0]; % initial robot position squarePath = [0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0; ... 0,30;0,30;0,30;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0; ... 0,30;0,30;0,30;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0; ... 0,30;0,30;0,30;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0;0.5,0; ... 0,30;0,30;0,30]; squarePath(:,1) = 0.25; robotPath = [squarePath;squarePath;squarePath]; errPer = 0.30; % odometry error percentage % End Generate Robot %-------------------------------------------------------------------------- % Select RSSI Filtering Method %========================================================================== % Method can be Menegatti,Particle,CorrFilt,NoFilter or Test (Test is default case) method = 'TrustFilt'; % End Select RSSI Filtering Method %-------------------------------------------------------------------------- % Simulation %-------------------------------------------------------------------------- for ii = 1:length(robotPath) % Follow specified path %====================================================================== % Rotate random amount oldRobotPose = robotPose; newRobotPose(3) = robotPose(3) + robotPath(ii,2); % Move forward X meters newRobotPose(1) = robotPose(1) + cosd(newRobotPose(3))*robotPath(ii,1); newRobotPose(2) = robotPose(2) + sind(newRobotPose(3))*robotPath(ii,1); robotPose = newRobotPose; locationHistory(:,ii) = robotPose; % End Follow Specified Path %---------------------------------------------------------------------- % Get odometry data %====================================================================== % Calculate odometry to introduce real errors that may be encountered % during actual implementation disp = (robotPose(1:2) - oldRobotPose(1:2)); disp = sqrt(disp(1)^2 + disp(2)^2) + normrnd(0,robotPath(ii,1)*errPer); u = [disp, (robotPose(3) - oldRobotPose(3)) + normrnd(0,1)]; % Get dead reckoning position if(ii == 1) dr_pose = oldRobotPose + [u(1)*cosd(oldRobotPose(3)+u(2)) u(1)*sind(oldRobotPose(3)+u(2)) u(2)]; ohist = u(1); % odometry history; else dr_pose = [dr_pose; (dr_pose(ii-1,1) + u(1)*cosd(dr_pose(ii-1,3)+u(2))), ... (dr_pose(ii-1,2) + u(1)*sind(dr_pose(ii-1,3)+u(2))), ... (dr_pose(ii-1,3) + u(2))]; ohist = [ohist;u(1)]; % Get dead reckoned distance to each node for jj = 1:numOfNodes temp(jj) = euclidDist(dr_pose(ii,1:2),node(jj).pos); end o2nhist = [o2nhist;temp]; end % u = [robotPath(ii,1),robotPath(ii,2)]; % End odometry data %---------------------------------------------------------------------- % Get RSSI Value and distance estimate to each node %====================================================================== for jj = 1:numOfNodes rssi(jj) = getRSSI(node(jj),robotPose); dist2node(jj) = euclidDist(node(jj).pos,robotPose); d(jj) = 10^((rssi(jj)-A(jj))/(10*n(jj))); % Log-Distance Model if(dist2node(jj) > max(node(jj).dist)) warning('Distance to node exceeds maximum value.'); end end % Record history of rssi and distance values if(ii == 1) dhist = d; % estimated distance history rhist = rssi; % measured rssi history d2nhist = dist2node; else dhist = [dhist;d]; rhist = [rhist;rssi]; d2nhist = [d2nhist;dist2node]; end % End Get RSSI Value and distance estimate to each node %---------------------------------------------------------------------- % Get change in distance relative to each node %====================================================================== if(ii == 2) % Run filter after 2 measurements are made to integrate % Also to match vector lenght of rhist after differentiation for jj = 1:numOfNodes d1 = euclidDist([x((jj-1)*2 + 4),x((jj-1)*2 + 5)],x(1:2)); % get updated state variables from measured odometry odom_x = [x(1) + u(1)*cosd(x(3)+u(2)),x(2) + u(1)*sind(x(3)+u(2))]; d2 = euclidDist([x((jj-1)*2 + 4),x((jj-1)*2 + 5)],odom_x(1:2)); v_d2n(jj) = d2 - d1; end vhist = v_d2n; elseif(ii > 2) for jj = 1:numOfNodes d1 = euclidDist([x((jj-1)*2 + 4),x((jj-1)*2 + 5)],x(1:2)); % get updated state variables from measured odometry odom_x = [x(1) + u(1)*cosd(x(3)+u(2)),x(2) + u(1)*sind(x(3)+u(2))]; d2 = euclidDist([x((jj-1)*2 + 4),x((jj-1)*2 + 5)],odom_x(1:2)); v_d2n(jj) = d2 - d1; end vhist = [vhist;v_d2n]; end % End get change in distance relative to each node %---------------------------------------------------------------------- % RSSI Particle Filter %====================================================================== if(strcmp(method,'Particle')) % Don't run particle filter until EFK has ran if(ii == 1) partDist = dist2node; else if(~exist('numParts')) numParts = 1000; for jj = 1:numParts % Actual random particle values % particles(jj,:) = rand([1 numOfNodes])*10 + normrnd(0,3,[1,numOfNodes]); % Initialize particles with estimated distances particles(jj,:) = d + normrnd(0,1,[1,numOfNodes]); % Cheese it with perfect initialization % particles(jj,:) = dist2node + normrnd(0,0.1,[1,numOfNodes]); end newParticles = zeros(size(particles)); end % Use odometry and estimated states to update particles for jj = 1:numOfNodes % get updated state variables from measured odometry odom_x = [x(1) + u(1)*cosd(x(3)+u(2)),x(2) + u(1)*sind(x(3)+u(2))]; % Extract landmark positions from state vector lmx = x((jj-1)*2 + 4); lmy = x((jj-1)*2 + 5); % Prediction vector contains changes to each node prediction(jj) = euclidDist(odom_x,[lmx,lmy]) - ... euclidDist(x(1:2),[lmx,lmy]); end if(ii <= 2) p_hist = prediction; else p_hist = [p_hist;prediction]; end % Apply changes to particles for jj = 1:numParts particles(jj,:) = particles(jj,:) + prediction; end % Get fitness value for each particle err = zeros(size(particles)); for jj = 1:numParts err(jj,:) = abs(d-particles(jj,:)); end for jj = 1:numParts err(jj,:) = abs(err(jj,:) - max(err)); end for jj = 1:numParts err_dist(jj,:) = err(jj,:)./s