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
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026 B = randn(12);
0027 Cp = B'*B*sigma_n^2;
0028
0029
0030
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
0037 r = mvnrnd(zeros(6,1),Cp(1:6,1:6),1)';
0038 S1 = calc_S(r(1:3));
0039 R1 = expm(S1)*R01;
0040 S2 = calc_S(r(4:6));
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
0049
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
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
0062 Ms.Ms = M1;
0063 Ms.Cs = Cp(1:6,1:6);
0064
0065 Msp.Msp = [M1,M2];
0066 Msp.Csp = Cp;
0067
0068
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
0075 Mz.Mz = M1;
0076 Mz.Cz = Cp(1:6,1:6);
0077
0078 Mzp.Mzp = [M1,M2];
0079 Mzp.Czp = Cp;
0080
0081
0082
0083
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
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
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