0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020 function [E, sigma_0, R] = sugr_estimation_ml_E_Matrix_from_point_pairs(l, xa, T, maxiter)
0021
0022 global print_option_estimation
0023 global min_redundancy
0024
0025
0026 U = 5;
0027 Nlr = 4;
0028 Gc = 1;
0029
0030 lh = l.h;
0031 lCrr = l.Crr;
0032 Nc = size(lh, 1);
0033
0034 G = Nc * Gc;
0035 R = G - U;
0036 if R < 0
0037 return
0038 end;
0039
0040 estl = lh;
0041 w_f = ones(Nc, 1);
0042
0043 if isstruct(xa)
0044 estx = xa.bR;
0045 else
0046 estx = xa;
0047 end
0048
0049 s = 0;
0050 residuals = zeros(Nc, 1);
0051
0052
0053 for nu = 1:maxiter
0054 if print_option_estimation > 0
0055 sprintf('nu = %2', nu);
0056 end
0057 Cr = zeros(Nc, Nlr, Nlr);
0058 v_r = zeros(Nc, Nlr);
0059 A = zeros(Nc, Gc, U);
0060 B = zeros(Nc, Gc, Nlr);
0061 W = zeros(Nc, Gc, Gc);
0062 cg = zeros(Nc, Gc);
0063
0064 N_matrix = zeros(U);
0065 h_vector = zeros(U, 1);
0066
0067
0068 for n = 1:Nc
0069 estl_n = estl(n, :)';
0070 l_n = lh(n, :)';
0071 Crr_n = squeeze(lCrr(n, :, :));
0072
0073 [lr_n, Cr_n, cg_n, atr_n, btr_n] = sugr_ghm_cg_E_matrix_from_point_pairs(l_n, estl_n, estx, Crr_n);
0074
0075 A(n, :, :) = atr_n;
0076 B(n, :, :) = btr_n;
0077 Cr(n, :, :) = Cr_n;
0078 v_r(n, :) = - lr_n';
0079 cg(n, :) = cg_n';
0080
0081
0082 bCovb_n = btr_n * Cr_n * btr_n';
0083 W(n, :, :) = inv(bCovb_n) * w_f(n);
0084 aW = atr_n' * squeeze(W(n,:,:));
0085
0086 N_matrix = N_matrix + aW * atr_n;
0087 h_vector = h_vector + aW * cg_n;
0088
0089 end
0090
0091 if print_option_estimation > 0
0092 N_matrix = N_matrix
0093 l_lambda = log(eigs(N_matrix))
0094 h_vector
0095 end
0096
0097 det(N_matrix);
0098
0099 Cxrxr = inv(N_matrix);
0100 estx_r = Cxrxr * h_vector;
0101
0102 if print_option_estimation > 0
0103 disp(['Result of estimation in iteration: ', num2str(nu)]);
0104 estimated_x = estx_r'
0105 end
0106
0107 max(abs(estx_r) ./ sqrt(diag(Cxrxr)));
0108 if max(abs(estx_r) ./ sqrt(diag(Cxrxr))) < T
0109 s = 2;
0110 end
0111
0112
0113 Omega = 0;
0114
0115 check = zeros(Gc, 1);
0116 for n = 1:Nc
0117
0118 Clrlr = squeeze(Cr(n, :, :));
0119
0120
0121 delta_l_r = Clrlr * squeeze(B(n, :, :)) * squeeze(W(n, :, :)) * ...
0122 (cg(n, :)'-squeeze(A(n,:,:))' * estx_r) - v_r(n, :)';
0123 ver_r = v_r(n, :)' + delta_l_r;
0124
0125
0126 if w_f(n) > 0
0127 vvp_r = ver_r' * inv(Clrlr) * ver_r;
0128 Omega = Omega + vvp_r;
0129 residuals(n) = vvp_r;
0130
0131
0132
0133
0134
0135
0136
0137
0138 end
0139
0140
0141
0142
0143 estl(n, 1:3) = sugr_ghm_update_vector(estl(n, 1:3)',delta_l_r(1:2))';
0144 estl(n, 4:6) = sugr_ghm_update_vector(estl(n, 4:6)',delta_l_r(3:4))';
0145
0146 end
0147 sigma_0 = 1;
0148 if R > min_redundancy
0149 sigma_0 = sqrt(Omega / R);
0150 end
0151 if print_option_estimation > 0
0152 R
0153 sigma_0
0154
0155 end
0156
0157
0158 estx = sugr_ghm_update_E_Matrix(estx, estx_r);
0159
0160
0161 for n = 1:Nc
0162 E = calc_S(estx(:, 1)) * estx(:, 2:4)';
0163 check(n) = estl(n, 1:3) * E * estl(n, 4:6)';
0164 end
0165
0166
0167 if s == 2
0168 break
0169 end
0170
0171 end
0172
0173
0174
0175
0176 if R > min_redundancy
0177 sigma_0 = sqrt(Omega / R);
0178 else
0179 sigma_0 = 1;
0180 end
0181
0182
0183
0184
0185
0186
0187
0188
0189 E = sugr_E_Matrix(estx(:, 1), estx(:, 2:4), Cxrxr);
0190
0191
0192
0193
0194