% calculate uncertain concatenated motion M = M2 * M1 Msp motion pair, Msp.Msp=[Ms1,Ms2] = [4x8], Msp.Csp = [12x12] Ms [4x4] concatenated motion, Ms.Ms , Ms.Cs J [4x8] Jacobian wf 6/2020
0001 function [Ms,J] = calc_concatenated_M_s(Msp); 0002 %% calculate uncertain concatenated motion M = M2 * M1 0003 % 0004 % Msp motion pair, Msp.Msp=[Ms1,Ms2] = [4x8], Msp.Csp = [12x12] 0005 % 0006 % Ms [4x4] concatenated motion, Ms.Ms , Ms.Cs 0007 % J [4x8] Jacobian 0008 % 0009 % wf 6/2020 0010 0011 % M2 and M 0012 Ms2 = Msp.Msp(:,5:8); 0013 Ms.Ms = Ms2 * Msp.Msp(:,1:4); 0014 0015 % adjoint motion matrix for M2 0016 R2 = Ms2(1:3,1:3); 0017 Z2 = Ms2(1:3,4); 0018 M_2_ad = [R2 zeros(3); calc_S(Z2)*R2 R2]; 0019 0020 % Jacobian and CovM 0021 J = [M_2_ad eye(6)]; 0022 Ms.Cs = J * Msp.Csp * J'; 0023 0024 return