Home > .. > Matlab_Uncertain_Frames_sz > generate_test_data_uncertain_frames.m

generate_test_data_uncertain_frames

PURPOSE ^

% generate test data for checking uncertain frames

SYNOPSIS ^

function [R0p,M0p,Cp,R,Ms,Mz,Rp,Msp,Mzp,Rs,Mss,Mzs]=generate_test_data_uncertain_frames(sigma_g,sigma_n,N_samples)

DESCRIPTION ^

%  generate test data for checking uncertain frames

 sigma_g       stdv. for generating frames
 sigma_n       stdv. for generaing noise
 N_samples     number of samples for checking CovMs

 R0p   [3x6] rotation pair [R1,R2]
 M0p   [4x8] motion pair [M1,M2]
 Cp    [12x12] CovM, also used partially 
 R     uncertain rotation, R.R=[3x3], R.C=[3x3]
 Ms    uncertain motion_s, Ms.Ms=[4x4], Ms.Cs=[6x6]
 Mz    uncertain motion_zeta, Mz.Mz=[4x4], Mz.Cz=[6x6]
 Rp    uncertain rotation pair, Rp.Rp=[R1,R2]=[3x6], Rp.Cp=[6x6]
 Msp   uncertain motion pair s, Msp.Msp=[Ms1,Ms2]=[4x8], Msp.Csp=[12x12]
 Mzp   uncertain motion pair zeta, Mzp.Mzp=[Mz1,Mz2]=[4x8], Mzp.Czp=[12x12]
 Rs    [9xN_samples] noisy vectorized rotation matrices
 Mss   [16xN_samples] noisy vectorized motion matrices s
 Mzs   [16xN_samples] noisy vectorized motion matrices zeta

 wf 5/2020

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 function [R0p,M0p,Cp,R,Ms,Mz,Rp,Msp,Mzp,Rs,Mss,Mzs]=...
0002     generate_test_data_uncertain_frames(sigma_g,sigma_n,N_samples)
0003 %%  generate test data for checking uncertain frames
0004 %
0005 % sigma_g       stdv. for generating frames
0006 % sigma_n       stdv. for generaing noise
0007 % N_samples     number of samples for checking CovMs
0008 %
0009 % R0p   [3x6] rotation pair [R1,R2]
0010 % M0p   [4x8] motion pair [M1,M2]
0011 % Cp    [12x12] CovM, also used partially
0012 % R     uncertain rotation, R.R=[3x3], R.C=[3x3]
0013 % Ms    uncertain motion_s, Ms.Ms=[4x4], Ms.Cs=[6x6]
0014 % Mz    uncertain motion_zeta, Mz.Mz=[4x4], Mz.Cz=[6x6]
0015 % Rp    uncertain rotation pair, Rp.Rp=[R1,R2]=[3x6], Rp.Cp=[6x6]
0016 % Msp   uncertain motion pair s, Msp.Msp=[Ms1,Ms2]=[4x8], Msp.Csp=[12x12]
0017 % Mzp   uncertain motion pair zeta, Mzp.Mzp=[Mz1,Mz2]=[4x8], Mzp.Czp=[12x12]
0018 % Rs    [9xN_samples] noisy vectorized rotation matrices
0019 % Mss   [16xN_samples] noisy vectorized motion matrices s
0020 % Mzs   [16xN_samples] noisy vectorized motion matrices zeta
0021 %
0022 % wf 5/2020
0023 
0024 %% generate covariance matrix 12x12
0025 % will be used throughout
0026 B  = randn(12);
0027 Cp = B'*B*sigma_n^2;
0028 
0029 %% rotations
0030 % two rotations
0031 theta0  = randn(6,1)*sigma_g;
0032 R01    = expm(calc_S(theta0(1:3)));
0033 R02    = expm(calc_S(theta0(4:6)));
0034 R0p    = [R01,R02];
0035 
0036 % uncertain rotations
0037 r = mvnrnd(zeros(6,1),Cp(1:6,1:6),1)';
0038 S1 = calc_S(r(1:3));  % S = r_wedge
0039 R1 = expm(S1)*R01;
0040 S2 = calc_S(r(4:6));  % S = r_wedge
0041 R2 = expm(S2)*R02;
0042 
0043 R.R = R1;
0044 R.C = Cp(1:3,1:3);
0045 Rp.Rp = [R1,R2];
0046 Rp.Cp = Cp(1:6,1:6);
0047 
0048 %% motions
0049 % motions
0050 s0    = randn(12,1)*sigma_g;
0051 M01    = expm(calc_A_from_s(s0(1:6)));
0052 M02    = expm(calc_A_from_s(s0(7:12)));
0053 M0p    = [M01,M02];
0054 
0055 % Uncertain motions s
0056 
0057 s = mvnrnd(zeros(12,1),Cp,1)';
0058 M1 = expm(calc_A_from_s(s(1:6)))*M01;
0059 M2 = expm(calc_A_from_s(s(7:12)))*M02;
0060 
0061 % single motion
0062 Ms.Ms = M1;
0063 Ms.Cs = Cp(1:6,1:6);
0064 % motion pair
0065 Msp.Msp = [M1,M2];
0066 Msp.Csp = Cp;
0067 
0068 % uncertain motions zeta
0069 
0070 zeta = mvnrnd(zeros(12,1),Cp,1)';
0071 M1 = [expm(calc_S(zeta(1:3)))*M01(1:3,1:3) M01(1:3,4)+zeta(4:6); 0 0 0 1];
0072 M2 = [expm(calc_S(zeta(7:9)))*M02(1:3,1:3) M02(1:3,4)+zeta(10:12); 0 0 0 1];
0073 
0074 % single motion
0075 Mz.Mz = M1;
0076 Mz.Cz = Cp(1:6,1:6);
0077 % motion pair
0078 Mzp.Mzp = [M1,M2];
0079 Mzp.Czp = Cp;
0080 
0081 %% samples
0082 
0083 % rotations
0084 rs = mvnrnd(zeros(3,1),Cp(1:3,1:3),N_samples)';
0085 check_CovM(cov(rs'),Cp(1:3,1:3),N_samples,0.99);
0086 Rs = zeros(9,N_samples);
0087 for n = 1:N_samples
0088     Rn      = expm(calc_S(rs(:,n)))*R01;
0089     Rs(:,n) = Rn(:);
0090 end
0091 
0092 % motions s
0093 ss = mvnrnd(zeros(6,1),Cp(1:6,1:6),N_samples)';
0094 Mss = zeros(16,N_samples);
0095 for n=1:N_samples
0096     Mn = expm(calc_A_from_s(ss(:,n)))*M01;
0097     Mss(:,n)=Mn(:);
0098 end
0099 
0100 % motions zeta
0101 zetas = mvnrnd(zeros(6,1),Cp(1:6,1:6),N_samples)';
0102 Mzs = zeros(16,N_samples);
0103 for n=1:N_samples
0104     Mn = [expm(calc_S(zetas(1:3,n)))*M01(1:3,1:3) M01(1:3,4)+zetas(4:6,n); ...
0105         0 0 0 1];
0106     Mzs(:,n)=Mn(:);
0107 end
0108 
0109 
0110 return

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