estimate_profile_robust_flat_smooth

% estimate_profile_robust_flat_smooth

 xest = estimate_profile_robust_flat_smooth(N,fs,ts,ys,
          sigma_e1,sigma_e2,sigma_n,Niter,type_out,type_r,print_type)

 N        = scalar, length of profile
 fs       = Nx1 vector 1,2
 ts,ys    = Mx1 vectors, observed profile
 sigma_e  = std of curvature
 sigma_n  = std of observations
 Niter    = number of iterations
 type_out = type of outlier (0=symm, 1=asymm)
 type_r   (1) > 0 = L1, 1 = Kraus 
          (2) = g for Kraus
          (3) = w for Kraus
          (4) = 1 only dem
                2 only points
                3 both
 print_type   = 0,1,2 (no, low, high)

 Wolfgang Förstner 2014-10-05
 last changes: Susanne Wenzel 09/16
 wfoerstn@uni-bonn.de, wenzel@igg.uni-bonn.de

0001 %% estimate_profile_robust_flat_smooth
0002 %
0003 % xest = estimate_profile_robust_flat_smooth(N,fs,ts,ys,
0004 %          sigma_e1,sigma_e2,sigma_n,Niter,type_out,type_r,print_type)
0005 %
0006 % N        = scalar, length of profile
0007 % fs       = Nx1 vector 1,2
0008 % ts,ys    = Mx1 vectors, observed profile
0009 % sigma_e  = std of curvature
0010 % sigma_n  = std of observations
0011 % Niter    = number of iterations
0012 % type_out = type of outlier (0=symm, 1=asymm)
0013 % type_r   (1) > 0 = L1, 1 = Kraus
0014 %          (2) = g for Kraus
0015 %          (3) = w for Kraus
0016 %          (4) = 1 only dem
0017 %                2 only points
0018 %                3 both
0019 % print_type   = 0,1,2 (no, low, high)
0020 %
0021 % Wolfgang Förstner 2014-10-05
0022 % last changes: Susanne Wenzel 09/16
0023 % wfoerstn@uni-bonn.de, wenzel@igg.uni-bonn.de
0024 
0025 function [xest,A,v,weights,Cov] = estimate_profile_robust_flat_smooth(...
0026     N,fs,ts,ys,sigma_e1,sigma_e2,sigma_n,Niter,type_out,type_r,print_type)
0027 
0028 % robust
0029 type_robust=type_r(1);
0030 g_factor   =type_r(2);
0031 w_factor   =type_r(3);
0032 rob_p_d    =type_r(4);
0033 
0034 % Number of observations
0035 M = length(ts);
0036 A=zeros(M+N-2,N);
0037 
0038 % initialize
0039 xa=zeros(N,1);
0040 weights=ones(M+N-2,1);
0041 Cov=0;
0042 
0043 %% robust estimation
0044 for iter = 1:Niter+1
0045     
0046     if print_type > 0
0047         disp(iter)
0048     end
0049     
0050     b = zeros(M+N-1,1);
0051     for m = 1:M
0052         w = 1/sigma_n*sqrt(weights(m));
0053         A(m,ts(m)) = w;
0054         dl      = ys(m)-xa(ts(m));
0055         b(m)    = dl*w;
0056     end
0057     
0058     if iter == Niter+1 && print_type > 1
0059         rhs_w = [b(1:M)';weights(1:M)'];
0060         disp(rhs_w)
0061     end
0062     
0063     k = M;
0064     for n = 1:N-1
0065         k = k+1;
0066         if n == 1 || fs(n) == 1 
0067             A(k,n:n+1)   = [1 -1]/sigma_e1;
0068         else
0069             A(k,n-1:n+1) = [1 -2 1]/sigma_e2;
0070         end
0071     end
0072     
0073     % normal system
0074     Nmatrix = A'*A;
0075     nvector = A'*b;
0076     if N <= 1000 
0077         Cov = inv(Nmatrix);
0078     end
0079     dx = Nmatrix\nvector;
0080     xa = xa+dx;
0081     
0082     
0083     if print_type > 0
0084         norm_dx=[iter,norm(dx)];
0085         disp(norm_dx)
0086     end
0087     
0088     if iter == Niter+1 && print_type > 1
0089         xa_dx=[xa';dx'];
0090         disp(xa_dx)        
0091     end
0092     v = A*dx-b;
0093     
0094     %% adapt weights
0095     if print_type > 0
0096         disp('adapt weights')
0097         disp([iter,rob_p_d])
0098         sv = median(abs(v(M+1:M+N-2)))*1.48;
0099         disp(sv)
0100     end
0101     
0102     if iter < Niter
0103 
0104         if type_out == 0
0105         
0106             k=3;
0107             if iter < 4
0108                 if rob_p_d == 2 || rob_p_d == 3
0109                     weights(1:M) = min(1,1./abs(v(1:M))/k+0.0001);
0110                 else
0111                     weights(M+1:M+N-2) = min(1,1./abs(v(M+1:M+N-2))/k+0.0001);
0112                 end
0113             else
0114                 if rob_p_d == 2 || rob_p_d == 3
0115                     weights(1:M) = exp(-abs(v(1:M)).^2/k^2);
0116                 else
0117                     weights(M+1:M+N-2) = exp(-abs(v(M+1:M+N-2)).^2/(k^2*sv^2));
0118                 end
0119             end
0120             
0121         else  % asymmetric L1 or Kraus/Pfeifer
0122             
0123             k = 1;
0124             g = g_factor*k;
0125             w = w_factor*k;
0126             
0127             if type_robust == 0
0128                 weights(1:M) = max(min(1,1./abs(v(1:M))/k+0.0001),((sign(v(1:M))/k)+1)/2);
0129                 
0130             else
0131                 for m = 1:M
0132                     if v(m) < g-w
0133                         weights(m) = 0;
0134                         
0135                     elseif v(m) > k
0136                         weights(m)=1;
0137                         
0138                     else
0139                         weights(m)=1/(1+(v(m)-g)^4);
0140                         
0141                     end
0142                 end
0143             end
0144         end
0145     else
0146         
0147         weights(1:M) = weights(1:M)>0.5;
0148         if iter == Niter
0149             xa = zeros(N,1);
0150         end
0151     end
0152     
0153 end
0154 
0155 xest = xa;
0156 
0157