Home > .. > Matlab_Uncertain_Frames_sz > calc_relative_M_z.m

calc_relative_M_z

PURPOSE ^

% calculate uncertain relative motion M12 = inv(M1) * M2

SYNOPSIS ^

function [Mzd,J] = calc_relative_M_z(Mzp)

DESCRIPTION ^

% 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

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Wed 10-Jun-2020 19:54:31 by m2html © 2005