% calculate uncertain relative motion M12 = inv(M1) * M2 partially exponential representation Mzp rotation pair, Mzp.Mzp=[M1,M2]=[4,8], Mzp.Czp = [12x12] Mzd relative rotation, Mzd.Mz=[4x4], Mzd.Cz=[6x6] J [6x12] Jacobian wf 6/2020
0001 function [Mzd,J] = calc_relative_M_z(Mzp) 0002 %% calculate uncertain relative motion M12 = inv(M1) * M2 0003 % partially exponential representation 0004 % 0005 % Mzp rotation pair, Mzp.Mzp=[M1,M2]=[4,8], Mzp.Czp = [12x12] 0006 % 0007 % Mzd relative rotation, Mzd.Mz=[4x4], Mzd.Cz=[6x6] 0008 % J [6x12] Jacobian 0009 % 0010 % wf 6/2020 0011 0012 % M1, M2, M12 0013 Mz1 = Mzp.Mzp(:,1:4); 0014 Mz2 = Mzp.Mzp(:,5:8); 0015 Mzd.Mz = inv(Mz1) * Mz2; 0016 0017 % Jacobian 0018 R1 = Mz1(1:3,1:3); 0019 Z1 = Mz1(1:3,4); 0020 Z2 = Mz2(1:3,4); 0021 J = [-R1' zeros(3) R1' zeros(3);... 0022 R1'*calc_S(Z2-Z1) -R1' zeros(3) R1' ]; 0023 % CovM 0024 Mzd.Cz = J * Mzp.Czp * J'; 0025 0026 return