0001 function check_basics_motions_s(Mss,Msps,b_output)
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014 sigma_n = 10^-8;
0015
0016 Ms = Mss.Ms;
0017 Msp = Msps.Msp;
0018
0019
0020 Mad = [Ms(1:3,1:3) zeros(3); calc_S(Ms(1:3,4))*Ms(1:3,1:3) Ms(1:3,1:3)];
0021
0022
0023
0024 s = randn(6,1)*sigma_n;
0025 M_s = expm(calc_A_from_s(s));
0026
0027
0028 M_s_ad = Ms * M_s * inv(Ms);
0029
0030
0031 s_ad = calc_s_from_A(M_s_ad);
0032
0033
0034 check_s_ad = (s_ad - Mad * s)';
0035 if b_output
0036 check_s_ad = (s_ad - Mad * s)'
0037 end
0038
0039
0040
0041
0042 M_s_r = M_s * Ms;
0043 M_s_inv = inv(M_s_r);
0044
0045
0046 s_inv = calc_s_from_A(M_s_inv * Ms);
0047
0048
0049 check_s_inv = (s_inv -(- inv(Mad) * s))';
0050 if b_output
0051 check_s_inv = (s_inv -(- inv(Mad) * s))'
0052 end
0053
0054
0055 [Mssi,J] = calc_inverse_M_s(Mss);
0056 MM = calc_inverse_M_s(Mssi);
0057
0058
0059 check_Ms = Mss.Ms-MM.Ms;
0060 check_Cs = Mss.Cs-MM.Cs;
0061 check_J = J-(-inv(Mad));
0062 if b_output
0063 check_Ms = Mss.Ms-MM.Ms
0064 check_Cs = Mss.Cs-MM.Cs
0065 check_J = J-(-inv(Mad));
0066 end
0067
0068
0069
0070
0071 s = randn(12,1)*sigma_n;
0072
0073 M1_r = expm(calc_A_from_s(s(1:6 )))*Msp(:,1:4);
0074 M2_r = expm(calc_A_from_s(s(7:12)))*Msp(:,5:8);
0075
0076 M = Msp(:,5:8) * Msp(:,1:4);
0077 M_r = M2_r * M1_r;
0078
0079 s_e = calc_s_from_A(M_r*inv(M));
0080
0081
0082 R2 = Msp(1:3,5:7);
0083 Z2 = Msp(1:3,8);
0084 M_2_ad = [R2 zeros(3); calc_S(Z2)*R2 R2];
0085
0086
0087 check_s_concatenate = (s_e - (M_2_ad*s(1:6) + s(7:12)))';
0088 if b_output
0089 check_s_concatenate = (s_e - (M_2_ad*s(1:6) + s(7:12)))'
0090 end
0091
0092
0093
0094
0095
0096 s = randn(12,1)*sigma_n;
0097
0098 M1_r = expm(calc_A_from_s(s(1:6 )))*Msp(:,1:4);
0099 M2_r = expm(calc_A_from_s(s(7:12)))*Msp(:,5:8);
0100
0101 M = inv(Msp(:,1:4)) * Msp(:,5:8) ;
0102 M_r = inv(M1_r) * M2_r;
0103
0104 s_e = calc_s_from_A(M_r*inv(M));
0105
0106
0107 R1 = Msp(1:3,1:3);
0108 Z1 = Msp(1:3,4);
0109 M_1_ad = [R1 zeros(3); calc_S(Z1)*R1 R1];
0110
0111
0112 check_s_relative = (s_e - (inv(M_1_ad)*(s(7:12) - s(1:6))))';
0113 if b_output
0114 check_s_relative = (s_e - (inv(M_1_ad)*(s(7:12) - s(1:6))))'
0115 end
0116
0117
0118 Ms1.Ms = Msps.Msp(:,1:4);
0119 Ms2.Ms = Msps.Msp(:,5:8);
0120
0121 C = [Msps.Csp(1:6,1:6) zeros(6); zeros(6) Msps.Csp(7:12,7:12)];
0122 C1 = Msps.Csp(1:6,1:6);
0123 C2 = Msps.Csp(7:12,7:12);
0124
0125 Msps.Csp = C;
0126
0127 Ms1.Cs = C1;
0128 Ms2.Cs = C2;
0129
0130
0131 Msd = calc_relative_M_s(Msps);
0132
0133
0134 Ms1i = calc_inverse_M_s(Ms1);
0135
0136 Ms12.Msp = [Ms2.Ms,Ms1i.Ms];
0137 Ms12.Csp = [Ms2.Cs zeros(6); zeros(6) Ms1i.Cs];
0138 Msd_ch = calc_concatenated_M_s(Ms12);
0139
0140
0141 check_relative_Ms = Msd_ch.Ms-Msd.Ms;
0142 check_relative_Cs = Msd_ch.Cs-Msd.Cs;
0143
0144 if b_output
0145 check_relative_Ms = Msd_ch.Ms-Msd.Ms
0146 check_relative_Cs = Msd_ch.Cs-Msd.Cs
0147 end
0148
0149
0150 T = 10^-6;
0151 if norm(check_s_ad) < T*sigma_n && ...
0152 norm(check_s_inv) < T*sigma_n && ...
0153 norm(check_Ms) < T*sigma_n && ...
0154 norm(check_Cs) < T*sigma_n^2 && ...
0155 norm(check_J) < T*sigma_n && ...
0156 norm(check_s_concatenate) < T*sigma_n && ...
0157 norm(check_s_relative) < T*sigma_n && ...
0158 norm(check_relative_Ms) < T*sigma_n && ...
0159 norm(check_relative_Cs) < T*sigma_n^2
0160
0161 display('motions s ok +++++++++++++++++++++++++++++++++++++++')
0162 else
0163 display('notions s not ok ***********************************')
0164 end
0165 end
0166