demo_plot2_pf_and_kf.m

机器人运行轨迹描绘的第二种方法

 clear;
  %%%保存的数据格式为: save filename  N  sita  fe yx yy
  
%细线，KF得到的非高斯条件下结果
    load data_KF_Non_Gaussian.mat
    sita1=sita;  
    fe1=fe; 
    
%粗线，PF得到的非高斯条件下结果
    load data_PF_Non_Gaussian.mat
    sita2=sita;  
    fe2=fe; 
        
            %计算以度为单位的关节角
                sitadeg1=sita1*180/pi;
                sitadeg2=sita2*180/pi;
                k=1:N;%对应时间为(k-1)*0.05秒
                t=(k-1)*0.05;
  %=====关节角=====
                figure();	
                subplot(2,1,1);
plot(k-1,sitadeg1(1,:),'LineWidth',1);
                hold on;
plot(k-1,sitadeg2(1,:),'b','LineWidth',2);
                xlabel('k');
                ylabel('sita_1');
                subplot(2,1,2);
plot(k-1,sitadeg1(2,:),'LineWidth',1);
                hold on;
plot(k-1,sitadeg2(2,:),'b','LineWidth',2);
                 xlabel('k');
                ylabel('sita_2');
%=====图象特征误差=====
                   figure();
                  subplot(2,1,1)
                  axis([0,N,-40,40]);
                  hold on;
                  plot([0:1:N],0*ones(1,N+1),'y');
                  hold on;
 plot(k-1,fe1(1,:),'LineWidth',1);
                 hold on;
 plot(k-1,fe2(1,:),'b','LineWidth',2);
                 xlabel('k');
                 ylabel('fe_x');
  
                subplot(2,1,2)
                axis([0,N,-50,150]);
                hold on;
                 plot([0:1:N],0*ones(1,N+1),'y');
                  hold on;
plot(k-1,fe1(2,:),'LineWidth',1);
                  hold on;
plot(k-1,fe2(2,:),'b','LineWidth',2);
                    xlabel('k');
                   ylabel('fe_y');
%=====机器人实际轨迹=====
             figure();
               grid on; 
               hold on;% axis([0,1,0,1]) 
 plot(yx,yy,':b');
               hold on;    
                 plot(yx(1),yy(1),'--rs','LineWidth',2,...
                             'MarkerEdgeColor','k',...
                             'MarkerFaceColor','r',...
                             'MarkerSize',10);
 x=cos(sita1(1,:))/2+cos(sita1(1,:)+sita1(2,:))/2;
 y=sin(sita1(1,:))/2+sin(sita1(1,:)+sita1(2,:))/2;
 
  x2=cos(sita2(1,:))/2+cos(sita2(1,:)+sita2(2,:))/2;
 y2=sin(sita2(1,:))/2+sin(sita2(1,:)+sita2(2,:))/2;

                 plot(x(1),y(1),'--ro','LineWidth',2,...
                             'MarkerEdgeColor','k',...
                             'MarkerFaceColor','b',...
                             'MarkerSize',10);
plot(x,y);
hold on;
plot(x2,y2,'b','LineWidth',2);
                 hold on;  
                 xlabel('x');
                 ylabel('y');