demo_plot1_kf.m

来自「机器人运行轨迹描绘的第二种方法」· M 代码 · 共 72 行

 clear;
 %%%导入数据画图
  %%%保存的数据格式为: save filename  N  sita  fe yx yy
  
%噪声为非高斯的前提下KF的估计结果
    load data_KF_Non_Gaussian.mat
    sita1=sita;  
    fe1=fe; 
    
%**********************************************
%=============画图==============
% **********************************************
    %计算以度为单位的关节角
    sitadeg=sita*180/pi;
    k1=1:N;%对应时间为(k-1)*0.05秒
    t=(k1-1)*0.05;
  %=====关节角=====
    figure();	
        subplot(2,1,1);
                plot(k1-1,sitadeg(1,:));
                xlabel('k');
                ylabel('sita_1');
        subplot(2,1,2);
                plot(k1-1,sitadeg(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(k1-1,fe(1,:),'LineWidth',1);
                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(k1-1,fe(2,:),'LineWidth',1);
                xlabel('k');
                ylabel('fe_y');
%=====机器人实际轨迹=====
    figure();
        grid on; 
        hold on;
        axis([0.1,0.9,0.25,0.75]);
        hold on;
        plot(yx,yy,'--r');
        hold on;    
        plot(yx(1),yy(1),'--rs','LineWidth',2,...
            'MarkerEdgeColor','k',...
            'MarkerFaceColor','r',...
            'MarkerSize',10);
        x=cos(sita(1,:))/2+cos(sita(1,:)+sita(2,:))/2;
        y=sin(sita(1,:))/2+sin(sita(1,:)+sita(2,:))/2;
        plot(x(1),y(1),'--ro','LineWidth',2,...
            'MarkerEdgeColor','k',...
            'MarkerFaceColor','b',...
            'MarkerSize',10);
        plot(x,y,'LineWidth',2);
        hold on;  
        xlabel('x');
        ylabel('y');