demo_pf_nongaussian.m

粒子滤波程序

%2DOF, eye-in-hand
%本程序仿真实现2自由度机器人对目标的跟踪
%使用粒子滤波估计总雅可比矩阵J
%噪声为非高斯噪声

clear all;     
    disp('=====Simulating ,please wait.......');
	m=2; n=3;		%m-feature number; n-sita number
	L=[0.5, 0.5];	 %两杆长均为0.5米
	dt=0.05;
	N=121;              %卡尔曼迭代次数
    k_out=0;
	fd = [240,240]; %期望的图像特征
    S=300;              %粒子数目
    %----------产生噪声----------
    Q=10.^1;
    R=10.^0;
    K_Q=Q*eye(6);
    K_R=R*eye(2);
    W= sqrt(Q)*randn(6,N)+sqrt(400)*randn(6,N);
   	V=1*sqrt(R)*randn(2,N)+1*(sqrt(400)*(randn(2,N)));;        

%*********************************************
%==========系统初始化===========                                                                                                                                                                                                             
%*********************************************
k=1;
        [sitad(1),sitad(2)]=Function_xy_to_sita(L(1),L(2),0.5,0.5);    
        sita(:,1)=sitad;
        Jsita(:,:,1)=Function_Inital_Jacobian(sita,[0.8,0.5,0]);
        Jt(:,1)=[0 0];
        tmp_Jsita=Jsita(:,:,1);tmp_Jt=Jt(:,1);
        K_X(:,1)=[tmp_Jsita(1),   tmp_Jsita(3),   tmp_Jt(1),   tmp_Jsita(2),   tmp_Jsita(4),    tmp_Jt(2)]';
        K_P(:,:,1)=10.^5*eye(6);

    %@@@产生初始时刻的粒子样本@@@
        P_X(:,1)=[tmp_Jsita(1),   tmp_Jsita(3),   tmp_Jt(1),   tmp_Jsita(2),   tmp_Jsita(4),    tmp_Jt(2)]';
        s_x(:,:,1)=[ones(S,1)*P_X(1,1),ones(S,1)*P_X(2,1),ones(S,1)*P_X(3,1),ones(S,1)*P_X(4,1),ones(S,1)*P_X(5,1),ones(S,1)*P_X(6,1),];

	%-----运动点的初始位置-----
        yx(1)=0.8;
        yy(1)=0.5;
    %-----初始特征差-----
        yo(:,:,1)=[yx(1), yy(1),0];
        fe(:,1)=Function_image_feature(sita(:,1)',yo)-fd;	
    
	    
%******************************************************
%===第二个到第KM个时刻，时间t=N*dt ====
%******************************************************
for k=2:N
    %======目标点运动======
	    yx(k)=0.5+0.3*cos(1*(k-1)*dt);
	    yy(k)=0.5+0.2*sin(1*(k-1)*dt);   
   	%======机械手运动======
        Jsita_add=(Jsita(:,:,k-1)'*Jsita(:,:,k-1))^(-1)*Jsita(:,:,k-1)';
        fe_e=-fe(:,k-1);
        if (max(fe(:,k-1))>80)
            sita(:,k)=sita(:,k-1)+(1*Jsita_add*fe_e-Jsita_add*Jt(:,k-1)*dt)/5;
        else
            sita(:,k)=sita(:,k-1)+(1*Jsita_add*fe_e-Jsita_add*Jt(:,k-1)*dt);
        end 
        %----------若角度超出范围，则调整----------
                if (abs(sita(1,k))>=pi/2)        
                    sita(1,k)=pi/2*sign(sita(1,k)); 
                end  
                if (sita(2,k)>=pi)         	
                    sita(2,k)=pi;            
                end   
                if (sita(2,k)<0)     		
                    sita(2,k)=0;           
                end
	%======获取当前图像特征差======
        yo(:,:,k)=[yx(k), yy(k), 0];
        fe(:,k)=Function_image_feature(sita(:,k)',yo(:,:,k))-fd;%特征差
        %----------判断是否超出视野----------         
                    if (abs(fe(1,k))>240|abs(fe(2,k))>240)
                        disp('模拟的噪声太大,导致超出视野！请重试');
                        k_out=k;
                         %break;
                    end    
    %======计算观测向量Z======
        Z(:,k)=fe(:,k)-fe(:,k-1);    
 	%======求出H======
        ds=sita(:,k)-sita(:,k-1);
        H=[ ds', dt, 0,  0,  0 ;      0,  0,   0,  ds',    dt  ];
    %@@@计算dsita@@@
        dsita=[    [ds',dt]',[0,0,0]';
                        [0,0,0]',[ds',dt]'];
        %@@@@@@@@@@
        %======PF滤波======
        %@@@@@@@@@@
        [s_x(:,:,k)] = Function_bootstrap( S,  s_x(:,:,k-1),   Z(:,k), dsita); 
        P_X(:,k)=mean(s_x(:,:,k));
        J_tmp=P_X(:,k);   
        %----------根据观测值X得到Jsita----------
        Jsita(:,:,k)=[J_tmp(1),J_tmp(2);J_tmp(4),J_tmp(5)];
        Jt(:,:,k)=[J_tmp(3),J_tmp(6)];
end

  

if(k_out==0)
%**********************************************
%=============画图==============
% **********************************************
    %计算以度为单位的关节角
    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.15,0.85,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);
        if(k_out>0),
            t=1:N;
            plot(x(1,t),y(1,t));
            t=k_out-1:(k_out+1);
            plot(x(1,t),y(1,t),'y-*');
        else
            plot(x,y,'LineWidth',2);
        end
        hold on;  
        xlabel('x');
        ylabel('y'); 
        
        disp('PF Successful!!!');

%    save data_PF_Non_Gaussian.mat  N  sita  fe yx yy
end