www.gusucode.com > robotics 案例源码程序 matlab代码 > robotics/UseParticleFilterToEstimateStateInALoopExample.m
%% Estimate Robot Position in a Loop Using Particle Filter % Use the |ParticleFilter| object to track a % robot as it moves in a 2-D space. The measured position has random % noise added. Using |predict| and |correct|, track the robot % based on the measurement and on an assumed motion model. %% % Initialize the particle filter and specify the default state % transition function, the measurement likelihood function, and the resampling % policy. pf = robotics.ParticleFilter; pf.StateEstimationMethod = 'mean'; pf.ResamplingMethod = 'systematic'; %% % Sample 1000 particles with an initial position of [0 0] and unit % covariance. initialize(pf,1000,[0 0],eye(2)); %% % Prior to estimation, define a sine wave path for the dot to follow. % Create an array to store the predicted and estimated position. % Define the amplitude of noise. t = 0:0.1:4*pi; dot = [t; sin(t)]'; robotPred = zeros(length(t),2); robotCorrected = zeros(length(t),2); noise = 0.1; %% % Begin the loop for predicting and correcting the estimated position based on measurements. % The resampling of particles occurs based on the |ResamplingPolicy| property. The robot % moves based on a sine wave function with random noise added to the % measurement. for i = 1:length(t) % Predict next position. Resample particles if necessary. [robotPred(i,:),robotCov] = predict(pf); % Generate dot measurement with random noise. This is % equivalent to the observation step. measurement(i,:) = dot(i,:) + noise*(rand([1 2])-noise/2); % Correct position based on the given measurement to get best estimation. % Actual dot position is not used. Store corrected position in data array. [robotCorrected(i,:),robotCov] = correct(pf,measurement(i,:)); end %% % Plot the actual path versus the estimated position. Actual results may % vary due to the randomness of particle distributions. plot(dot(:,1),dot(:,2),robotCorrected(:,1),robotCorrected(:,2),'or') xlim([0 t(end)]) ylim([-1 1]) legend('Actual position','Estimated position') grid on %% % The figure shows how close the estimate state matches the actual position % of the robot. Try tuning the number of particles or specifying a % different initial position and covariance to see how it affects tracking % over time.