Home > .. > Matlab_Uncertain_Frames_sz > calc_concatenated_M_z.m

calc_concatenated_M_z

PURPOSE ^

% calculate uncertain concatenated motion M = M2 * M1

SYNOPSIS ^

function [Mz,J] = calc_concatenated_M_z(Mzp);

DESCRIPTION ^

% 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

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

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