Datei: main.m
% number of particles N = 1000; % number of simulation steps T = 40; % number of seconds to wait until next step t = 0.01; % number of landmarks to provide (1,2,3,4 or 5) noLandmarks = 5; % flag to indicate whether you want to save the animation - 'true' or % 'false' saveAnimation = 'false'; myParticleFilterRun(N, T, t, noLandmarks, saveAnimation);
Datei: myParticleFilterRun.m
function [] = myParticleFilterRun(N, T, t, noLandmarks, saveAnimation) %% Configuration % % set the World and place landmarks if noLandmarks == 1 % 1 landmark landmarks = [[50;50]]; elseif noLandmarks == 2 % 2 landmarks landmarks = [[50;75], [50; 25]]; elseif noLandmarks == 3 % 3 landmarks landmarks = [[50;75], [25; 25], [75; 25]]; elseif noLandmarks == 4 % 4 landmarks landmarks = [[20.0; 20.0], [80.0; 80.0], [20.0; 80.0], [80.0; 20.0]]; else % 5 landmarks landmarks = [[10.0; 20.0], [50.0; 10.0], [50.0; 30.0], [10.0; 70.0], [70.0; 70.0]]; end worldSize = 100.0; % number of particles %N = 1000; % number of simulation steps %T = 10; %% Initialize robot and particles % % create the robot... myRobot = createRobot(worldSize); myRobot = setRobot(myRobot, worldSize, 10, 10, 0.3); % ...and let it make it's first move! myRobot = robotMove(myRobot, worldSize, 0.1, 5.0); % Sense it's position (NOTE: we have no Noise yet, so sensing and % movement before should be 'perfect') Z = robotSense(myRobot, landmarks); % create particles for i = 1:N p = createRobot(worldSize); p = setNoise(p, 0.05, 0.05, 5.0); particlesList(i) = p; end disp( sprintf('Initial Error is: %.6f', robotEvaluate(myRobot, particlesList, worldSize) ) ); % additionally add some noise to robot myRobot = setNoise(myRobot, 0.05, 0.05, 2.0); % Plot initial configuration figure(1); hold on; axis([0 worldSize 0 worldSize]); set(gcf, 'PaperUnits', 'inches', 'PaperPosition', [0 0 1.906666667 0.9]); plot(landmarks(1,:), landmarks(2,:), 'bh', 'MarkerFaceColor', 'b', 'MarkerSize', 20); plot(myRobot.x, myRobot.y, 'g^', 'MarkerFaceColor', 'g', 'MarkerSize', 15); for i = 1:N plot(particlesList(i).x, particlesList(i).y, 'r.', 'MarkerFaceColor', 'r'); end drawnow; if saveAnimation saveas(gcf,sprintf('img/particleStep_%02d', 1),'png'); end hold off; pause(1); %% Particle Filtering on Robot Localization % for t = 1:T % preliminary randomly set how much the robot should turn and how % far it should drive (random walk!) currentTurn = rand(1) * pi/4 - pi/8; % random turn between [-pi/8;pi/8] currentStep = rand(1) * 8; % random step between [0;8] % 1st Step - Robot moves myRobot = robotMove(myRobot, worldSize, currentTurn, currentStep); % 2nd Step - Robot senses Z = robotSense(myRobot, landmarks); % 3rd Step - Move all Particles (let them do the same movement as the robot) for i = 1:N particlesList(i) = robotMove(particlesList(i), worldSize, currentTurn, currentStep); end % 4th Step - Calculate Measurement Probabilitys (for Importance % Weight of each Particle) W = []; for i = 1:N W(i) = measurementProbability(particlesList(i), landmarks, Z); end % 5th Step - Resample Particles according to Measurement % Probabilitys index = int32( rand(1) * N ); if index == 0 index = 1; end beta = 0.0; mw = max(W); for i = 1:N beta = beta + rand(1) * 2.0 * mw; while beta > W(index) beta = beta - W(index); index = mod( index, N ) + 1; end newParticlesList(i) = particlesList(index); end particlesList = newParticlesList; % 6th Step - Plot Everything clf; hold on; axis([0 worldSize 0 worldSize]); set(gcf, 'PaperUnits', 'inches', 'PaperPosition', [0 0 1.906666667 0.9]); plot(landmarks(1,:), landmarks(2,:), 'bh', 'MarkerFaceColor', 'b', 'MarkerSize', 20); plot(myRobot.x, myRobot.y, 'g^', 'MarkerFaceColor', 'g', 'MarkerSize', 15); for i = 1:N plot(particlesList(i).x, particlesList(i).y, 'r.', 'MarkerFaceColor', 'r'); end drawnow; if saveAnimation saveas(gcf,sprintf('img/particleStep_%02d', t+1),'png'); end hold off; % 7th Step - Print Error disp( sprintf('Error is: %.6f', robotEvaluate(myRobot, particlesList, worldSize) ) ); pause(t); end input('Press Any Key to Exit...!'); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Robot Class Functions from CS373 - Course from UDACITY %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function robot = createRobot(worldSize) robot.x = rand(1) * worldSize; robot.y = rand(1) * worldSize; robot.orientation = rand(1) * 2.0 * pi; robot.forwardNoise = 0.0; robot.turnNoise = 0.0; robot.senseNoise = 0.0; end function robot = setRobot(robot, worldSize, newX, newY, newOrientation) if newX < 0 || newX >= worldSize error('X coordinate out of bound'); end if newY < 0 or newY >= worldSize error('Y coordinate out of bound'); end if newOrientation < 0 or newOrientation >= 2 * pi error('Orientation must be in [0..2pi]'); end robot.x = newX; robot.y = newY; robot.orientation = newOrientation; end function robot = setNoise(robot, newForwardNoise, newTurnNoise, newSensorNoise) % makes it possible to change the noise parameters % this is often useful in particle filters robot.forwardNoise = newForwardNoise; robot.turnNoise = newTurnNoise; robot.senseNoise = newSensorNoise; end function distList = robotSense(robot, landmarks) distList = []; for i = 1:size(landmarks,2) dist = sqrt((robot.x - landmarks(1,i)) ^ 2 + (robot.y - landmarks(2,i)) ^ 2); dist = dist + randn(1) * robot.senseNoise; distList(i) = dist; end end function robot = robotMove(robot, worldSize, turn, forward) if forward < 0 error('Robot cant move backwards'); end % turn, and add randomness to the turning command newOrientation = robot.orientation + turn + randn(1) * robot.turnNoise; newOrientation = mod(newOrientation, 2 * pi); % move, and add randomness to the motion command dist = forward + randn(1) * robot.forwardNoise; newX = robot.x + cos(newOrientation) * dist; newY = robot.y + sin(newOrientation) * dist; % cyclic truncate newX = mod(newX, worldSize); newY = mod(newY, worldSize); % set particle newRobot = createRobot(worldSize); newRobot = setRobot(newRobot, worldSize, newX, newY, newOrientation); newRobot = setNoise(newRobot, robot.forwardNoise, robot.turnNoise, robot.senseNoise); robot = newRobot; end function w = gaussianProbability(mu, sigma, x) % calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma w = exp(- ((mu - x) .^ 2) ./ (sigma .^ 2) ./ 2.0) ./ sqrt(2.0 .* pi .* (sigma .^ 2)); end function prob = measurementProbability(robot, landmarks, measurement) % calculates how likely a measurement should be % for every measurement-point (landmark) we first calculate the % distance of the particle to this landmark and then it's gaussian % probability prob = 1.0; for i = 1:size(landmarks,2) dist = sqrt((robot.x - landmarks(1,i)) ^ 2 + (robot.y - landmarks(2,i)) ^ 2); prob = prob * gaussianProbability(dist, robot.senseNoise, measurement(i)); end end function [] = robotDisplay(robot) % display current position and orientation disp(sprintf('[x=%.6f y=%.6f orient=%.6f]', robot.x, robot.y, robot.orientation)); end function sumError = robotEvaluate(robot, particleList, worldSize) sumError = 0.0; % calculate mean error for i = 1:length(particleList) dx = mod(particleList(i).x - robot.x + (worldSize/2.0), worldSize) - (worldSize/2.0); dy = mod(particleList(i).y - robot.y + (worldSize/2.0), worldSize) - (worldSize/2.0); err = sqrt(dx * dx + dy * dy); sumError = sumError + err; end sumError = sumError / length(particleList); end
Partikel Filter aus UDACITY Course CS373 - Artificial Intelligence
Beispiel für einen Partikel Filter zum Tracking eines Roboters
- Übersetzung von python nach MatLab
- Erweiterung um Kleinigkeiten (variable Anzahl Landmarks)
- Roboter bestimmt Position anhand der Landmarks
- Partikel bestimmen Position wie Roboter
- Random Walk des Roboters