0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019 clearvars
0020 close all
0021 clc
0022
0023 addpath(genpath('../General-Functions'))
0024
0025 S_level = 0.99;
0026 sigma = 0.001;
0027 N_iter = 500;
0028
0029 disp('----- Test: selection of constraints for estimating X from (Li) -----')
0030
0031 X_true = randn(4,1);
0032 X_true = X_true/norm(X_true);
0033
0034 Y1 = randn(4,1);
0035 Y2 = randn(4,1);
0036 Y3 = randn(4,1);
0037
0038 L1_true = calc_Pi(X_true)*Y1;
0039 L2_true = calc_Pi(X_true)*Y2;
0040 L3_true = calc_Pi(X_true)*Y3;
0041 L1_true = L1_true/norm(L1_true);
0042 L2_true = L2_true/norm(L2_true);
0043 L3_true = L3_true/norm(L3_true);
0044
0045 Cov1 = sigma^2*(eye(6)-...
0046 [L1_true,calc_Dual*L1_true]*[L1_true,calc_Dual*L1_true]');
0047 Cov2 = sigma^2*(eye(6)-...
0048 [L2_true,calc_Dual*L2_true]*[L2_true,calc_Dual*L2_true]');
0049 Cov3 = sigma^2*(eye(6)-...
0050 [L3_true,calc_Dual*L3_true]*[L3_true,calc_Dual*L3_true]');
0051 Cov = [Cov1 zeros(6,12);...
0052 zeros(6,6) Cov2 zeros(6,6);...
0053 zeros(6,12) Cov3];
0054
0055 Ao = [calc_Gammadual(L1_true);...
0056 calc_Gammadual(L2_true);...
0057 calc_Gammadual(L3_true)];
0058
0059 [Uo,Do,Vo] = svd(Ao);
0060
0061 Apo = Vo(:,1:3)*inv(Do(1:3,1:3))*Uo(:,1:3)';
0062
0063
0064 B = [calc_Pidual(X_true)' zeros(4,6) zeros(4,6);...
0065 zeros(4,6) calc_Pidual(X_true)' zeros(4,6);...
0066 zeros(4,6) zeros(4,6) calc_Pidual(X_true)'];
0067
0068 CovMo = Apo*(B*Cov*B')*Apo';
0069
0070 Jr = null(X_true');
0071 CovM_rr_o = Jr'*CovMo*Jr;
0072 disp('Theoretical covariance matrix of estX_r, without selection: ');disp(CovM_rr_o)
0073
0074
0075 [Gr1,M1] = calc_Gammadual_reduced(L1_true);
0076 [Gr2,M2] = calc_Gammadual_reduced(L2_true);
0077 [Gr3,M3] = calc_Gammadual_reduced(L3_true);
0078 Am = [Gr1;Gr2;Gr3];
0079
0080 [Um,Dm,Vm] = svd(Am);
0081
0082 Apm = Vm(:,1:3)*inv(Dm(1:3,1:3))*Um(:,1:3)';
0083
0084
0085 B = [M1*calc_Pidual(X_true)' zeros(2,6) zeros(2,6);...
0086 zeros(2,6) M2*calc_Pidual(X_true)' zeros(2,6);...
0087 zeros(2,6) zeros(2,6) M3*calc_Pidual(X_true)'];
0088
0089
0090 CovMm = Apm*(B*Cov*B')*Apm';
0091 CovM_rr_m = Jr'*CovMm*Jr;
0092 disp('Theoretical covariance matrix of estX_r, with selection: ');disp(CovM_rr_m)
0093
0094
0095
0096
0097 mean_o = zeros(3,1);
0098 mean_m = zeros(3,1);
0099 CovM_emp_o = zeros(3);
0100 CovM_emp_m = zeros(3);
0101
0102 for iter = 1:N_iter
0103 L1 = rand_gauss(L1_true,Cov1,1);
0104 L2 = rand_gauss(L2_true,Cov2,1);
0105 L3 = rand_gauss(L3_true,Cov3,1);
0106 L1 = sugr_constrain_3D_Line(L1);
0107 L2 = sugr_constrain_3D_Line(L2);
0108 L3 = sugr_constrain_3D_Line(L3);
0109 L1 = L1/norm(L1);
0110 L2 = L2/norm(L2);
0111 L3 = L3/norm(L3);
0112
0113
0114 A = [calc_Gammadual(L1);calc_Gammadual(L2);calc_Gammadual(L3)];
0115
0116
0117 [~,~,V] = svd(A);
0118
0119 X_est_o = V(:,4);
0120
0121 Xr_o = Jr' * X_est_o;
0122
0123 mean_o = mean_o+Xr_o;
0124 CovM_emp_o = CovM_emp_o+Xr_o*Xr_o';
0125
0126
0127 Gr1 = calc_Gammadual_reduced(L1);
0128 Gr2 = calc_Gammadual_reduced(L2);
0129 Gr3 = calc_Gammadual_reduced(L3);
0130 A = [Gr1;Gr2;Gr3];
0131
0132 [U,D,V] = svd(A);
0133
0134 X_est_m = V(:,4);
0135 Xr_m = Jr' * X_est_m;
0136 mean_m = mean_m+Xr_m;
0137 CovM_emp_m = CovM_emp_m+Xr_m*Xr_m';
0138 end
0139 mean_o = mean_o/N_iter;
0140 mean_m = mean_m/N_iter;
0141 CovM_emp_m = CovM_emp_m/N_iter;
0142 CovM_emp_o = CovM_emp_o/N_iter;
0143
0144 disp('Empirical covariance matrix of estX_r, with selection: ');disp(CovM_emp_m)
0145 disp('Empirical covariance matrix of estX_r, without selection: ');disp(CovM_emp_o)
0146
0147 disp(strcat('Sample size: ',num2str(N_iter)));
0148 disp('Test of bias')
0149 Xo = mean_o'*inv(CovM_rr_o)*mean_o*N_iter;
0150 Xm = mean_m'*inv(CovM_rr_m)*mean_m*N_iter;
0151 Tm = chi2inv(S_level,3);
0152 To = Tm;
0153 if Xo < To
0154 disp(strcat('Estimation without selection : ',num2str(Xo),' < [',num2str(To),']'));
0155 else
0156 disp(strcat('Estimation without selection : ',num2str(Xo),' > [',num2str(To),'] ***'));
0157 end
0158 if Xm < Tm
0159 disp(strcat('estimation with selection : ',num2str(Xm),' < [',num2str(Tm),']'));
0160 else
0161 disp(strcat('estimation with selection : ',num2str(Xm),' > [',num2str(Tm),'] ***'));
0162 end
0163 disp(' ')
0164 disp('Test of empirical and theoretical CovM')
0165 [lambdam,Tm] = check_CovM(CovM_emp_m,CovM_rr_m,N_iter,S_level);
0166 [lambdao,To] = check_CovM(CovM_emp_o,CovM_rr_o,N_iter,S_level);
0167
0168 lo_To_lm_Tm = [lambdao,lambdam,Tm];
0169 if lambdao > To(1) && lambdao < To(2)
0170 disp(strcat('estimation without selection : ',num2str(lambdao),' in [',num2str(To),']'));
0171 else
0172 disp(strcat('estimation without selection : ',num2str(lambdao),' not in [',num2str(To),'] ***'));
0173 end
0174 if lambdam > To(1) && lambdao < To(2)
0175 disp(strcat('estimation with selection : ',num2str(lambdam),' in [',num2str(Tm),']'));
0176 else
0177 disp(strcat('estimation with selection : ',num2str(lambdam),' not in [',num2str(Tm),'] ***'));
0178 end
0179
0180
0181
0182