%2DOF, eye-in-hand
%本程序仿真實(shí)現(xiàn)2自由度機(jī)器人對目標(biāo)的跟蹤
%使用粒子濾波估計(jì)總雅可比矩陣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;              %卡爾曼迭代次數(shù)
    k_out=0;
	fd = [240,240]; %期望的圖像特征
    S=300;              %粒子數(shù)目
    %----------產(chǎn)生噪聲----------
    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)));;        

%*********************************************
%==========系統(tǒng)初始化===========                                                                                                                                                                                                             
%*********************************************
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);

    %@@@產(chǎn)生初始時(shí)刻的粒子樣本@@@
        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),];

	%-----運(yùn)動(dòng)點(diǎn)的初始位置-----
        yx(1)=0.8;
        yy(1)=0.5;
    %-----初始特征差-----
        yo(:,:,1)=[yx(1), yy(1),0];
        fe(:,1)=Function_image_feature(sita(:,1)',yo)-fd;	
    
	    
%******************************************************
%===第二個(gè)到第KM個(gè)時(shí)刻，時(shí)間t=N*dt ====
%******************************************************
for k=2:N
    %======目標(biāo)點(diǎn)運(yùn)動(dòng)======
	    yx(k)=0.5+0.3*cos(1*(k-1)*dt);
	    yy(k)=0.5+0.2*sin(1*(k-1)*dt);   
   	%======機(jī)械手運(yùn)動(dòng)======
        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 
        %----------若角度超出范圍，則調(diào)整----------
                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
	%======獲取當(dāng)前圖像特征差======
        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('模擬的噪聲太大,導(dǎo)致超出視野！請重試');
                        k_out=k;
                         %break;
                    end    
    %======計(jì)算觀測向量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  ];
    %@@@計(jì)算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);   
        %----------根據(jù)觀測值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)
%**********************************************
%=============畫圖==============
% **********************************************
    %計(jì)算以度為單位的關(guān)節(jié)角
    sitadeg=sita*180/pi;
    k1=1:N;%對應(yīng)時(shí)間為(k-1)*0.05秒
    t=(k1-1)*0.05;
  %=====關(guān)節(jié)角=====
    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');
%=====機(jī)器人實(shí)際軌跡=====
    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