iteration_image_jacobian_robot_kalman.m

Kalman Example for robotics Visual Servoing

clear
close all
randn('seed',1900000)
L=5;
R{1}=link([-pi/2 0 pi/2 10 1]);
R{2}=link([pi/2 0 0 9 0]);
R{3}=link([0 9 0 0 0]);
R{4}=link([0 9 0 0 0]);
R{5}=link([0 6.563 0 0 0]);
xrhino=robot(R,'xraahino');
xrhino.base=roty(pi/2);
xi=10;yi=10;zi=25;thetai=pi/6;phii=pi/2;
Initial=[xi yi zi phii thetai]';
xf=20;yf=-5;zf=20;thetaf=pi/3;phif=pi/6;
Final=[xf yf zf phif thetaf]';
xi1=xi-L/2*sin(thetai)*cos(phii);xi2=xi+L/2*sin(thetai)*cos(phii);
yi1=yi-L/2*sin(thetai)*sin(phii);yi2=yi+L/2*sin(thetai)*sin(phii);
zi1=zi-L/2*cos(thetai);zi2=zi+L/2*cos(thetai);
angle1=asin((zi2-zi1)/L)
angle2=acos((xi2-xi1)/(L*cos(angle1)))
tri=transl(xi2,yi2,zi2)*rotz(angle2)*roty(-angle1)
qi=ikine(xrhino,tri,[11.0380   -phii    1.3794   -0.2233   -1.1289],[1 1 1 1 1 0])
Start_position=[xi1 yi1 zi1;xi2 yi2 zi2]';% Start position of the robot which respective to the Robot frame
xf1=xf-L/2*sin(thetaf)*cos(phif);xf2=xf+L/2*sin(thetaf)*cos(phif);
yf1=yf-L/2*sin(thetaf)*sin(phif);yf2=yf+L/2*sin(thetaf)*sin(phif);
zf1=zf-L/2*cos(thetaf);zf2=zf+L/2*cos(thetaf);
anglef1=asin((zf2-zf1)/L);
anglef2=acos((xf2-xf1)/(L*cos(anglef1)));%trf=transl(xf2,yf2,zi2)*rotz(anglef2)*roty(-anglef1)
Final_position=[xf1 yf1 zf1;xf2 yf2 zf2]'; % Final position of the robot which respective to the Robot frame
C_T_R=transl(40,20,-190)*rotz(pi/2)*roty(pi/2);   % Robot respects to the camera
R_T_C=pinv(C_T_R); % Camera respects to the robot
%%%%%%%%%%%%%%%%%%%%%%%% Homogenous Transformation Metrix%%%%%%%%%%%%%%
r=50;
CPr=C_T_R*[Start_position;1 1];  %Start_point(real point) which is respective to the camera frame
CXr1=CPr(1,1);CYr1=CPr(2,1);CZr1=CPr(3,1);
CXr2=CPr(1,2);CYr2=CPr(2,2);CZr2=CPr(3,2);
MXr1=CXr1;MYr1=CYr1;MZr1=CZr1+300;
MXr2=CXr2;MYr2=CYr2;MZr2=CZr2+300;
MX1=sqrt(MXr1^2+MYr1^2+MZr1^2);
MX2=sqrt(MXr2^2+MYr2^2+MZr2^2);
MCO1=r/(2*MX1-r);
MCO2=r/(2*MX2-r);
MXm1=MCO1*MXr1;MYm1=MCO1*MYr1;MZm1=MCO1*MZr1;
MXm2=MCO2*MXr2;MYm2=MCO2*MYr2;MZm2=MCO2*MZr2;
CXm1=MXm1;CYm1=MYm1;CZm1=MZm1-300; 
CXm2=MXm2;CYm2=MYm2;CZm2=MZm2-300; 
CPm1=[CXm1;CYm1;CZm1;1];%Start_point(reflected from mirror) which is respective to the camera frame
CPm2=[CXm2;CYm2;CZm2;1];%Start_point(reflected from mirror) which is respective to the camera frame
CPm=[CPm1 CPm2];
CPrg=C_T_R*[Final_position;1 1];  %Final_point(real point) which is respective to the camera frame
CXrg1=CPrg(1,1);CYrg1=CPrg(2,1);CZrg1=CPrg(3,1);
CXrg2=CPrg(1,2);CYrg2=CPrg(2,2);CZrg2=CPrg(3,2);
MXrg1=CXrg1;MYrg1=CYrg1;MZrg1=CZrg1+300;
MXrg2=CXrg2;MYrg2=CYrg2;MZrg2=CZrg2+300;
MXg1=sqrt(MXrg1^2+MYrg1^2+MZrg1^2);
MXg2=sqrt(MXrg2^2+MYrg2^2+MZrg2^2);
MCOg1=r/(2*MXg1-r);
MCOg2=r/(2*MXg2-r);
MXmg1=MCOg1*MXrg1;MYmg1=MCOg1*MYrg1;MZmg1=MCOg1*MZrg1;
MXmg2=MCOg2*MXrg2;MYmg2=MCOg2*MYrg2;MZmg2=MCOg2*MZrg2;
CXmg1=MXmg1;CYmg1=MYmg1;CZmg1=MZmg1-300; 
CXmg2=MXmg2;CYmg2=MYmg2;CZmg2=MZmg2-300; 
CPmg1=[CXmg1;CYmg1;CZmg1;1];%Start_point(reflected from mirror) which is respective to the camera frame
CPmg2=[CXmg2;CYmg2;CZmg2;1];%Start_point(reflected from mirror) which is respective to the camera frame
CPmg=[CPmg1 CPmg2];
if max([CPr(3,1) CPr(3,2) CPm(3,1) CPm(3,2) CPrg(3,1) CPrg(3,2) CPmg(3,1) CPmg(3,2)])<0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Calculation of the image information%%%
alpha_u=-1000; % Focal lens of X-axis
alpha_v=-1000;% Focal lens of Y-axis
Uc=0;Vc=0; %The center of image(pixel)

Um1=round(Uc+(CPm(1,1)*alpha_u)/CPm(3,1));
Vm1=round(Vc+(CPm(2,1)*alpha_v)/CPm(3,1));
Ur1=round(Uc+(CPr(1,1)*alpha_u)/CPr(3,1));          %for the Start_point
Vr1=round(Vc+(CPr(2,1)*alpha_v)/CPr(3,1));
Um_s1=Um1;
Vm_s1=Vm1;
Ur_s1=Ur1;
Vr_s1=Vr1;
Um2=round(Uc+(CPm(1,2)*alpha_u)/CPm(3,2));
Vm2=round(Vc+(CPm(2,2)*alpha_v)/CPm(3,2));
Ur2=round(Uc+(CPr(1,2)*alpha_u)/CPr(3,2));          %for the Start_point
Vr2=round(Vc+(CPr(2,2)*alpha_v)/CPr(3,2));
Um_s2=Um2;
Vm_s2=Vm2;
Ur_s2=Ur2;
Vr_s2=Vr2;

Ugm1=round(Uc+(CPmg(1,1)*alpha_u)/CPmg(3,1));
Vgm1=round(Vc+(CPmg(2,1)*alpha_v)/CPmg(3,1));
Ugr1=round(Uc+(CPrg(1,1)*alpha_u)/CPrg(3,1));       %for the Final_point
Vgr1=round(Vc+(CPrg(2,1)*alpha_v)/CPrg(3,1));
Ugm2=round(Uc+(CPmg(1,2)*alpha_u)/CPmg(3,2));
Vgm2=round(Vc+(CPmg(2,2)*alpha_v)/CPmg(3,2));
Ugr2=round(Uc+(CPrg(1,2)*alpha_u)/CPrg(3,2));       %for the Final_point
Vgr2=round(Vc+(CPrg(2,2)*alpha_v)/CPrg(3,2));
MX1=sqrt(((Ur1^2)*(CZr1^2))/(alpha_u^2)+((Vr1^2)*(CZr1^2))/(alpha_v^2)+(CZr1+300)^2); 
MX2=sqrt(((Ur2^2)*(CZr2^2))/(alpha_u^2)+((Vr2^2)*(CZr2^2))/(alpha_v^2)+(CZr2+300)^2);   
Zr1=CPrg(3,1);Zr2=CPrg(3,2); %
Zm1=CPmg(3,1);Zm2=CPmg(3,2);% The depth of the point which is respective to the camera frame
%%%%%%%%%%%%%%%%%%% Estimation of Image Jacobian %%%%%%%%%%%%



%%%%%%%%%%%%%%%%%%%%%%% Image feature for the Start_point%%%%
l1=sqrt((Um1-Ur1)^2+(Vm1-Vr1)^2); % Calculation of L (the length between Cpm and Cpr)
d1=sqrt(((Um1+Ur1)/2-(Ugm1+Ugr1)/2)^2+((Vm1+Vr1)/2-(Vgm1+Vgr1)/2)^2); % Calculation of d (the distance of the middle point between the Start_point and the Final_point)
l2=sqrt((Um2-Ur2)^2+(Vm2-Vr2)^2); % Calculation of L (the length between Cpm and Cpr)
d2=sqrt(((Um2+Ur2)/2-(Ugm2+Ugr2)/2)^2+((Vm2+Vr2)/2-(Vgm2+Vgr2)/2)^2); % Calculation of d (the distance of the middle point between the Start_point and the Final_point)
theta=atan((Vm1-Vr1)/(Um1-Ur1)); % Calculation of the angle of the connection line
Start_point=[l1;d1;theta;l2;d2]; %Image feature at the Start_point
%%%%%%%%%%%%%%%%%%%%%%%Image feature for the Final_point%%%
 lg1=sqrt((Ugm1-Ugr1)^2+(Vgm1-Vgr1)^2); % Calculation of L1 (the length between Cpmg and Cprg)
 lg2=sqrt((Ugm2-Ugr2)^2+(Vgm2-Vgr2)^2); % Calculation of L2 (the length between Cpmg and Cprg)
 theta_g=atan((Vgm1-Vgr1)/(Ugm1-Ugr1));% Calculation of d (the distance of the middle point between the Start_point and the Final_point)
 
%d1 d2 at disired position equals to zero
Final_point=[lg1;1;theta_g;lg2;1]; %Image feature at the Goal_point
Different_l1=abs(lg1-l1);
Different_d1=abs(1-d1);
Different_l2=abs(lg2-l2);
Different_d2=abs(1-d2);
Different_theta=abs(theta_g-theta);
Different=[Different_l1;Different_d1;Different_theta;Different_l2;Different_d2];

D1=[(Um1-Ur1)/l1 (Vm1-Vr1)/l1 -(Um1-Ur1)/l1 -(Vm1-Vr1)/l1;
    ((1/d1)*((Um1+Ur1)/2-(Ugr1+Ugm1)/2)) (1/d1)*(((Vm1+Vr1)/2-(Vgr1+Vgm1)/2)) (1/d1)*(((Um1+Ur1)/2-(Ugr1+Ugm1)/2)) (1/d1)*(((Vm1+Vr1)/2-(Vgr1+Vgm1)/2)); 
   -(Vm1-Vr1)/l1^2 (Um1-Ur1)/l1^2 (Vm1-Vr1)/l1^2 -(Um1-Ur1)/l1^2];
D2=[(Um2-Ur2)/l2 (Vm2-Vr2)/l2 -(Um2-Ur2)/l2 -(Vm2-Vr2)/l2;
    ((1/d2)*((Um2+Ur2)/2-(Ugr2+Ugm2)/2)) (1/d2)*(((Vm2+Vr2)/2-(Vgr2+Vgm2)/2)) (1/d2)*(((Um2+Ur2)/2-(Ugr2+Ugm2)/2)) (1/d2)*(((Vm2+Vr2)/2-(Vgr2+Vgm2)/2))];
D=[D1 zeros(3,4);zeros(2,4) D2];
F1=r/2.*[1/MX1-(Ur1^2)*(CZr1^2)/((alpha_u^2)*(MX1^3))    -Ur1*Vr1*(CZr1^2)/(alpha_u*alpha_v*MX1^3)       -(CZr1+300)*Ur1*CZr1/(alpha_u*MX1^3);
          -Ur1*Vr1*(CZr1^2)/(alpha_u*alpha_v*MX1^3)        1/MX1-(Vr1^2)*(CZr1^2)/((alpha_v^2)*(MX1^3))   -(CZr1+300)*Vr1*CZr1/(alpha_v*MX1^3);
          -(CZr1+300)*Ur1*CZr1/(alpha_u*MX1^3)             -(CZr1+300)*Vr1*CZr1/(alpha_v*MX1^3)            1/MX1-(CZr1+300)^2/MX1^3;];
 Im1=[  alpha_u/Zm1        0      -(Um1-Uc)/Zm1 ;
         0            alpha_v/Zm1  -(Vm1-Vc)/Zm1];
     Im1=Im1*F1;
Ir1=[  alpha_u/Zr1        0      -(Ur1-Uc)/Zr1 ;
         0            alpha_v/Zr1  -(Vr1-Vc)/Zr1 ];
I1=[Im1;Ir1];
F2=r/2.*[1/MX2-(Ur2^2)*(CZr2^2)/((alpha_u^2)*(MX2^3))    -Ur2*Vr2*(CZr2^2)/(alpha_u*alpha_v*MX2^3)       -(CZr2+300)*Ur2*CZr2/(alpha_u*MX2^3);
          -Ur2*Vr2*(CZr2^2)/(alpha_u*alpha_v*MX2^3)        1/MX2-(Vr2^2)*(CZr2^2)/((alpha_v^2)*(MX2^3))   -(CZr2+300)*Vr2*CZr2/(alpha_v*MX2^3);
          -(CZr2+300)*Ur2*CZr2/(alpha_u*MX2^3)             -(CZr2+300)*Vr2*CZr2/(alpha_v*MX2^3)            1/MX2-(CZr2+300)^2/MX2^3;];
 Im2=[  alpha_u/Zm2        0      -(Um2-Uc)/Zm2 ;
         0            alpha_v/Zm2  -(Vm2-Vc)/Zm2];
     Im2=Im2*F1;
Ir2=[  alpha_u/Zr2        0      -(Ur2-Uc)/Zr2 ;
         0            alpha_v/Zr2  -(Vr2-Vc)/Zr2 ];
I2=[Im2;Ir2];
I=[I1 zeros(4,3);zeros(4,3) I2];
if abs(thetai)==0 thetai=thetai+0.01; end
if abs(phii)==0 phii=phii+0.01; end
    Q=[1 0 0 L/2*cos(thetai)*sin(phii) -L/2*cos(thetai)*cos(phii);
       0 1 0 -L/2*cos(thetai)*cos(phii) -L/2*cos(thetai)*sin(phii);
       0 0 1 0                          L/2*sin(thetai);
       1 0 0 -L/2*cos(thetai)*sin(phii) L/2*cos(thetai)*cos(phii);
       0 1 0 L/2*cos(thetai)*cos(phii)  L/2*cos(thetai)*sin(phii);
       0 0 1 0                          -L/2*sin(thetai);];
R=rotz(pi/2)*roty(pi/2); %Robot makes the angle to the camera frame
H1=[R(1,1) R(1,2) R(1,3);
   R(2,1) R(2,2) R(2,3);  %Rotation metrix of the Robot respect to the camera frame(Need only 3 by 3)
   R(3,1) R(3,2) R(3,3)];
H=[H1 zeros(3,3);zeros(3,3) H1];
J=D*I*H*Q;%Initial Image Jacobian 


%%%%%%%%%%%%%%%%%%%Creating some variables to keep the result %%%%%%%%%


Position=[];
P1=[];
P2=[];
Point=[];
Jacobian=[];
Gain=[];
Velocity=[];
Ur1_point=[];
Vr1_point=[];
Um1_point=[];
Vm1_point=[];
Ur2_point=[];
Vr2_point=[];
Um2_point=[];
Vm2_point=[];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ur1_point=[Ur1_point;Ur1];
Vr1_point=[Vr1_point;Vr1];
Um1_point=[Um1_point;Um1];
Vm1_point=[Vm1_point;Vm1];
Ur2_point=[Ur2_point;Ur2];
Vr2_point=[Vr2_point;Vr2];
Um2_point=[Um2_point;Um2];
Vm2_point=[Vm2_point;Vm2];
%%%%%%%%%%%%%%%%%%%%%%%%set the time interval%%%%%%%%%%%%%
dt=0.01;
Final_time=100;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%Kalman filter initial parameters%%%%%
measnoise =[1 0 0 0 0;0 1 0 0 0;0 0 1 0 0;0 0 0 0.1 0;0 0 0 0 0.1;]; % position measurement noise 
accelnoise =1; % acceleration noise 

QQ = accelnoise^2 * eye(25); % process noise covariance
PP = QQ; % initial estimation covariance
RR = measnoise^2*eye(5); % measurement error covariance
Inn = zeros(size(RR));% set up the size of the innovations vector

aa = eye(25); % transition matrix


xx = [J(1,1) J(2,1) J(3,1) J(4,1) J(5,1) J(1,2) J(2,2) J(3,2) J(4,2) J(5,2) J(1,3) J(2,3) J(3,3) J(4,3) J(5,3) J(1,4) J(2,4) J(3,4) J(4,4) J(5,4) J(1,5) J(2,5) J(3,5) J(4,5) J(5,5)]'; % initial Image Jacobian vector
xhatx=xx;
Jnew=J;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Point_now=Start_point;
Position_now=Initial;
n=0;qg=qi;
J_p=[qg'];
%%%%%%%%%%%%Starting to do the motion%%%%%%%%%%%
for i=0:dt:Final_time
    l_old1=l1;
    d_old1=d1;  
    l_old2=l2;
    d_old2=d2;
    theta_old=theta;
    velocity=pinv(J)*(Point_now-Final_point);
    if velocity == [0 ;0 ;0;0;0];  %Control gain
        Control_Gain=0;
    else
     Control_Gain1=min([20/(abs(velocity(1))+0.001) 20/(abs(velocity(2))+0.001) 20/(abs(velocity(3))+0.001) 0.2])*eye(3);  
     Control_Gain2=min([20/(abs(velocity(4))+0.001) 20/(abs(velocity(5))+0.001) 0.1])*eye(2);
     Control_Gain=[Control_Gain1 zeros(3,2);
                   zeros(2,3)   Control_Gain2]; 
               
 end 
    T=-Control_Gain*pinv(J)*(Point_now-Final_point);
    X=T(1)*dt;
    Y=T(2)*dt;
    Z=T(3)*dt;
    Phi=T(4)*dt;
    Theta=T(5)*dt;
    Position_now=Position_now+(T*dt);   %Robot moving
    Position=[Position Position_now];
    Velocity=[Velocity T];
%%%%%%%%%%%%%%%%%%%%%%%%%Updat the Image Information
    Xnow=Position_now(1);
    Ynow=Position_now(2);
    Znow=Position_now(3);
    phinow=Position_now(4);
    thetanow=Position_now(5);
    thetai=thetanow;
    phii=phinow;
    Position1=[Xnow-L/2*sin(thetanow)*cos(phinow) Ynow-L/2*sin(thetanow)*sin(phinow) Znow-L/2*cos(thetanow);
               Xnow+L/2*sin(thetanow)*cos(phinow) Ynow+L/2*sin(thetanow)*sin(phinow) Znow+L/2*cos(thetanow)]';
    P1=[P1 Position1(:,1)];
    P2=[P2 Position1(:,2)];
   % if n==10
   %   n=0;
   % xi1=Position1(1,1);yi1=Position1(2,1);zi1=Position1(3,1);
   % xi2=Position1(1,2);yi2=Position1(2,2);zi2=Position1(3,2);
   % angle1=asin((zi2-zi1)/L);
   % angle2=acos((xi2-xi1)/(L*cos(angle1)));
   % tri=transl(xi2,yi2,zi2)*rotz(angle2)*roty(-angle1);
   % qi=ikine(xrhino,tri,qg,[1 1 1 1 1 0]);
   % qg=qi;
   % J_p=[J_p qg'];
   % end
   % n=n+1
    CPr= C_T_R*[Position1;1 1];
    CXr1=CPr(1,1);CYr1=CPr(2,1);CZr1=CPr(3,1);
    CXr2=CPr(1,2);CYr2=CPr(2,2);CZr2=CPr(3,2);
    MXr1=CXr1;MYr1=CYr1;MZr1=CZr1+300;
    MXr2=CXr2;MYr2=CYr2;MZr2=CZr2+300;
    MX1=sqrt(MXr1^2+MYr1^2+MZr1^2);
    MX2=sqrt(MXr2^2+MYr2^2+MZr2^2);
    MCO1=r/(2*MX1-r);
    MCO2=r/(2*MX2-r);
    MXm1=MCO1*MXr1;MYm1=MCO1*MYr1;MZm1=MCO1*MZr1;
    MXm2=MCO2*MXr2;MYm2=MCO2*MYr2;MZm2=MCO2*MZr2;
    CXm1=MXm1;CYm1=MYm1;CZm1=MZm1-300; 
    CXm2=MXm2;CYm2=MYm2;CZm2=MZm2-300; 
    CPm1=[CXm1;CYm1;CZm1;1];%Start_point(reflected from mirror) which is respective to the camera frame
    CPm2=[CXm2;CYm2;CZm2;1];%Start_point(reflected from mirror) which is respective to the camera frame
    CPm=[CPm1 CPm2];
    Um1=round(Uc+(CPm(1,1)*alpha_u)/CPm(3,1));
    Vm1=round(Vc+(CPm(2,1)*alpha_v)/CPm(3,1));
    Ur1=round(Uc+(CPr(1,1)*alpha_u)/CPr(3,1));          
    Vr1=round(Vc+(CPr(2,1)*alpha_v)/CPr(3,1));      %Update Image information after robot moved
    Um2=round(Uc+(CPm(1,2)*alpha_u)/CPm(3,2));
    Vm2=round(Vc+(CPm(2,2)*alpha_v)/CPm(3,2));
    Ur2=round(Uc+(CPr(1,2)*alpha_u)/CPr(3,2));         
    Vr2=round(Vc+(CPr(2,2)*alpha_v)/CPr(3,2));
    l1=sqrt((Um1-Ur1)^2+(Vm1-Vr1)^2); 
    d1=sqrt(((Um1+Ur1)/2-(Ugm1+Ugr1)/2)^2+((Vm1+Vr1)/2-(Vgm1+Vgr1)/2)^2);
    l2=sqrt((Um2-Ur2)^2+(Vm2-Vr2)^2); 
    d2=sqrt(((Um2+Ur2)/2-(Ugm2+Ugr2)/2)^2+((Vm2+Vr2)/2-(Vgm2+Vgr2)/2)^2);
     %%%%%store the old Metrix%%%%%
    Dk=D;
    Ik=I;
    Qk=Q;
   
    %%%%%%%%%%%%%%%The conditions to avoid metrix singularity.....
    if l1 < 0.5
        l1=0.5;
    end
    if l2 < 0.5
        l2=0.5;
    end
   %%% 
    if   abs(Um1-Ur1)<0.5 & abs(Vm1-Vr1)<0.5 
        theta=theta_old;
        D1=[ 0.5/l1 0.5/l1 -0.5/l1 -0.5/l1;
           ((Um1+Ur1)/2-(Ugr1+Ugm1)/2)/d1 ((Vm1+Vr1)/2-(Vgr1+Vgm1)/2)/d1 ((Um1+Ur1)/2-(Ugr1+Ugm1)/2)/d1 ((Vm1+Vr1)/2-(Vgr1+Vgm1)/2)/d1;
            -0.5/(l1^2) 0.5/(l1^2) 0.5/(l1^2) -0.5/(l1^2) ];
    elseif abs(Um1-Ur1) < 0.5
        theta=pi/2;
        D1=[0.5/l1 (Vm1-Vr1)/l1 -0.5/l1 -(Vm1-Vr1)/l1;
          ((Um1+Ur1)/2-(Ugr1+Ugm1)/2)/d1 ((Vm1+Vr1)/2-(Vgr1+Vgm1)/2)/d1 ((Um1+Ur1)/2-(Ugr1+Ugm1)/2)/d1 ((Vm1+Vr1)/2-(Vgr1+Vgm1)/2)/d1;
           -(Vm1-Vr1)/(l1^2) 0.5/(l1^2) (Vm1-Vr1)/(l1^2) -0.5/(l1^2)];
   else
      theta=atan((Vm1-Vr1)/(Um1-Ur1)); 
     D1=[(Um1-Ur1)/l1 (Vm1-Vr1)/l1 -(Um1-Ur1)/l1 -(Vm1-Vr1)/l1;
        ((Um1+Ur1)/2-(Ugr1+Ugm1)/2)/d1 ((Vm1+Vr1)/2-(Vgr1+Vgm1)/2)/d1 ((Um1+Ur1)/2-(Ugr1+Ugm1)/2)/d1 ((Vm1+Vr1)/2-(Vgr1+Vgm1)/2)/d1;
        -(Vm1-Vr1)/(l1^2) (Um1-Ur1)/(l1^2) (Vm1-Vr1)/(l1^2) -(Um1-Ur1)/(l1^2)];
  end
    if   abs(Um2-Ur2)<0.5 & abs(Vm2-Vr2)<0.5 
           D2=[ 0.5/l2 0.5/l2 -0.5/l2 -0.5/l2;
           ((Um2+Ur2)/2-(Ugr2+Ugm2)/2)/d2 ((Vm2+Vr2)/2-(Vgr2+Vgm2)/2)/d2 ((Um2+Ur2)/2-(Ugr2+Ugm2)/2)/d2 ((Vm2+Vr2)/2-(Vgr2+Vgm2)/2)/d2];
       %elseif abs(Um2-Ur2) < 0.5
       %    D2=[0.5/l2 (Vm2-Vr2)/l2 -0.5/l2 -(Vm2-Vr2)/l2;
         %     ((Um2+Ur2)/2-(Ugr2+Ugm2)/2)/d2 ((Vm2+Vr2)/2-(Vgr2+Vgm2)/2)/d2 ((Um2+Ur2)/2-(Ugr2+Ugm2)/2)/d2 ((Vm2+Vr2)/2-(Vgr2+Vgm2)/2)/d2];
      else
           D2=[(Um2-Ur2)/l2 (Vm2-Vr2)/l2 -(Um2-Ur2)/l2 -(Vm2-Vr2)/l2;
    ((1/d2)*((Um2+Ur2)/2-(Ugr2+Ugm2)/2)) (1/d2)*(((Vm2+Vr2)/2-(Vgr2+Vgm2)/2)) (1/d2)*(((Um2+Ur2)/2-(Ugr2+Ugm2)/2)) (1/d2)*(((Vm2+Vr2)/2-(Vgr2+Vgm2)/2))];

  end
  diff=[l1-l_old1;d1-d_old1;theta-theta_old;l2-l_old2;d2-d_old2]; 
  %%%                             % 
    Point_now=[l1;d1;theta;l2;d2];    
    Point=[Point Point_now];
    
%%%%%%%%%%%%%%%%%%%%Updat the new Image Jacobian %%%    
    
   F1=r/2.*[1/MX1-(Ur1^2)*(CZr1^2)/((alpha_u^2)*(MX1^3))    -Ur1*Vr1*(CZr1^2)/(alpha_u*alpha_v*MX1^3)       -(CZr1+300)*Ur1*CZr1/(alpha_u*MX1^3);
          -Ur1*Vr1*(CZr1^2)/(alpha_u*alpha_v*MX1^3)        1/MX1-(Vr1^2)*(CZr1^2)/((alpha_v^2)*(MX1^3))   -(CZr1+300)*Vr1*CZr1/(alpha_v*MX1^3);
          -(CZr1+300)*Ur1*CZr1/(alpha_u*MX1^3)             -(CZr1+300)*Vr1*CZr1/(alpha_v*MX1^3)            1/MX1-(CZr1+300)^2/MX1^3;];
 Im1=[  alpha_u/Zm1        0      -(Um1-Uc)/Zm1 ;