% calculate uncertain inverse motion and Jacobian partially exponential reprensentation Mz motion, Mz.Mz = [4x4], Mz.Cz = [6x6] Mi inverse motion, Mi.Mz = [4x4], Mi.Cz = [6x6] J [6x6] Jacobian wf 5/2020
0001 function [Mi,J] = calc_inverse_M_z(Mz) 0002 %% calculate uncertain inverse motion and Jacobian 0003 % partially exponential reprensentation 0004 % 0005 % Mz motion, Mz.Mz = [4x4], Mz.Cz = [6x6] 0006 % 0007 % Mi inverse motion, Mi.Mz = [4x4], Mi.Cz = [6x6] 0008 % J [6x6] Jacobian 0009 % 0010 % wf 5/2020 0011 0012 % Motion 0013 M0 = Mz.Mz; 0014 % inverse motion 0015 Mi.Mz = 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.Cz = J * Mz.Cz * J'; 0021 0022 end 0023