%2DOF, eye-in-hand
%本程序仿真實現2自由度機器人對目標的跟蹤
%使用kalman濾波估計總雅可比矩陣J
%噪聲為非高斯噪聲

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)*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(:,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




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'); 
   
%    save data_KF_Non_Gaussian.mat  N  sita  fe yx yy
end