% calculate uncertain inverse motion and Jacobian exponential representation Ms motion, Ms.Ms = [4x4], Ms.Cs = [6x6] Mi inverse motion, Mi.Ms = [4x4], Mi.Cs = [6x6] J [6x6] Jacobian wf 6/2020
0001 function [Mi,J] = calc_inverse_M_s(Ms) 0002 %% calculate uncertain inverse motion and Jacobian 0003 % exponential representation 0004 % 0005 % Ms motion, Ms.Ms = [4x4], Ms.Cs = [6x6] 0006 % 0007 % Mi inverse motion, Mi.Ms = [4x4], Mi.Cs = [6x6] 0008 % J [6x6] Jacobian 0009 % 0010 % wf 6/2020 0011 0012 % Motion 0013 M0 = Ms.Ms; 0014 % inverse motion 0015 Mi.Ms = inv(M0); 0016 % Jacobian (neg. inverse adjunct motion matrix) 0017 R0 = M0(1:3,1:3); 0018 J = -[R0' zeros(3); R0'*calc_S(M0(1:3,4))' R0']; 0019 % CovM 0020 Mi.Cs = J*Ms.Cs*J'; 0021 0022 end 0023