机器人学习--粒子滤波定位-MATLAB仿真1

坐标系下，给定几个路标 landmarks，机器人行走路线是一个圆，60秒的行走时间，每秒更新一次结果，MATLAB仿真运动更新过程，以及结果的误差图显示。

代码：

function [] = particle_filter_localization()
%PARTICLE_FILTER_LOCALIZATION Summary of this function goes here
%   Detailed explanation goes here% -------------------------------------------------------------------------
% TASK for particle filter localization
% for robotic class in 2018 of ZJU% Preparartion:
% 1. you need to know how to code and debug in matlab
% 2. understand the theory of Monte Carlo% Then complete the code by YOURSELF!
% -------------------------------------------------------------------------close all;
clear all;disp('Particle Filter program start!!')%% initialization
tic;
time = 0;
endTime = 60; % second
global dt;
dt = 0.1; % secondnSteps = ceil((endTime - time)/dt);localizer.time = [];
localizer.xEst = [];
localizer.xGnd = [];
localizer.xOdom = [];
localizer.z = [];
localizer.PEst=[];
localizer.u=[];% Estimated State [x y yaw]'
xEst=[0 0 0]';
% GroundTruth State
xGnd = xEst;
% Odometry-only = Dead Reckoning
xOdom = xGnd;% Covariance Matrix for predict
Q=diag([0.1 0.1 toRadian(3)]).^2;
% Covariance Matrix for observation
R=diag([1]).^2;% range:meter% Simulation parameter
global Qsigma
Qsigma=diag([0.1 toRadian(5)]).^2;
global Rsigma
Rsigma=diag([0.1]).^2;% landmark position
landMarks=[10 0; 10 10; 0 15; -5 20];% longest observation confined
MAX_RANGE=20;
% Num of particles, initialized
NP=100;
% Used in Resampling Step, a threshold
NTh=NP/2.0;% particles produced
px=repmat(xEst,1,NP);
% weights of particles produced
pw=zeros(1,NP)+1/NP;errs=[];
% sum of error
%% Main Loop for i=1 : nStepstime = time + dt;u=doControl(time);% do observation[z,xGnd,xOdom,u]=doObservation(xGnd, xOdom, u, landMarks, MAX_RANGE);for ip=1:NP% process every particlex=px(:,ip);w=pw(ip);% do motion model and random samplingx=doMotion(x, u)+sqrt(Q)*randn(3,1);% calculate inportance weightfor iz=1:length(z(:,1))pz=norm(x(1:2)'-z(iz,2:3));dz=pz-z(iz,1);w=w*Gaussian(dz,0,sqrt(R));endpx(:,ip)=x;pw(ip)=w;endpw=Normalization(pw,NP);[px,pw]=ResamplingStep(px,pw,NTh,NP);xEst=px*pw';errs=[errs, norm(xGnd(1:2)'-xEst(1:2)')];% Simulation Resultlocalizer.time=[localizer.time; time];localizer.xGnd=[localizer.xGnd; xGnd'];localizer.xOdom=[localizer.xOdom; xOdom'];localizer.xEst=[localizer.xEst;xEst'];localizer.u=[localizer.u; u'];% Animation (remove some flames)if rem(i,10)==0 hold off;arrow=0.5;for ip=1:NPquiver(px(1,ip),px(2,ip),arrow*cos(px(3,ip)),arrow*sin(px(3,ip)),'ok');hold on;endplot(localizer.xGnd(:,1),localizer.xGnd(:,2),'.b');hold on;plot(landMarks(:,1),landMarks(:,2),'pk','MarkerSize',10);hold on;if~isempty(z)for iz=1:length(z(:,1))ray=[xGnd(1:2)';z(iz,2:3)];plot(ray(:,1),ray(:,2),'-r');hold on;endendplot(localizer.xOdom(:,1),localizer.xOdom(:,2),'.k');hold on;plot(localizer.xEst(:,1),localizer.xEst(:,2),'.r');hold on;axis equal;grid on;drawnow;endend% draw the final results of localizer, compared to odometry & ground truth
drawResults(localizer);toc
num2str(sum(errs.^2))
figure(2)
plot(0.1:0.1:60,errs)
xlabel("time")
ylabel("error square")
end%% Other functions% degree to radian
function radian = toRadian(degree)radian = degree/180*pi;
endfunction []=drawResults(localizer)
%Plot Resultfigure(1);hold off;x=[ localizer.xGnd(:,1:2) localizer.xEst(:,1:2)];set(gca, 'fontsize', 12, 'fontname', 'times');plot(x(:,1), x(:,2),'-.b','linewidth', 4); hold on;plot(x(:,3), x(:,4),'r','linewidth', 4); hold on;plot(localizer.xOdom(:,1), localizer.xOdom(:,2),'--k','linewidth', 4); hold on;title('Localization Result', 'fontsize', 12, 'fontname', 'times');xlabel('X (m)', 'fontsize', 12, 'fontname', 'times');ylabel('Y (m)', 'fontsize', 12, 'fontname', 'times');legend('Ground Truth','Particle Filter','Odometry Only');grid on;axis equal;endfunction [ u ] = doControl( time )
%DOCONTROL Summary of this function goes here
%   Detailed explanation goes here%Calc Input ParameterT=10; % [sec]% [V yawrate]V=1.0; % [m/s]yawrate = 5; % [deg/s]u =[ V*(1-exp(-time/T)) toRadian(yawrate)*(1-exp(-time/T))]';end%%  you need to complete% do Observation model
function [z, xGnd, xOdom, u] = doObservation(xGnd, xOdom, u, landMarks, MAX_RANGE)global Qsigma;global Rsigma;% Gnd Truth and OdometryxGnd=doMotion(xGnd, u);% Ground Truthu=u+sqrt(Qsigma)*randn(2,1); % add noise randomlyxOdom=doMotion(xOdom, u); % odometry only%Simulate Observationz=[];for iz=1:length(landMarks(:,1))% d = ?d = norm(xGnd(1:2)'-landMarks(iz,:));if d<MAX_RANGE z=[z;[d+sqrt(Rsigma)*randn(1,1) landMarks(iz,:)]];   % add observation noise randomlyendend
end% do Motion Model
function [ x ] = doMotion( x, u)global dt;Delta = [ dt*cos(x(3)) 0dt*sin(x(3)) 00 dt];% x = ?x = x + Delta*u;
end% Gauss function
function g = Gaussian(x,u,sigma)g = (1/sqrt(2*pi*sigma))*exp(-x^2/(2*sigma));
end% Normalization
function pw=Normalization(pw,NP)pw = pw./sum(pw);
end% Resampling
function [px,pw]=ResamplingStep(px,pw,NTh,NP)Neff = 1/sum(pw.*pw);if Neff<NThpx_ = px;for i=1:NPu = rand;tempsum = 0;for j=1:NPtempsum = tempsum + pw(j);if tempsum >= upx_(:,i) = px(:,j);break;endendendpx = px_;pw=zeros(1,NP)+1/NP;end
end

参考：Matlab：粒子滤波定位仿真__朝闻道_的博客-CSDN博客_粒子滤波定位matlab我一定要吐槽一下机器人学，秋学期课死多，上课么全是高大上算法，基本听不懂，居然还有期中考试。跟前面那个ROS大作业一起要在12.2前交，然而11.18才考完，我好怕啊。...https://blog.csdn.net/weixin_42231070/article/details/83795438

另一个版本：https://github.com/hujunhan/Particle-Filter-Localizationhttps://github.com/hujunhan/Particle-Filter-Localization

机器人学习--粒子滤波定位-MATLAB仿真1相关推荐

机器人学习--粒子滤波SLAM/MCL定位参考资料+学习经验
学习材料1: <概率机器人学> 谷歌无人驾驶之父 Sebastian Thrun等人著作. 注释: 可能是本人智商有限,或者是移动机器人学领域的基础知识了解不多. 刚刚看这本书的时候,尤其 ...
机器人学习--粒子滤波及其在定位中的应用
前提基础,先看一下贝叶斯滤波和蒙特卡洛方法一.什么是粒子滤波? 这里有个基于粒子滤波的物体跟踪案例说明: 参考:基于粒子滤波的物体跟踪 - yangyangcv - 博客园如果还是看不懂 ...
机器人学习--粒子滤波/MCL定位的理论基础（先验知识）
跨学科(未学过数理统计和滤波等课程)的研究人员看懂粒子滤波或MCL定位的理论 2019年剑桥大学一名教授 Simon Godsill 发表了一篇论文: Godsill S. Particle filt ...
机器人学习--网友资料系列激光SLAM建图、粒子滤波定位和位姿图优化
一.移动机器人自主导航的前提是在未知环境中先构建地图 (目前市内很多用的2D激光雷达,构建栅格地图,相当于立体空间中的某个水平面高度的切面) 一般用的是2D 激光SLAM算法构建概率栅格占用地图: ...
蒙特卡洛粒子滤波定位算法_蒙特卡罗定位算法(基于粒子滤波的定位算法) ——原理、理解与仿真...
1 算法原理 1.1 机器人定位问题关于机器人定位,有三大问题,它们分别是: (1)"全局定位":指初始位置未知,机器人靠自身运动确定自己在地图中的位姿. (2)"位姿 ...
蒙特卡洛粒子滤波定位算法_ROS -- 最简单的自主ACML定位
关于定位的原理和算法: ARUANTOU:自适应MCL(KLD采样)zhuanlan.zhihu.com ARUANTOU:定位算法 -- MCL蒙特卡洛(粒子)滤波zhuanlan.zhihu. ...
卫星伪距定位matlab,GPS卫星运动及定位matlab仿真.doc
书山有路勤为径! PAGE GPS卫星运动及定位matlab仿真摘要全球定位系统是具有全球性.全能性.全天候优势的导航定位.定时和测速系统,现在在全球很多领域获得了应用. GPS卫星的定位是一个比 ...
蒙特卡洛粒子滤波定位算法_基于粒子滤波的TBD算法仿真—MATLAB仿真
目标跟踪的最终目的是在最小的误差下确定目标的位置,而在无线传感器网络中要实现这个目的需要很多相关技术的支持,如定位技术.目标检测技术.估计技术.节能技术等.目标跟踪问题的求解有很多方法, 从算法的考虑 ...
rbpf粒子滤波slam matlab程序_学习笔记（优达学城）- 车辆定位之粒子滤波器（整合版）...
1.代码传送门首先,一如既往的,打开传送门! Fred159/CarND-Kidnapped-Vehicle-Projectgithub.com 代码, 很重要,但更重要的是从代码的行与行之间探索 ...

机器人学习--粒子滤波定位-MATLAB仿真1

机器人学习--粒子滤波定位-MATLAB仿真1相关推荐

最新文章

热门文章