机器人 里程计 下的 粒子滤波 相关介绍~~，持续更新(gmapping)

看了一段时间的粒子滤波这一块的内容，感觉很高sen，但是国外对该算法的推动和运用比较多~~希望我写的能对一些人有用处~如果发现文章有什么差错，请亲们立即指出，咱们共同学习~好了，废话少说，直接开干~

1. 粒子滤波是一种蒙特卡洛算法，需要一定的计算量

2. 粒子滤波解决的是贝叶斯计算后验概率时无法求解积分的问题，即使用平均值来代替积分~不知道有没有说明白，这个我们记住就可以，下文我会拿出一个公式来对应~进行相关的解释

由于个人 没找到相关的公式编辑器，所以只能上图片了，希望大家能理解~

上述两个公式 就是对应 2中我所叙述的问题~

解释完毕上面的问题，接下来开始说下粒子滤波的过程~

首先，我们需要知道状态模型和观测模型，举个例子如下所示，本文借用相关其他博客给的一个粒子，然后结以来说明阐述：

两个模型已经有了~接下来我们需要进行相关推进~

1、初始状态：用大量粒子模拟X（t），粒子在空间内均匀分布；这里的X（t）代表t时刻的粒子的状态，在机器人里程计里面代表的是（x,y,theta）,即全局的x，y坐标和机器人朝向与全局的坐标系下X轴的角度~！

2、预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子；这里的状态转移就是机器人的运动模型~详情可以参见概率机器人这本书。

3、校正阶段：对预测粒子进行评价，越接近于真实状态的粒子，其权重越大；通过观测值和估算值进行比较，进行权值赋予~

4、重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；重采样的过程就是避免粒子退化现象的发生，让权值大的多复制几次，权值小的少复制几次，这样能将预测的结果更加准确~

5、求估计结果，求估计结果的过程就是利用所复制出的粒子的所有状态进行求解均值得出我们预测的结果。一开始的时候感觉粒子滤波很高深，你们有没有想到最后只需要平均一下就能解决问题~哈？我开始的时候是想到了使用平均粒子求解位置，但是自己跟着其他论坛推公式的时候就把这一茬给我忘了~~！最后发现了平均，才恍然大悟。。。。

6、滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2。

下面给出一个程序，这个程序 是我copy的http://blog.csdn.net/heyijia0327/article/details/41142679，这个大家可以跟着我讲的去看看，感觉这篇博客讲的还可以

但是给大家另外一个渠道~去看视频http://v.youku.com/v_show/id_XMTM3MTE4MDkwOA==.html

徐老师讲的视频还是不错的~~你可以花一个小时就能完全了解粒子滤波的所有过程~~

另外 视频里面会涉及为什么使用蒙特卡洛方法，但是不清楚，我们可以看上面我给大家的博客地址，里面对使用蒙特卡洛做了一个很详细的叙述~，但是重要性概率密度~也就是q(x)函数的构造过程，为什么这么选取，选取这样的好处结果是什么，视频里面讲解的很清楚！比你看那个博客里面的需要清楚~~

如果我要把所有的推导公式全部放到博客里面得需要太多的图片，后期我自己会整理下 完善这个博客~

%% 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');

粒子滤波的程序中重采样使用的方法是利用轮盘赌的思想~大家可以看程序

 for i = 1 : N
        x_P(i) = x_P_update(find(rand <= cumsum(P_w),1));   % 粒子权重大的将多得到后代
    end                

就是这一块 我没想明白为什么这样做就会很精确~我认为应该还有其他的方法去代替这个轮盘赌的重采样方案~如果大家有什么好的方法希望提出，这一块重采样的原理我是明白，但是具体实现 我只找到这个方法~