Automotive Vision_5 II Particle Filter（粒子滤波）

Kalman Filter is limited to linear gaussian models
Particle Filter has no model limitations but is more computational expensive
粒子滤波更加通用却更加耗算力

下面的图很好的解释了过程，通过几次过程就可以判断更为准确的位置

load('data_simple.mat');

% initialization
numParticles = 500;
initial_state_uncertainty = 4; % initial uncertainty: 4m
initial_state_cov_matrix = diag([initial_state_uncertainty^2 initial_state_uncertainty^2]);

% the particle set, the weight is supposed to be stored in the first column. Please define
% the remaining part of the particle set vector according to the task.
% initialization of particles
particles = pf_init (numParticles, initial_state_cov_matrix);

% preparation for simulation of particle filter
particle_sets = cell(numT + 1, 3);
particle_sets{1, 3} = particles;

% this vectors stores the list of estimated states (excluding the initial state). 
state_history = zeros(numT, 2);

% main loop of particle filter
for i = 1:numT
    particles_predicted = pf_predict(particles, delta_motion(i,:), noise_system);
    particles_updated = pf_update(particles_predicted, measurements(i,:), noise_meas);
    particles_resampled = pf_resample(particles_updated);
    particles = particles_resampled;
    
    % store the particle sets for the simulation
    particle_sets(i+1,:) = {particles_predicted, particles_updated, particles_resampled};    

    % store the estimated state after resampling for error analysis in 'state_history'
    state_history (i,:) = compute_particle_statistics (particles_updated);
end

% analyze the estimation error
mean_error = analyze_state_error(state_history, true_poses);

% simulate the particle filter
simulate_particle_filter(particle_sets, true_poses);

function particles = pf_init (numParticles, initial_state_cov_matrix)
% Create initial set of numParticles particles around 0 with random 
% deviation given by the covariance matrix initial_state_cov_matrix.
% Each particle should contain in its first column its weight and the 
% other columns its state.

particles = zeros(numParticles, 3);

% fill in your code below this line
particles (:,1) = 1/numParticles;%第一个是概率，w,都是一样的
particles (:,2:3) = mvnrnd([0 0], initial_state_cov_matrix, numParticles);%随机生成高斯分布点,x的坐标随机分布


end

function particles_pred = pf_predict(particles, delta_motion, noise_system)
particles_pred = zeros(size(particles));

% number of particles
num_particles = size(particles, 1);

% keep the weights during prediction
particles_pred(:,1) = particles(:,1);%w不变
% update the delta motion with noise
particles_pred(:,2:3) = particles(:,2:3) + repmat(delta_motion, num_particles, 1) + mvnrnd([0 0], noise_system, num_particles);%加入系统动态和噪声
%repmat重复矩阵
end

function particles_upd = pf_update(particles, measurement, noise_meas)
particles_upd = zeros(size(particles));

num_particles = size(particles, 1);

% the state variables are kept
particles_upd(:,2:3) = particles(:,2:3);%点位置不变

for i = 1:num_particles 
particles_upd(i,1) = particles(i,1) .* mvnpdf(measurement, particles(i,2:3), noise_meas);%w变化，加入了噪声，根据坐标以及测量值用高斯模型来修正w，所以是根据测量点的距离来判断w
end

function particles_resampled = pf_resample(particles)
particles_resampled = zeros(size(particles));
num_particles = size(particles, 1);
normalized_weights = particles(:,1) ./ sum(particles(:,1), 1);
%把weights和变成1再重新计算
% weights are distributed uniformly
particles_resampled(:,1) = repmat(1/num_particles, num_particles, 1);
%重新有同等weight的矩阵
% draw samples according to normalized weights，根据新的normalized_weights来计算各个粒子的分布。
indices = mnrnd(num_particles, normalized_weights);
cur_index = 1;
for i = 1:num_particles
    amount = indices(i);
    if (amount > 0)
        particles_resampled(cur_index:cur_index+amount-1, 2:3) = repmat(particles(i,2:3), amount, 1);
        cur_index = cur_index + amount;
    end
end

end

这个是一个一直再走直线的点，所以也没有感觉粒子有明显的变化

Robot Localization

这个代码跟上面其实比较类似，也是分了3步，不同的就是加入了一个面向角度，毕竟是机器人需要考虑面向