estimate_dem_robust_flat

% robustly estimate correction to dem, flatness

 [A,dx,v,W]=...
   estimate_dem_slope_dependent...
   (N,U,Np,Nr,Mc,poi,xa,weights,sigma_k,iter,out_print,type_robust);
 
 N         = number of observations (points+second derivatives)
 U         = number of unknowns (dem)
 Np        = number of points
 Nr        = number of rows
 Mc        = number oc columns 
 poi       = Npx4 matrix of points (x,y,h,sigma), x,y, referring to grid
 xa        = approximate values for dem
 weights   = Nx1 vector of weights in [0,1]
 sigma_s   = standard deviation of first derivatives
 out_print = 0,1,2
 type_robust = 0,1,2,3, (00,01,10,11) for points and dem

 A         = NxU sparse designa matrix
 weights   = new weights
 dx        = delta dem
 v         = normalized residuals
 W         = weigths of obs. combining uncertainty and robust factor

 wf 10/2014

0001 %% robustly estimate correction to dem, flatness
0002 %
0003 % [A,dx,v,W]=...
0004 %   estimate_dem_slope_dependent...
0005 %   (N,U,Np,Nr,Mc,poi,xa,weights,sigma_k,iter,out_print,type_robust);
0006 %
0007 % N         = number of observations (points+second derivatives)
0008 % U         = number of unknowns (dem)
0009 % Np        = number of points
0010 % Nr        = number of rows
0011 % Mc        = number oc columns
0012 % poi       = Npx4 matrix of points (x,y,h,sigma), x,y, referring to grid
0013 % xa        = approximate values for dem
0014 % weights   = Nx1 vector of weights in [0,1]
0015 % sigma_s   = standard deviation of first derivatives
0016 % out_print = 0,1,2
0017 % type_robust = 0,1,2,3, (00,01,10,11) for points and dem
0018 %
0019 % A         = NxU sparse designa matrix
0020 % weights   = new weights
0021 % dx        = delta dem
0022 % v         = normalized residuals
0023 % W         = weigths of obs. combining uncertainty and robust factor
0024 %
0025 % wf 10/2014
0026 %
0027 
0028 function [A,weights,dx,v,W]=...
0029     estimate_dem_robust_flat...
0030     (N,U,Np,Nr,Mc,poi,xa,weights,sigma_s,iter,type_robust,L1,...
0031     print_type,plot_type)
0032     
0033 
0034 %% update weights indicating outliers
0035 k=3;
0036 
0037 if print_type > 0
0038     disp('estimate_dem_robust_flat')
0039     iter_typerobust_minW=[iter,type_robust,min(weights(Np+1,end))]
0040 end
0041 %% start estimation
0042 % initiate size of A
0043 if print_type > 0
0044     tic
0045 end
0046 A = spalloc(N,U,6*N);
0047 b = zeros(N,1);
0048 W = spalloc(N,N,N);
0049 
0050 iter_switch=10;
0051 
0052 % observations, normalized with 1/sqrt of variances
0053 for p = 1:Np
0054     x = poi(p,1);
0055     y = poi(p,2);
0056     h = poi(p,3);
0057     w = sqrt(weights(p));
0058     s = poi(p,4);
0059     i = floor(x);
0060     j = floor(y);
0061     aa = x-i;
0062     bb = y-j;
0063     %delta_x
0064     A(p,i+j*Nr+1)       = (1-aa)*(1-bb)/s*w;
0065     A(p,i+j*Nr+2)       =    aa *(1-bb)/s*w;
0066 
0067     A(p,i+(j+1)*Nr+1)   = (1-aa)* bb   /s*w;
0068     A(p,i+(j+1)*Nr+2)   =    aa * bb   /s*w;
0069     delta_l =  h - ...
0070         ((1-aa)*(1-bb)*xa(i+j*Nr+1)    +...
0071         aa *(1-bb)*xa(i+j*Nr+2)    +...
0072         (1-aa)* bb   *xa(i+(j+1)*Nr+1)+...
0073         aa * bb   *xa(i+(j+1)*Nr+2));
0074     b(p)                =   delta_l   /s*w;
0075     W(p,p)=1/s*w;
0076 end
0077 
0078 
0079 if print_type > 0
0080     time_for_building_An=toc
0081 end
0082 
0083 %% priors
0084 if print_type > 0
0085     tic
0086 end
0087 Icc = Np;                    % first index for column curvatures
0088 Irr = Np + (Nr-1)*Mc;        % first index for row curvature
0089 % coefficients for column curvature
0090 % cc  1
0091 %    -1
0092 Jcc = [0,+1];  % index vector
0093 Vcc = [-1 1];                    % coefficients
0094 wcc = 1/sigma_s/sqrt(2);  % /norm(Vcc)
0095 % coefficients for row curvature
0096 % rr  1 -1
0097 Jrr = [0,+Nr];  % index vector
0098 Vrr = [-1  1];                    % coefficients
0099 wrr = 1/sigma_s/sqrt(2); % /norm(Vrr)
0100 
0101 % built up of priors in A
0102 % column slope
0103 for j=1:Mc
0104     for i=1:Nr-1
0105         IU  = i+(j-1)*Nr;
0106         %[i,j,IU]
0107         Icc = Icc+1;
0108         A(Icc,IU+Jcc)= Vcc * wcc * sqrt(weights(Icc));
0109         delta_cc     = 0-xa(IU+Jcc)' * Vcc';
0110         b(Icc)       = delta_cc * wcc * sqrt(weights(Icc));
0111         W(Icc,Icc)   = wcc * sqrt(weights(Icc));
0112     end
0113 end
0114 % row slope
0115 for j=1:Mc-1
0116     for i=1:Nr
0117         IU  = i+(j-1)*Nr;
0118         %[i,j,IU]
0119         Irr = Irr+1;
0120         A(Irr,IU+Jrr)= Vrr * wrr * sqrt(weights(Irr));
0121         delta_rr     = 0-xa(IU+Jrr)' * Vrr';
0122         b(Irr)       = delta_rr * wrr * sqrt(weights(Irr));
0123         W(Irr,Irr)=wrr * sqrt(weights(Irr));
0124     end
0125 end
0126 %
0127 if print_type > 0 
0128     time_for_building_A=toc
0129 end
0130 if print_type > 1
0131     if U < 1000
0132         Ab = 2*[full(A)]
0133         null_A_prior = U-rank(full(A(U+1:end,:)))
0134         rankA= rank(full(A(U+1:end,:)))
0135     end
0136 
0137 
0138         figure
0139         spy(A);
0140 
0141 
0142 end
0143 if print_type > 0
0144     time_for_building_Ae=toc
0145     start_time = cputime
0146 end
0147 %% solve
0148 % no sort
0149 % tic
0150 % [C,R]=qr(A,b,0);
0151 % dx = R\C;
0152 % time_for_QR_original=toc
0153 % % dx_unsorted = dx;
0154 
0155 
0156 % sort
0157 if print_type > 0
0158     tic
0159 end
0160 Nmatrix  = A'*A;
0161 
0162 if print_type > 0
0163     size_N_matrix = size(Nmatrix)
0164     non_zeros_N_matrix =nnz(Nmatrix)
0165 end
0166 if iter==1 && plot_type > 0
0167     figure
0168     subplot(1,3,1)
0169     spy(Nmatrix)
0170 end
0171 
0172 %permx    = symrcm(Nmatrix);
0173 permx    = symamd(Nmatrix);
0174 Aperm    = A(:,permx);
0175 
0176 if iter==1 && plot_type > 0
0177     subplot(1,3,2)
0178     spy(Aperm'*Aperm)
0179 end
0180 
0181 [C,R]    = qr(Aperm,b,0);
0182 dxperm   = R\C;
0183 Pm       = speye(U);
0184 Pm       = Pm(:,permx);
0185 dx       = Pm*dxperm;
0186 if print_type > 0
0187     time_for_QR_sorted=toc
0188 end
0189 if iter==1 && plot_type > 0
0190     subplot(1,3,3)
0191     spy(R)
0192     fill_in = nnz(R)-(nnz(Nmatrix)/2+U/2);
0193     percentage_nnz   = nnz(R)/(U*(U+1)/2)
0194     relative_fill_in = fill_in/(nnz(Nmatrix)/2+U/2)-1
0195 end
0196 
0197 dx_sorted = dx;
0198 
0199 % comparison
0200 % comparison=[permx;dx_unsorted';dx_sorted';dx_sorted'-dx_unsorted';dxperm']
0201 
0202 if U < 1600 && plot_type > 0
0203     Aqt = R\A';
0204     figure
0205     spy(Aqt')
0206 end
0207 % xo=xa;
0208 % [ xa, err, iter, flag ] = jacobi_iterative_solution(A'*A, xa, A'*b, 200, 10^(-4))
0209 % dx=xa-xo;
0210 
0211 if print_type > 0
0212     time_for_solution=cputime-start_time
0213 end
0214 
0215 
0216 % residuals (normalized)
0217 v = A*dx-b;
0218 % estimated variance factor
0219 if print_type > 0
0220     eso = norm(v)/sqrt(N-U)
0221 end
0222 
0223 
0224 eso_p=median(abs(v(1:Np)))*1.48;
0225 eso_s=median(abs(v(Np+1:end)))*1.48;
0226 if print_type > 0
0227     est_s0_s0p_s0k=[eso,eso_p,eso_s]
0228 end
0229 
0230 check_AtV_null=norm((A'*v));
0231 max_v_points = max(abs(v(1:Np)));
0232 max_v_slope  = max(abs(v(Np+1,end)));
0233 
0234 % update weights indicating outliers
0235 
0236 kp = k*eso_p;
0237 kp = k;
0238 ks = k*eso_s;
0239 if print_type > 0
0240     criticalvalue_kp_sigmap_ks_sigmas=[kp,poi(1,4),ks,sigma_s]
0241 
0242     disp('vor Regewichtung')
0243     initial=[iter,type_robust,iter_switch,max(abs(v(Np+1:end)))]
0244 end
0245 
0246 if iter == 1 && type_robust > 0
0247     weights=ones(N,1);
0248 else
0249     if type_robust > 0 && L1
0250         if type_robust == 1 | type_robust == 3
0251             weights(Np+1:end) = min(1,1./(abs(v(Np+1:end))/ks+eps));
0252             check=[iter,type_robust,min(weights(Np+1:end))];
0253         end
0254         if type_robust == 2 | type_robust ==3
0255             weights(1:Np) = min(1,1./(abs(v(1:Np))/kp+eps));
0256         end
0257     else
0258         if type_robust == 1 | type_robust == 3
0259             weights(Np+1:end) = exp(-abs(v(Np+1:end))/ks);
0260         end
0261         if type_robust == 2 | type_robust ==3
0262             weights(1:Np) = exp(-abs(v(1:Np))/kp);
0263         end
0264     end
0265 end
0266 abs_dx = norm(dx);
0267 min_w_p=min(weights(1:Np));
0268 min_w_c=min(weights(Np+1:end));
0269 
0270 if print_type > 1
0271     full(inv(W)*A)
0272 end
0273 
0274 return