% robustly estimate correction to dem, using design matrix
Optimization function using curvature for regularization
energy = sum_m rho(l_m-a_m)/sigma_n) +
1/sigma_d sum_ij d_ii^2+2d_ij^2+djj^2
l_m = observed heights, interpolated bilinearly
a_m unknown grid heights, d_ij second derivatives
[weights,dx,v,W,Nmatrix]=...
estimate_dem_robust_2_plus...
(A,b,N,U,Np,Nr,Mc,poi,xa,weights,sigma_k,iter,iter_switch,type_robust,L1,...
out_in,print_type,plot_type)
A = design matrix (sparse)
b = right hand sides
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
weights = Nx1 vector of weights in [0,1]
sigma_k = standard deviation of second derivatives
iter = current iteration for controling weighting
iter_switch = iteration number where to switch type of weighting
type_robust = 0,1,2, (00,01,10) for points and dem
L1 = 0/1 choice of weight function (0 = exponential, 1 = L1)
out_in = boolean Np-vector indicating inliers
print_type = 0,1,2,3
plot_type = 0,1,2
weights = new weights
dx = delta dem
v = normalized residuals
W = weigths of obs. combining uncertainty and robust factor
Nmatrix = normal equation matrix
wf 7/204, 4/20180001 %% robustly estimate correction to dem, using design matrix 0002 % 0003 % Optimization function using curvature for regularization 0004 % energy = sum_m rho(l_m-a_m)/sigma_n) + 0005 % 1/sigma_d sum_ij d_ii^2+2d_ij^2+djj^2 0006 % l_m = observed heights, interpolated bilinearly 0007 % a_m unknown grid heights, d_ij second derivatives 0008 % 0009 % [weights,dx,v,W,Nmatrix]=... 0010 % estimate_dem_robust_2_plus... 0011 % (A,b,N,U,Np,Nr,Mc,poi,xa,weights,sigma_k,iter,iter_switch,type_robust,L1,... 0012 % out_in,print_type,plot_type) 0013 % 0014 % A = design matrix (sparse) 0015 % b = right hand sides 0016 % N = number of observations (points+second derivatives) 0017 % U = number of unknowns (dem) 0018 % Np = number of points 0019 % Nr = number of rows 0020 % Mc = number oc columns 0021 % poi = Npx4 matrix of points (x,y,h,sigma), x,y, referring to grid 0022 % weights = Nx1 vector of weights in [0,1] 0023 % sigma_k = standard deviation of second derivatives 0024 % iter = current iteration for controling weighting 0025 % iter_switch = iteration number where to switch type of weighting 0026 % type_robust = 0,1,2, (00,01,10) for points and dem 0027 % L1 = 0/1 choice of weight function (0 = exponential, 1 = L1) 0028 % out_in = boolean Np-vector indicating inliers 0029 % print_type = 0,1,2,3 0030 % plot_type = 0,1,2 0031 % 0032 % weights = new weights 0033 % dx = delta dem 0034 % v = normalized residuals 0035 % W = weigths of obs. combining uncertainty and robust factor 0036 % Nmatrix = normal equation matrix 0037 % 0038 % wf 7/204, 4/2018 0039 % 0040 0041 function [weights,dx,v,W,Nmatrix]=... 0042 estimate_dem_robust_2_plus... 0043 (A,b,permx,N,U,Np,Nr,Mc,poi,weights,sigma_k,type_robust,L1,... 0044 out_in,print_type) 0045 0046 Nmatrix=0; 0047 0048 %% factor for update weights indicating outliers 0049 k= 3; 0050 0051 %% start estimation 0052 if print_type > 1 0053 start_time_sol = cputime; 0054 end 0055 0056 %b = zeros(N,1); 0057 %W = spalloc(N,N,N); 0058 W = zeros(N,1); 0059 0060 % if print_type>0 0061 % tic 0062 % end 0063 0064 %% determine linearized observations dl by bilinear intoerpolation 0065 for p = 1:Np 0066 %W(p,p)= 1/poi(p,4)*sqrt(weights(p))*out_in(p); 0067 W(p)= 1/poi(p,4)*sqrt(weights(p))*out_in(p); 0068 end 0069 0070 % if print_type > 0 0071 % display(['Time for dl points = ',num2str(toc)]) 0072 % end 0073 0074 %% determin lineraized observations for priors 0075 0076 % if print_type > 0 0077 % tic 0078 % end 0079 % switch typec 0080 % case 0 0081 Icc = Np; % first index for column curvatures 0082 Irr = Np + (Nr-2)*Mc; % first index for row curvature 0083 Irc = Np + (Nr-2)*Mc + Nr*(Mc-2); % first index for torsion 0084 % coefficients for column curvature 0085 % cc 1 0086 % -2 0087 % 1 0088 Jcc = [-1,0,+1]; % index vector 0089 Vcc = [ 1 -2 1]; % coefficients 0090 wcc = 1/sigma_k; %/sqrt(6); % /norm(Vcc) 0091 % coefficients for row curvature 0092 % rr 1 -2 1 0093 Jrr = [-Nr,0,+Nr]; % index vector 0094 Vrr = [1 -2 1]; % coefficients 0095 wrr = 1/sigma_k; % /norm(Vrr) 0096 % coefficients for torsion 0097 % rc 1 -1 0098 % -1 1 ^ 0099 Jrc = [-Nr-1,-Nr,-1,0]; % indices 0100 Vrc = [1 -1 -1 1]; % coefficients 0101 wrc = 1/sigma_k/sqrt(2); % for quadratic variation 0102 % built up of priors in A 0103 % column curvature 0104 for j=1:Mc 0105 for i=2:Nr-1 0106 Icc = Icc+1; 0107 %W(Icc,Icc) = wcc * sqrt(weights(Icc)); 0108 W(Icc) = wcc * sqrt(weights(Icc)); 0109 end 0110 end 0111 % row curvature 0112 for j=2:Mc-1 0113 for i=1:Nr 0114 Irr = Irr+1; 0115 %W(Irr,Irr) = wrr * sqrt(weights(Irr)); 0116 W(Irr) = wrr * sqrt(weights(Irr)); 0117 end 0118 end 0119 % torsion 0120 for j=2:Mc 0121 for i=2:Nr 0122 Irc = Irc+1; 0123 %W(Irc,Irc) = wrc * sqrt(weights(Irc)); 0124 W(Irc) = wrc * sqrt(weights(Irc)); 0125 end 0126 end 0127 % if print_type > 0 0128 % display(['Time for dl priors = ',num2str(toc)]) 0129 % end 0130 0131 if print_type > 1 0132 time_for_building_dl=toc 0133 end 0134 0135 %% solve 0136 0137 if print_type > 0 0138 tic 0139 end 0140 0141 Aperm = A(:,permx); 0142 0143 %[C,R] = qr(W*A,W*b,0); 0144 0145 Apermw =Aperm; 0146 for u=1:U 0147 Apermw(:,u)=W.*Aperm(:,u); 0148 end 0149 0150 [C,R] = qr(Apermw,W.*b,0); 0151 dxperm = R\C; 0152 Pm = speye(U); 0153 Pm = Pm(:,permx); 0154 dx = Pm*dxperm; 0155 0156 % residuals (non-normalized) 0157 v = A*dx-b; 0158 0159 % estimated sqrt of variance factor 0160 eso = norm(W.*v)/sqrt(N-U); 0161 0162 if type_robust > 0 0163 % robust sigma_0 for points 0164 eso_p = median(abs(v(1:Np)))*1.48; 0165 0166 % robust sigma_0 for curvatures 0167 eso_k = median(abs(v(Np+1:end)))*1.48; 0168 if print_type > 1 0169 est_s0_s0p_s0k=[eso,eso_p,eso_k] 0170 end 0171 0172 % critical values 0173 kp = k*eso_p; 0174 kk = k*eso_k; 0175 0176 if print_type > 1 0177 criticalvalue_kp_sigmap_kk_sigma_k=[kp,poi(1,4),kk,sigma_k] 0178 end 0179 end 0180 0181 0182 %% perform reqeighting on points or dem 0183 if L1 % for the first iterations 0184 % dem 0185 if type_robust == 1 0186 weights(Np+1:end) = min(1,1./(abs(v(Np+1:end))/kk+eps)); 0187 end 0188 % points 0189 if type_robust == 2 0190 weights(1:Np) = min(1,1./(abs(v(1:Np))/kp+eps)).*out_in(1:Np); 0191 end 0192 else 0193 if type_robust == 1 0194 weights(Np+1:end) = exp(-abs(v(Np+1:end)).^2/kk^2/2)+0.0001; 0195 end 0196 if type_robust == 2 0197 weights(1:Np) = (exp(-abs(v(1:Np)).^2/kp^2/2)+0.0001).*out_in(1:Np); 0198 end 0199 end 0200 0201 0202 if print_type > 0 0203 display(['Time for solution = ',num2str(toc)]) 0204 end 0205 0206 return