粒子滤波计算过程

已知量：
1，初始状态
2，状态的协方差矩阵（协方差矩阵：状态为多维向量，所以需要一个协方差矩阵来统计多维向量之间的关系）
3，测量协方差矩阵（同上）
4，粒子数
5，状态转移矩阵或者方程——x(k-1) -> x(k)
6，迭代次数
7，系统方差（可以理解为系统的误差）
8，测量方差（可以理解为测量的误差）
9，测量函数矩阵或方程（由x -> y）

求：
1，状态估计输出 ——初始为 0

步奏一：
    a)：根据初始状态产生随机初始的粒子——粒子初始化，一般用高斯随机分布产生
    b)：由初始状态根据测量函数方程计算初始测量量

步骤二：进入迭代期，一般为时间轴
    a）：由初始状态根据状态转移矩阵或者方程计算当前状态
    b）：由当前状态根据测量状态矩阵或方程计算当前观测值
    c）：根据粒子数生成粒子，步骤如下：
         1）：根据状态转移矩阵或者方程生成当前粒子
         2）：根据测量函数矩阵或者方程计算粒子当前观测值
         3）：计算似然函数值，得到粒子权重值（根据粒子当前观测值和系统当前观测值计算似然函数值（高斯计算））
    d）：归一化所有粒子的权值（这样所有粒子的权值在 0 -1 之间）
    e）：随机重要性重采样所有粒子，获得随机数大于在粒子权值叠加界限下的下标
    f）：依据下标重新将当前粒子采样赋值给新粒子
    g）：状态估计为新粒子的均值

demo on matlab:

%% 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 = 88; % 粒子数，越大效果越好，计算量也越大  


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


V = 2; %初始分布的方差  
x_P = rand(1,N); % 粒子  
x_P2 = [];
% 用一个高斯分布随机的产生初始的粒子  
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_update(i) - z)^2/(2*x_R)); %已知z，求z_update的概率

    end  

    % 归一化.
    P_w = P_w./sum(P_w);  % for the later cumsum

    %% Resampling这里改用histc函数，效果一样 
    [~, dd] = histc(rand(1,N), cumsum(P_w));% if rand > cumsun(P_w),,dd will be zero
    x_P =  x_P_update(:, dd + 1); % dd shuld be from 1,,but what if dd is the dd is the max of the dd,, filter the new sampling

    %状态估计，重采样以后，每个粒子的权重都变成了1/N  
    x_est = mean(x_P);

    % Save data in arrays for later plotting  
    x_out = [x_out x];  %pingjie x_out
    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');