% calculate uncertain relative motion M12 = inv(M1) * M2 exponential representation Msp rotation pair, Msp.Msp=[M1,M2]=[4,8], Msp.Csp = [12x12] Msd relative rotation, Msd.Ms=[4x4], Msd.Cs=[6x6] J [6x12] Jacobian wf 6/2020
0001 function [Msd,J] = calc_relative_M_s(Msp) 0002 %% calculate uncertain relative motion M12 = inv(M1) * M2 0003 % exponential representation 0004 % 0005 % Msp rotation pair, Msp.Msp=[M1,M2]=[4,8], Msp.Csp = [12x12] 0006 % 0007 % Msd relative rotation, Msd.Ms=[4x4], Msd.Cs=[6x6] 0008 % J [6x12] Jacobian 0009 % 0010 % wf 6/2020 0011 0012 % M1, M 0013 Ms1 = Msp.Msp(:,1:4); 0014 Msd.Ms = inv(Ms1) * Msp.Msp(:,5:8); 0015 0016 % inverse adjunct motion matrix 0017 R1 = Ms1(1:3,1:3); 0018 Z1 = Ms1(1:3,4); 0019 M_1_ad_inv = [R1' , zeros(3); ... 0020 R1'*calc_S(Z1)', R1']; 0021 0022 % Jacobian and CovM 0023 J = [-M_1_ad_inv , M_1_ad_inv]; 0024 Msd.Cs = J * Msp.Csp * J'; 0025 0026 return