main.m

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

%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 stepnum;
stepnum=1;
errornum(1:30)=0;
plotnum=1;
num=1000;
error1=0.04;
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;