demo3_j_trace.m

卡尔曼滤波估计雅可比的matlab代码,用于计算机视觉的在线估计

%2DOF, eye-in-hand
%本程序仿真实现2自由度机器人对目标的跟踪
%使用估计总雅可比矩阵J的kalman滤波，J包含了Jsita和Jt

clear all;     
	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]; %期望的图像特征
    %----------产生噪声----------
    Q=10.^1*eye(6);
    R=10.^0*eye(2);
    W= sqrt(Q(1))*randn(6,N);
   	V=sqrt(R(1))*randn(2,N);    
       
%*********************************************
%==========系统初始化===========                                                                                                                                                                                                             
%*********************************************
k=1;
    [sitad(1),sitad(2)]=Function_xy_to_sita(L(1),L(2),0.5,0.5);    
    sita(:,k)=sitad;
    Jsita(:,:,k)=Function_Inital_Jacobian(sita,[0.8,0.5,0]);
    Jt(:,k)=[0 0];
    tmp_Jsita=Jsita(:,:,k);tmp_Jt=Jt(:,k);
	X(:,k)=[tmp_Jsita(1),   tmp_Jsita(3),   tmp_Jt(1),   tmp_Jsita(2),   tmp_Jsita(4),    tmp_Jt(2)]';
	P(:,:,k)=10.^5*eye(6);
	%-----运动点的初始位置-----
	yx(k)=0.8;
	yy(k)=0.5;
    %-----初始特征差-----
	yo(:,:,k)=[yx(k), yy(k),0];
	fe(:,k)=Function_image_feature(sita(:,k)',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======
        dsita=sita(:,k)-sita(:,k-1);
        H=[ dsita', dt, 0,  0,  0 ;      0,  0,   0,  dsita',    dt  ];
    %======kalman滤波======
        [X(:,k),P(:,:,k)] =Function_kalman_filter( m,n,X(:,k-1),P(:,:,k-1),Z(:,k),H,W(:,k-1),Q,R);    
    %----------根据观测值X得到Jsita----------
        J_tmp=X(:,k);
        Jsita(:,:,k)=[J_tmp(1),J_tmp(2);J_tmp(4),J_tmp(5)];
        Jt(:,:,k)=[J_tmp(3),J_tmp(6)];
end





%**********************************************
%=============画图==============
% **********************************************
    %计算以度为单位的关节角
    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'); 

% delete data_J_trace.mat        
%   save data_J_trace.mat  N  sita  fe yx yy