📄 iteration_image_jacobian_robot_kalman.m
字号:
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);];
D=[D1 zeros(3,4);zeros(2,4) D2];
Jacobian=[Jacobian xx];
%%%%%%%%%%%%%Kalman Filter%%%%%%%%%%%%%%%%%%%%
cc = [X 0 0 0 0 Y 0 0 0 0 Z 0 0 0 0 Phi 0 0 0 0 Theta 0 0 0 0;
0 X 0 0 0 0 Y 0 0 0 0 Z 0 0 0 0 Phi 0 0 0 0 Theta 0 0 0;
0 0 X 0 0 0 0 Y 0 0 0 0 Z 0 0 0 0 Phi 0 0 0 0 Theta 0 0;
0 0 0 X 0 0 0 0 Y 0 0 0 0 Z 0 0 0 0 Phi 0 0 0 0 Theta 0;
0 0 0 0 X 0 0 0 0 Y 0 0 0 0 Z 0 0 0 0 Phi 0 0 0 0 Theta;]; % measurement matrix
% Simulate the process
ProcessNoise = sqrt(0.0001)* randn(25,1);
xhatx = aa * xx +ProcessNoise ; % State estimate plus noise
% Jacobian_noise=[Jacobian_noise x]; %Keeping the noise Image Jacobian
% Simulate the measurement
MeasNoise = sqrt(.0001)*randn(5,1);
zz = diff+MeasNoise; %Measurement plus noise
% Innovation
Inn = zz - cc * xhatx;
% Covariance of Innovation
ss = cc * PP * cc' + RR;
% Gain matrix
KK = aa * PP * cc' * inv(ss);
% State estimate
xhatx = aa * xhatx + KK* Inn;
% Covariance of prediction error
PP = aa * PP * aa' + QQ - aa * PP * cc' * inv(ss) * cc * PP * aa';
Jnew=[xhatx(1,1) xhatx(6,1) xhatx(11,1) xhatx(16,1) xhatx(21,1)
xhatx(2,1) xhatx(7,1) xhatx(12,1) xhatx(17,1) xhatx(22,1) %Changing the new estimate to the metrix form
xhatx(3,1) xhatx(8,1) xhatx(13,1) xhatx(18,1) xhatx(23,1)
xhatx(4,1) xhatx(9,1) xhatx(14,1) xhatx(19,1) xhatx(24,1)
xhatx(5,1) xhatx(10,1) xhatx(15,1) xhatx(20,1) xhatx(25,1)];
% Jacobian_estimate=[Jacobian_estimate xhat]; %Keeping the estimation of Image Jacobian
xx = xhatx;
J=Jnew;
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];
%if abs(Ur1-Ugr1)<=1 && abs(Ur2-Ugr2)<=1 && abs(Um1-Ugm1)<=1 && abs(Um1-Ugm1)<=1 && abs(Um2-Ugm2)<=1 && abs(Vr1-Vgr1)<=1 && abs(Vr2-Vgr2)<=1 && abs(Vm1-Vgm1)<=1 && abs(Vm1-Vgm1)<=1 && abs(Vm2-Vgm2)<=1
% Final_time=i;
% break
%end
end
%%%%%%%%%%%%%%%%%%Ploting%%%%%%%%%%%%%
Final1=[];
Final2=[];
for t=0:dt:Final_time
Final1=[Final1 Final_position(:,1)];
Final2=[Final2 Final_position(:,2)];
end
Final_feature=[];
for t=0:dt:Final_time
Final_feature=[Final_feature Final_point];
end
t=0:dt:Final_time;
figure(1);
plot(t,P1(1,:),'-',t,P1(2,:),':',t,P1(3,:),'-.');
title('Robot Landmark1 Trajectory');
xlabel('Time(Sec.)');
ylabel('Position(cm)');
legend('x','y','z',0);
figure(2);
plot(t,P2(1,:),'-',t,P2(2,:),':',t,P2(3,:),'-.');
title('Robot Landmark2 Trajectory');
xlabel('Time(Sec.)');
ylabel('Position(cm)');
legend('x','y','z',0);
figure(3);
subplot(5,1,1)
plot(t,Point(1,:))
title('Image Trajectory L1 d1 Theta L2 d2')
subplot(5,1,2)
plot(t,Point(2,:))
subplot(5,1,3)
plot(t,Point(3,:))
ylabel('Position(Pixel)');
subplot(5,1,4)
plot(t,Point(4,:))
subplot(5,1,5)
plot(t,Point(5,:))
xlabel('Time(Sec.)');
%error1=Final1-P1;
%figure(4);
%plot(t,error1(1,:),'-',t,error1(2,:),':',t,error1(3,:),'-.');
%title('Landmark1 Position Error');
%xlabel('time(Sec.)');
%ylabel('Position cm.');
%legend('x','y','z',0);
%error2=Final2-P2;
%figure(5);
%plot(t,error2(1,:),'-',t,error2(2,:),':',t,error2(3,:),'-.');
%title('Landmark2 Position Error');
%xlabel('time(Sec.)');
%ylabel('Position cm.');
%legend('x','y','z',0);
error_image=Final_feature-Point;
figure(4);
subplot(5,1,1)
plot(t,error_image(1,:))
title('Pixel error L1 d1 Theta L2 d2')
subplot(5,1,2)
plot(t,error_image(2,:))
subplot(5,1,3)
plot(t,error_image(3,:))
ylabel('Pixel');
subplot(5,1,4)
plot(t,error_image(4,:))
subplot(5,1,5)
plot(t,error_image(5,:))
xlabel('Time(Sec.)');
figure(5);
subplot(5,1,1)
plot(t,Position(1,:))
title('Position Robot Velocity X Y Z Phi Theta')
subplot(5,1,2)
plot(t,Position(2,:))
subplot(5,1,3)
plot(t,Position(3,:))
ylabel('Position cm or rad');
subplot(5,1,4)
plot(t,Position(4,:))
subplot(5,1,5)
plot(t,Position(5,:))
xlabel('Time(Sec.)');
figure(6);
subplot(5,1,1)
plot(t,Velocity(1,:))
title('Robot Velocity X Y Z Phi Theta')
subplot(5,1,2)
plot(t,Velocity(2,:))
subplot(5,1,3)
plot(t,Velocity(3,:))
ylabel('Velocity cm/s');
subplot(5,1,4)
plot(t,Velocity(4,:))
subplot(5,1,5)
plot(t,Velocity(5,:))
xlabel('Time(Sec.)');
figure(7);
[b,z]=size(P1);
a=1:z;
plot3(P1(1,a),P1(2,a),P1(3,a),'b-d','MarkerFaceColor','c','MarkerSize',2);
grid on;
hold on;
plot3(P2(1,a),P2(2,a),P2(3,a),'g-d','MarkerFaceColor','c','MarkerSize',2);
plot3(Final_position(1,1),Final_position(2,1),Final_position(3,1),'-s','MarkerFaceColor','r','MarkerSize',5);
plot3(Final_position(1,2),Final_position(2,2),Final_position(3,2),'-s','MarkerFaceColor','r','MarkerSize',5);
plot3(Start_position(1,1),Start_position(2,1),Start_position(3,1),'-d','MarkerFaceColor','b','MarkerSize',5);
plot3(Start_position(1,2),Start_position(2,2),Start_position(3,2),'-d','MarkerFaceColor','b','MarkerSize',5);
title('Motion of the end-effector in 3D');
axis([min([min(P1(1,a)) min(P2(1,a)) min(Final_position(1,:)) min(Start_position(1,:))])-10 max([max(P1(1,a)) max(P2(1,a)) max(Final_position(1,:)) max(Start_position(1,:))])+10 min([min(P1(2,a)) min(P2(2,a)) min(Final_position(2,:)) min(Start_position(2,:))])-10 max([max(P1(2,a)) max(P2(2,a)) max(Final_position(2,:)) max(Start_position(2,:))])+10 min([min(P1(3,a)) min(P2(3,a)) min(Final_position(3,:)) min(Start_position(3,:))])-10 max([max(P1(3,a)) max(P2(3,a)) max(Final_position(3,:)) max(Start_position(3,:))])+10])
xlabel('X');
ylabel('Y');
zlabel('Z');
legend('Robot landmark1 Trajectory','Robot landmark2 Trajectory','Final Position','Final Position','Start Position','Start Position',0);
figure(8);
a=length(Ur1_point);
z=1:a;
plot(Um_s1,Vm_s1,'-s','MarkerFaceColor','g','MarkerSize',4);
hold on;
plot(Um_s2,Vm_s2,'-s','MarkerFaceColor','g','MarkerSize',4);
plot(Ur_s1,Vr_s1,':s','MarkerFaceColor','b','MarkerSize',4);
plot(Ur_s2,Vr_s2,':s','MarkerFaceColor','b','MarkerSize',4);
plot(Ugm1,Vgm1,'-d','MarkerFaceColor','r','MarkerSize',4);
plot(Ugm2,Vgm2,'-d','MarkerFaceColor','r','MarkerSize',4);
plot(Ugr1,Vgr1,':d','MarkerFaceColor','y','MarkerSize',4);
plot(Ugr2,Vgr2,':d','MarkerFaceColor','y','MarkerSize',4);
plot(Ur1_point(z),Vr1_point(z),'r',Um1_point(z),Vm1_point(z),'r');
plot(Ur2_point(z),Vr2_point(z),'b',Um2_point(z),Vm2_point(z),'b');
i=1;
while(i<a)
plot([Ur1_point(i) Um1_point(i)],[Vr1_point(i) Vm1_point(i)],'g');
plot([Ur2_point(i) Um2_point(i)],[Vr2_point(i) Vm2_point(i)],'k');
i=i+20;
end
plot([Ugr1 Ugm1],[Vgr1 Vgm1],'g');
plot([Ugr2 Ugm2],[Vgr2 Vgm2],'k');
legend('Start mirror point1','Start mirror point2','Start real point1','Start real point2','Desire mirror point1','Desire mirror point2','Desire real point1','Desire real point2','Real point1','Mirror point1','Real point2','Mirror point2',0);
title('Point on the screen');
xlabel('U');
ylabel('V');
figure(9)
subplot(5,1,1)
plot(t,Jacobian(1,:))
title('J11 J21 J31 J41 J51')
subplot(5,1,2)
plot(t,Jacobian(2,:))
subplot(5,1,3)
plot(t,Jacobian(3,:))
ylabel('Value');
subplot(5,1,4)
plot(t,Jacobian(4,:))
subplot(5,1,5)
plot(t,Jacobian(5,:))
xlabel('Time(Sec.)');
figure(10)
subplot(5,1,1)
plot(t,Jacobian(6,:))
title('J12 J22 J32 J42 J52')
subplot(5,1,2)
plot(t,Jacobian(7,:))
subplot(5,1,3)
plot(t,Jacobian(8,:))
ylabel('Value');
subplot(5,1,4)
plot(t,Jacobian(9,:))
subplot(5,1,5)
plot(t,Jacobian(10,:))
xlabel('Time(Sec.)');
figure(11)
subplot(5,1,1)
plot(t,Jacobian(11,:))
title('J13 J23 J33 J43 J53')
subplot(5,1,2)
plot(t,Jacobian(12,:))
subplot(5,1,3)
plot(t,Jacobian(13,:))
ylabel('Value');
subplot(5,1,4)
plot(t,Jacobian(14,:))
subplot(5,1,5)
plot(t,Jacobian(15,:))
xlabel('Time(Sec.)');
figure(12)
subplot(5,1,1)
plot(t,Jacobian(16,:))
title('J14 J24 J34 J44 J54')
subplot(5,1,2)
plot(t,Jacobian(17,:))
subplot(5,1,3)
plot(t,Jacobian(18,:))
ylabel('Value');
subplot(5,1,4)
plot(t,Jacobian(19,:))
subplot(5,1,5)
plot(t,Jacobian(20,:))
xlabel('Time(Sec.)');
figure(13)
subplot(5,1,1)
plot(t,Jacobian(21,:))
title('J51 J52 J53 J54 J55')
subplot(5,1,2)
plot(t,Jacobian(22,:))
subplot(5,1,3)
plot(t,Jacobian(23,:))
ylabel('Value');
subplot(5,1,4)
plot(t,Jacobian(24,:))
subplot(5,1,5)
plot(t,Jacobian(25,:))
xlabel('Time(Sec.)');
figure(14);
plot(t,Jacobian,'-d','MarkerFaceColor','c','MarkerSize',2);
title('Image Jacobian');
xlabel('Time(Sec.)');
ylabel('Value');
%figure(10);
%t=0:0.1:Final_time;
%subplot(5,1,1)
%plot(t, J_p(1,:),'.')
%title('Q1 Q2 Q3 Q4 Q5 Position');
%ylabel('cm');
%subplot(5,1,2),
%plot(t, J_p(2,:),'.')
%ylabel('rad');
%subplot(5,1,3)
%plot(t, J_p(3,:),'.')
%ylabel('rad');
%subplot(5,1,4)
%plot(t, J_p(4,:),'.')
%ylabel('rad');
%subplot(5,1,5)
%plot(t, J_p(5,:),'.');
%xlabel('time(Sec.)');
%ylabel('rad')
%figure(10);
%plot(t,Jacobian,'-d','MarkerFaceColor','c','MarkerSize',2);
%title('Image Jacobian');
%xlabel('Time(Sec.)');
%ylabel('Value');
%legend('J11','J12','J13','J14','J15','J21','J22','J23','J24','J25','J31','J32','J33','J34','J35','J41','J42','J43','J44','J45','J51','J52','J53','J54','J55',0);
%figure(11)
%text(0.05,0.5,'Press any KEY to see the animation!')
%pause
%close
%figure(11)
%plot(xrhino,J_p')
else
disp('In this configuration, there are some points can not be seen by the camera');
end⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -