matlab说话代码-particle-filter-localization-simple-implementation:粒子...

matlab说话代码 粒子滤波算法 1 粒子滤波算法过程解析 粒子滤波定位算法的思想也是贝叶斯规则： 以下的解析是结合Matlab代码所作的说明 初始化 初始化一堆粒子，在MatLab中的代码显示NP=50，也就是总共有50个采样粒子。Estimated State [x y yaw]，可见每个状态有3个数据。px=repmat(xEst,1,NP);， 计算初始权重pw=zeros(1,NP)+1/NP; 可见初始权重是均匀的 初始化路标，landMarks=[10 0; 10 10; 0 15; -5 20]; 可见一共有4个路标 预测：根据motion model与物体的控制信息u预测下个时刻粒子群中粒子的位置 doControl()函数，输入参数time，得到控制指令u doMotion()函数，输入初始状态x和控制指令u，得到下一时刻的x状态 doObservation()函数，输入参数xGnd（没有噪声的里程计位置估计）, xOdom（有噪声的里程计位置估计）, u（控制指令）, landMarks, MAX_RANGE，输出参数是z,xGnd,xOdom,u 更新：根据物