0001 function check_basics_motions_z(Mzs,Mzps,b_output)
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014 sigma_n = 10^-8;
0015
0016 Mz = Mzs.Mz;
0017 Mzp = Mzps.Mzp;
0018
0019
0020 zeta = randn(6,1)*sigma_n;
0021
0022 M_zeta = [expm(calc_S(zeta(1:3))) zeta(4:6); 0 0 0 1];
0023 M_zeta_ad = Mz * M_zeta * inv(Mz);
0024
0025
0026 M_ad = [Mz(1:3,1:3) zeros(3); calc_S(Mz(1:3,4))*Mz(1:3,1:3) Mz(1:3,1:3)];
0027
0028
0029 zeta_ad = calc_s_from_A(M_zeta_ad);
0030
0031
0032 check_zeta_ad = (zeta_ad - M_ad * zeta)';
0033 if b_output
0034 check_zeta_ad = (zeta_ad - M_ad * zeta)'
0035 end
0036
0037
0038
0039
0040 M_zeta_r = [expm(calc_S(zeta(1:3)))*Mz(1:3,1:3) Mz(1:3,4)+zeta(4:6); 0 0 0 1];
0041 M_zeta_inv = inv(M_zeta_r);
0042
0043
0044 zeta_inv = calc_z_from_M_M0(M_zeta_inv,inv(Mz));
0045
0046
0047 M_ad_zeta_inv = [Mz(1:3,1:3)' zeros(3); Mz(1:3,1:3)'*calc_S(Mz(1:3,4)) Mz(1:3,1:3)'];
0048
0049
0050 check_zeta_inv = (zeta_inv - (-M_ad_zeta_inv * zeta))';
0051 if b_output
0052 check_zeta_inv = (zeta_inv - (-M_ad_zeta_inv * zeta))'
0053 end
0054
0055
0056 [Mzsi,J] = calc_inverse_M_z(Mzs);
0057 MM = calc_inverse_M_z(Mzsi);
0058
0059
0060 check_Mz_inv = Mzs.Mz-MM.Mz;
0061 check_Cz_inv = Mzs.Cz-MM.Cz;
0062 if b_output
0063 check_Mz_inv = Mzs.Mz-MM.Mz
0064 check_Cz_inv = Mzs.Cz-MM.Cz
0065 end
0066
0067
0068
0069
0070 zeta = randn(12,1)*sigma_n;
0071
0072 M1_r = [expm(calc_S(zeta(1:3)))*Mzp(1:3,1:3) Mzp(1:3,4)+zeta(4:6); 0 0 0 1];
0073 M2_r = [expm(calc_S(zeta(7:9)))*Mzp(1:3,5:7) Mzp(1:3,8)+zeta(10:12); 0 0 0 1];
0074
0075 M = Mzp(:,5:8) * Mzp(:,1:4);
0076 M_r = M2_r * M1_r;
0077
0078 zeta_e = calc_z_from_M_M0(M_r,M);
0079
0080
0081 R2 = Mzp(1:3,5:7);
0082 Z1 = Mzp(1:3,4);
0083
0084 check_zeta_concatenate_rho = ...
0085 zeta_e(1:3) - (R2 * zeta(1:3) + zeta(7:9));
0086 check_zeta_concatenate_tau = ...
0087 zeta_e(4:6) - (R2 * zeta(4:6) + zeta(10:12) - calc_S(R2*Z1)*zeta(7:9));
0088
0089 check_zeta_concatenate = [check_zeta_concatenate_rho;check_zeta_concatenate_tau]';
0090 if b_output
0091 check_zeta_concatenate = [check_zeta_concatenate_rho;check_zeta_concatenate_tau]'
0092 end
0093
0094
0095
0096
0097
0098 zeta = randn(12,1)*sigma_n;
0099
0100 M1_r = [expm(calc_S(zeta(1:3)))*Mzp(1:3,1:3) Mzp(1:3,4)+zeta(4:6); 0 0 0 1];
0101 M2_r = [expm(calc_S(zeta(7:9)))*Mzp(1:3,5:7) Mzp(1:3,8)+zeta(10:12); 0 0 0 1];
0102
0103 M = inv(Mzp(:,1:4)) * Mzp(:,5:8) ;
0104 M_r = inv(M1_r) * M2_r;
0105
0106 zeta_e = calc_z_from_M_M0(M_r,M);
0107
0108
0109
0110
0111
0112
0113
0114 R1 = Mzp(1:3,1:3);
0115 Z1 = Mzp(1:3,4);
0116 Z2 = Mzp(1:3,8);
0117
0118 check_zeta_concatenate_rho = ...
0119 zeta_e(1:3) - (R1' * (zeta(7:9) - zeta(1:3)));
0120 check_zeta_concatenate_tau = ...
0121 zeta_e(4:6) - (R1' * (zeta(10:12) - zeta(4:6)) ...
0122 + R1' * calc_S(Z2-Z1) * zeta(1:3));
0123
0124 check_zeta_relative = [check_zeta_concatenate_rho;check_zeta_concatenate_tau]';
0125 if b_output
0126 check_zeta_relative = [check_zeta_concatenate_rho;check_zeta_concatenate_tau]'
0127 end
0128
0129
0130
0131 Mz1.Mz = Mzps.Mzp(:,1:4);
0132 Mz2.Mz = Mzps.Mzp(:,5:8);
0133
0134 C1 = Mzps.Czp(1:6,1:6);
0135 C2 = Mzps.Czp(7:12,7:12);
0136
0137 C = [C1 zeros(6); zeros(6) C2];
0138 Mz1.Cz = C1;
0139 Mz2.Cz = C2;
0140 Mzps.Czp = C;
0141
0142
0143 Mzd = calc_relative_M_z(Mzps);
0144
0145
0146 Mz1i = calc_inverse_M_z(Mz1);
0147
0148 Mz12.Mzp = [Mz2.Mz,Mz1i.Mz];
0149 Mz12.Czp = [Mz2.Cz zeros(6); zeros(6) Mz1i.Cz];
0150 Mzd_ch = calc_concatenated_M_z(Mz12);
0151
0152
0153 check_relative_Mz = Mzd_ch.Mz-Mzd.Mz;
0154 check_relative_Cz = Mzd_ch.Cz-Mzd.Cz;
0155 if b_output
0156 check_relative_Mz = Mzd_ch.Mz-Mzd.Mz
0157 check_relative_Cz = Mzd_ch.Cz-Mzd.Cz
0158 end
0159
0160
0161 T = 10^-6;
0162 if norm(check_zeta_ad) < T*sigma_n && ...
0163 norm(check_zeta_inv) < T*sigma_n && ...
0164 norm(check_Mz_inv) < T*sigma_n && ...
0165 norm(check_Cz_inv) < T*sigma_n^2 && ...
0166 norm(check_zeta_concatenate) < T*sigma_n && ...
0167 norm(check_zeta_relative) < T*sigma_n && ...
0168 norm(check_relative_Mz) < T*sigma_n && ...
0169 norm(check_relative_Cz) < T*sigma_n^2
0170
0171 display('motions zeta ok +++++++++++++++++++++++++++++++++++++++')
0172 else
0173 display('notions zeta not ok ***********************************')
0174 end
0175 end
0176