0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023 function [x,sigma_0,R] = ...
0024 sugr_estimation_ml_Point_2D_from_Lines_Hessian(l,xa,T,maxiter)
0025
0026 global print_option_estimation
0027 global min_redundancy
0028
0029
0030
0031
0032 lh = l.h;
0033 lCrr = l.Crr;
0034
0035 [N,~] = size(lh);
0036 R = N-2;
0037 if R < 0
0038 return
0039 end;
0040
0041
0042 Cee=zeros(N,2,2);
0043 le = zeros(N,2);
0044 for n=1:N
0045 lhn.h = lh(n,:)';
0046 lhn.Crr = reshape(lCrr(n,:,:),2,2);
0047 lhn.type = 2;
0048 [en,Ceen] = sugr_Line_2D_hom2Hes(lhn);
0049 le(n,:) = en';
0050 Cee(n,1:2,1:2) = Ceen(1:2,1:2);
0051 end
0052
0053 estl = le;
0054 w_g = ones(N,1);
0055
0056 if isstruct(xa)
0057 estx = xa.h(1:2)/xa.h(3);
0058 else
0059 estx = xa(1:2)/xa(3);
0060 end
0061
0062 s=0;
0063 residuals=zeros(N,1);
0064
0065
0066
0067 v = zeros(N,2);
0068 for nu=1:maxiter
0069 if print_option_estimation > 0
0070 sprintf('nu = %2',nu);
0071 end
0072
0073
0074 A = zeros(N,2);
0075 B = zeros(N,2);
0076 W = zeros(N,1);
0077 cg = zeros(N,1);
0078
0079 N_matrix = zeros(2);
0080 h_vector = zeros(2,1);
0081
0082
0083 for n=1:N
0084 estl_n = estl(n,:)';
0085 l_n = le(n,:)';
0086 Cee_n = squeeze(Cee(n,:,:));
0087
0088 [va,Cee_n,cg_n,at_n,bt_n] = ...
0089 sugr_ghm_cg_Point_2D_from_Lines_Hessian(l_n,estl_n,estx,Cee_n);
0090
0091 A(n,:) = at_n(:);
0092 B(n,:) = bt_n(:);
0093 v(n,:) = -va';
0094 cg(n) = cg_n;
0095
0096
0097 bCovb_n = bt_n * Cee_n * bt_n';
0098 W(n) = 1 / bCovb_n * w_g(n);
0099 aW = at_n' * W(n);
0100
0101 N_matrix = N_matrix + aW * at_n;
0102 h_vector = h_vector + aW * cg_n;
0103
0104 end
0105
0106
0107
0108 Cxx = inv(N_matrix);
0109 Delta_estx = Cxx * h_vector;
0110
0111 if print_option_estimation > 1
0112 disp(['Result of estimation in iteration: ',num2str(nu)]);
0113
0114 end
0115
0116 max(abs(Delta_estx)./sqrt(diag(Cxx)));
0117 if max(abs(Delta_estx)./sqrt(diag(Cxx))) < T
0118 s=2;
0119 end
0120
0121
0122 Omega = 0;
0123 check=zeros(2,1);
0124 for n=1:N
0125
0126 Cee_n = squeeze(Cee(n,:,:));
0127
0128
0129 delta_l = Cee_n * B(n,:)' * W(n) * (cg(n)-A(n,:)*Delta_estx)- v(n,:)';
0130 ver = v(n,:)' + delta_l;
0131
0132
0133 if w_g(n) > 0
0134 vvp = ver' * inv(Cee_n) * ver;
0135 Omega = Omega + vvp;
0136 residuals(n)=vvp;
0137 check=check+A(n,:)'*W(n)*B(n,:)*ver;
0138
0139
0140 if vvp > 10
0141 w_g(n)=0;
0142 end
0143 end
0144
0145 estl(n,:) = estl(n,:)+delta_l';
0146
0147 end
0148 if print_option_estimation > 0
0149 sigma_0 = sqrt(Omega/R)
0150 end
0151
0152
0153
0154
0155
0156
0157
0158
0159 if s == 2
0160 break
0161 end
0162
0163 end
0164
0165
0166
0167 if R > 0
0168 sigma_0 = sqrt(Omega/R);
0169 else
0170 sigma_0 = 1;
0171 end
0172
0173
0174
0175 f = 1;
0176 if R > min_redundancy
0177 f = sigma_0;
0178 end
0179
0180
0181 estCxx = f^2 * Cxx;
0182
0183
0184 x = sugr_Point_2D(estx,estCxx);
0185