% calculate uncertain concatenated motion M = M2 * M1 Mzp motion pair, Mzp.Mzp=[Mz1,Mz2] = [4x8], Mzp.Czp = [12x12] Mz [4x4] concatenated motion, Mz.Mz , Mz.Cz J [4x8] Jacobian wf 5/2020
0001 function [Mz,J] = calc_concatenated_M_z(Mzp); 0002 %% calculate uncertain concatenated motion M = M2 * M1 0003 % 0004 % Mzp motion pair, Mzp.Mzp=[Mz1,Mz2] = [4x8], Mzp.Czp = [12x12] 0005 % 0006 % Mz [4x4] concatenated motion, Mz.Mz , Mz.Cz 0007 % J [4x8] Jacobian 0008 % 0009 % wf 5/2020 0010 0011 % M1, M2, M 0012 Mz1 = Mzp.Mzp(:,1:4); 0013 Mz2 = Mzp.Mzp(:,5:8); 0014 Mz.Mz = Mz2 * Mz1; 0015 0016 % Jacobian and CovM 0017 R2 = Mz2(1:3,1:3); 0018 Z1 = Mz1(1:3,4); 0019 J = [R2 zeros(3) eye(3) zeros(3);... 0020 zeros(3) R2 -calc_S(R2*Z1) eye(3) ]; 0021 0022 Mz.Cz = J * Mzp.Czp * J'; 0023 0024 return