main.asv

基于声纳信息的粒子滤波算法

%main function
global num;
global error1;
global Robottest;
global Robot;
global p;
global lastx;
global lasty;
global lastTh;
global sonar;
global plotnum;
global errornum;
global step;
step=0;
errornum(1:30]=0;

plotnum=1;
num=300;
error1=0.05;
Robottest(1:12)=[random('normal',3.5,0.1),random('normal',2.5,0.1),360*rand(1),0, 0, 0, 0, 0, 0, 0, 0, 0];
Robot(1:12)=[0];
sonar=[-1.57,-0.87222,-0.52333,-0.17444,0.17444,0.52333,0.87222,1.57];
p(1:num,1:12)=[0];
lastx=Robottest(1);
lasty=Robottest(2);
lastTh=Robottest(3);
Orientat;