Home > .. > Matlab_Uncertain_Frames_sz > calc_inverse_M_s.m

calc_inverse_M_s

PURPOSE ^

% calculate uncertain inverse motion and Jacobian

SYNOPSIS ^

function [Mi,J] = calc_inverse_M_s(Ms)

DESCRIPTION ^

%  calculate uncertain inverse motion and Jacobian
   exponential representation

 Ms     motion, Ms.Ms = [4x4], Ms.Cs = [6x6]

 Mi     inverse motion, Mi.Ms = [4x4], Mi.Cs = [6x6]
 J      [6x6] Jacobian
 
 wf 6/2020

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 function [Mi,J] = calc_inverse_M_s(Ms)
0002 %%  calculate uncertain inverse motion and Jacobian
0003 %   exponential representation
0004 %
0005 % Ms     motion, Ms.Ms = [4x4], Ms.Cs = [6x6]
0006 %
0007 % Mi     inverse motion, Mi.Ms = [4x4], Mi.Cs = [6x6]
0008 % J      [6x6] Jacobian
0009 %
0010 % wf 6/2020
0011 
0012 % Motion
0013 M0    = Ms.Ms;
0014 % inverse motion
0015 Mi.Ms = 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.Cs = J*Ms.Cs*J';
0021 
0022 end
0023

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