Home > .. > Matlab_Uncertain_Frames_sz > calc_relative_M_s.m

calc_relative_M_s

PURPOSE ^

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

SYNOPSIS ^

function [Msd,J] = calc_relative_M_s(Msp)

DESCRIPTION ^

% 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

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

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