粒子滤波器的Matlab实现_粒子滤波matlab实现-优快云博客

本文深入浅出地讲解了粒子滤波器的工作原理及其在机器人定位中的应用，通过MATLAB代码实现了从理论到实践的全过程，特别关注了重采样和权重计算两个关键环节。

前言：粒子滤波器相较于卡尔曼滤波器或者UKF无迹卡尔曼滤波器而言，可以表达强非线性的变换且无需假设后验分布为高斯分布，在描述多峰分布时具有非常大的优势。粒子滤波器被广泛的应用于机器人系统中，如著名的Gmapping算法便是在粒子滤波器的基础上实现的，但是当前网络中对粒子滤波器的描述往往浅尝则止或长篇大论，导致学习起来往往是了解大概流程而不懂实际代码实现，无法应用于自己机器人中或困于理论推导。因此本文将简要的说明粒子滤波器的流程，然后在matlab中一步步的实现粒子滤波器，其中核心代码仅有20行，非常便于理解从而打通由理论到实际的过程。

原理介绍

算法流程

根据上一次的粒子分布和控制命令更新粒子位置
对更新后的粒子计算在该粒子的情况下，获得传感器观测的概率作为权重
根据权重进行重采样

一、根据上一次分布和控制指令更新粒子位置

假设某一粒子位置为 $x = \left[ x,y,\theta \right]^T$ ,控制命令为 $u_t= \begin{bmatrix} \theta & L \end{bmatrix}$ ,即旋转一定角度后前进一定的距离。跟新粒子的位置如下

$x = x + R*u_t + rand$

%控制机器人前进
function pos = move(robot_pos,set_w,set_v,control_noise)
    % 先进行旋转,加上噪声，并归一化到0~pi*2 
    pos(3) = mod(robot_pos(3) + set_w + control_noise*rand(),2*pi);
    % 前进距离+噪声
    V = set_v + control_noise*rand();
    % 坐标变换，因为前进距离是车体坐标系而要求的是世界坐标系下的
    pos(1) = mod(robot_pos(1) - V*sin(pos(3)),200); 
    pos(2) = mod(robot_pos(2) + V*cos(pos(3)),200); 
end