Home > .. > Matlab_Uncertain_Frames_sz > calc_inverse_M_z.m

calc_inverse_M_z

PURPOSE ^

% calculate uncertain inverse motion and Jacobian

SYNOPSIS ^

function [Mi,J] = calc_inverse_M_z(Mz)

DESCRIPTION ^

%  calculate uncertain inverse motion and Jacobian
   partially exponential reprensentation

 Mz     motion, Mz.Mz = [4x4], Mz.Cz = [6x6]

 Mi     inverse motion, Mi.Mz = [4x4], Mi.Cz = [6x6]
 J      [6x6] Jacobian
 
 wf 5/2020

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 function [Mi,J] = calc_inverse_M_z(Mz)
0002 %%  calculate uncertain inverse motion and Jacobian
0003 %   partially exponential reprensentation
0004 %
0005 % Mz     motion, Mz.Mz = [4x4], Mz.Cz = [6x6]
0006 %
0007 % Mi     inverse motion, Mi.Mz = [4x4], Mi.Cz = [6x6]
0008 % J      [6x6] Jacobian
0009 %
0010 % wf 5/2020
0011 
0012 % Motion
0013 M0    = Mz.Mz;
0014 % inverse motion
0015 Mi.Mz = inv(M0);
0016 % Jacobian (neg. inverse adjunct motion matrix)
0017 R0    = M0(1:3,1:3);
0018 J     = -[R0' zeros(3); R0'*calc_S(M0(1:3,4)) R0'];
0019 % CovM
0020 Mi.Cz = J * Mz.Cz * J';
0021 
0022 end 
0023

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