粒子滤波代码

压缩包中有三个粒子滤波的演示程序，一个滤波，一个目标跟踪，一个机器人定位。关于效果，大家可以先看看http://blog.csdn.net/heyijia0327/article/details/41142679。再决定是否下载。

%% SIR粒子滤波的应用，算法流程参见博客http://blog.csdn.net/heyijia0327/article/details/40899819
clear all
close all
clc
%% initialize the variables
x = 0.1; % initial actual state
x_N = 1; % 系统过程噪声的协方差  （由于是一维的，这里就是方差）
x_R = 1; % 测量的协方差
T = 75;  % 共进行75次
N = 100; % 粒子数，越大效果越好，计算量也越大

%initilize our initial prior particle distribution as a gaussian around
%the true initial value

V = 2; %初始分布的方差
x_P = []; % 粒子
% 用一个高斯分布随机的产生初始的粒子
for i = 1:N
    x_P（i） = x + sqrt（V） * randn;
end

z_out = [x^2 / 20 + sqrt（x_R） * randn];  %实际测量值
x_out = [x];  %the actual output vector for measurement values.
x_est = [x]; % time by time output of the particle filters estimate
x_est_out = [x_est]; % the vector of particle filter estimates.

for t = 1:T
    x = 0.5*x + 25*x/（1 + x^2） + 8*cos（1.2*（t-1）） +  sqrt（x_N）*randn;
    z = x^2/20 + sqrt（x_R）*randn;
    for i = 1:N
        %从先验p（x（k）|x（k-1））中采样
        x_P_update（i） = 0.5*x_P（i） + 25*x_P（i）/（1 + x_P（i）^2） + 8*cos（1.2*（t-1）） + sqrt（x_N）*randn;
        %计算采样粒子的值，为后面根据似然去计算权重做铺垫
        z_update（i） = x_P_update（i）^2/20;
        %对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p（y|x）对应下面的计算公式
        P_w（i） = （1/sqrt（2*pi*x_R）） * exp（-（z - z_update（i））^2/（2*x_R））;
    end
    % 归一化.
    P_w = P_w./sum（P_w）;
  
    %% Resampling
    %这里没有用博客里之前说的histc函数，不过目的和效果是一样的
    for i = 1 : N
        x_P（i） = x_P_update（find（rand <= cumsum（P_w）1））;   % 粒子权重大的将多得到后代
    end                                                     % find（ 1） 返回第一个 符合前面条件的数的 下标
    
    %状态估计，重采样以后，每个粒子的权重都变成了1/N
    x_est = mean（x_P）;
    
    % Save data in arrays for later plotting
    x_out = [x_out x];
    z_out = [z_out z];
    x_est_out = [x_est_out x_est];
    
end

t = 0:T;
figure（1）;
clf
plot（t x_out ‘.-b‘ t x_est_out ‘-.r‘‘linewidth‘3）;
set（gca‘FontSize‘12）; set（gcf‘Color‘‘White‘）;
xlabel（‘time step‘）; ylabel（‘flight position‘）;
legend（‘True flight position‘ ‘Particle filter estimate‘）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2211  2014-11-15 11:33  Particle_filter_SIR_demo1.m

    .......        28  2011-11-26 14:14  particle_filter_demo-master\.gitignore

    .......      4278  2011-11-26 14:14  particle_filter_demo-master\draw.py

     文件       4788  2014-10-22 23:05  particle_filter_demo-master\draw.pyc

    .......      8263  2011-11-26 14:14  particle_filter_demo-master\particle_filter.py

    .......      1358  2011-11-26 14:14  particle_filter_demo-master\README

     文件        713  2014-10-24 22:48  PF_Video_EN\calc_log_likelihood.m

     文件       1680  2014-10-23 21:09  PF_Video_EN\calc_log_likelihood_mex_handcoded.cpp

     文件        328  2014-10-24 20:32  PF_Video_EN\create_particles.m

     文件       1052  2014-11-15 11:35  PF_Video_EN\particle_filter_by_saved_movie.m

     文件        999  2014-10-23 21:09  PF_Video_EN\particle_filter_by_usb_camera.m

     文件     964717  2014-10-23 21:09  PF_Video_EN\Person.wmv

     文件        261  2014-10-23 21:09  PF_Video_EN\Readme.txt

     文件        322  2014-10-24 21:06  PF_Video_EN\resample_particles.m

     文件        153  2014-10-23 21:09  PF_Video_EN\show_particles.m

     文件        260  2014-10-23 21:09  PF_Video_EN\show_state_estimated.m

     文件     857622  2014-10-23 21:09  PF_Video_EN\Slides_LongVersion.pdf

     文件     569154  2014-10-23 21:09  PF_Video_EN\Slides_ShortVersion.pdf

     文件        258  2014-10-24 20:35  PF_Video_EN\update_particles.m

     目录          0  2014-10-22 23:05  particle_filter_demo-master

     目录          0  2014-11-15 11:42  PF_Video_EN

----------- ---------  ---------- -----  ----

              2418445                    21