%2DOF, eye-in-hand
%此程序仿真實現二自由度機器人對平面點的定位

clear;     
	m=2; n=2;		%m-feature number; n-sita number
	L=[0.5, 0.5];	 %兩桿長均為0.5米
	dt=0.05;
	N=50;               %卡爾曼迭代次數
    k_out=0;
	fd = [240,240]; %期望的圖像特征

    %----------產生噪聲----------
    Q=10.^1*eye(4);
    R=10.^0*eye(2);
    W= sqrt(Q(1))*randn(4,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;
    Jt(:,k)=[0 0];
    Jsita(:,:,k)=Function_Inital_Jacobian(sita,[0.8,0.5,0]);
    tmp_jx=Jsita(:,:,k);
	X(:,k)=[tmp_jx(1);tmp_jx(3);tmp_jx(2);tmp_jx(4)];sqrt(Q
	P(:,:,k)=10.^5*eye(4);
	%-----點的初始位置-----
	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);
 	    yx(k)=0.8;
 	    yy(k)=0.5;        
   	%======機械手運動======
    Jsita_add=(Jsita(:,:,k-1)'*Jsita(:,:,k-1))^(-1)*Jsita(:,:,k-1)';
    fe_e=-fe(:,k-1);
    sita(:,k)=sita(:,k-1)+(Jsita_add*fe_e-Jsita_add*Jt(:,k-1)*dt);
            %----------若角度超出范圍，則調整----------
            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',0,0;0,0,dsita'];
    %======kalman濾波======
    [X(:,k),P(:,:,k)] =Function_kalman_filter(  m,n,X(:,k-1),P(:,:,k-1),Z(:,k),H,W(:,k),Q,R);    
        %----------根據觀測值X得到Jsita----------
        J_tmp=X(:,k);
        Jsita(:,:,k)=[J_tmp(1),J_tmp(2);J_tmp(3),J_tmp(4)];
        Jt(:,k)=[0 0];
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;
        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);
        end
        hold on;  
        xlabel('x');
        ylabel('y'); 

%         save data_static  N  sita  fe yx yy