0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
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
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
0035 M = length(ts);
0036 A=zeros(M+N-2,N);
0037
0038
0039 xa=zeros(N,1);
0040 weights=ones(M+N-2,1);
0041 Cov=0;
0042
0043
0044 for iter = 1:Niter+1
0045
0046 if print_type > 0
0047 display(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 display(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
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 if print_type > 0
0083 norm_dx=[iter,norm(dx)];
0084 display(norm_dx)
0085 end
0086
0087 if iter == Niter+1 && print_type > 1
0088 xa_dx=[xa';dx'];
0089 display(xa_dx)
0090 end
0091 v = A*dx-b;
0092
0093
0094 if print_type > 0
0095 display('adapt weights')
0096 display([iter,rob_p_d])
0097 sv = median(abs(v(M+1:M+N-2)))*1.48;
0098 display(sv)
0099 end
0100
0101 if iter < Niter
0102
0103 if type_out == 0
0104
0105 k=3;
0106 if iter < 4
0107 if rob_p_d == 2 || rob_p_d == 3
0108 weights(1:M) = min(1,1./abs(v(1:M))/k+0.0001);
0109 else
0110 weights(M+1:M+N-2) = min(1,1./abs(v(M+1:M+N-2))/k+0.0001);
0111 end
0112 else
0113 if rob_p_d == 2 || rob_p_d == 3
0114 weights(1:M) = exp(-abs(v(1:M)).^2/k^2);
0115 else
0116 weights(M+1:M+N-2) = exp(-abs(v(M+1:M+N-2)).^2/(k^2*sv^2));
0117 end
0118 end
0119
0120 else
0121
0122 k = 1;
0123 g = g_factor*k;
0124 w = w_factor*k;
0125
0126 if type_robust == 0
0127 weights(1:M) = max(min(1,1./abs(v(1:M))/k+0.0001),((sign(v(1:M))/k)+1)/2);
0128
0129 else
0130 for m = 1:M
0131 if v(m) < g-w
0132 weights(m) = 0;
0133
0134 elseif v(m) > k
0135 weights(m)=1;
0136
0137 else
0138 weights(m)=1/(1+(v(m)-g)^4);
0139
0140 end
0141 end
0142 end
0143 end
0144 else
0145
0146 weights(1:M) = weights(1:M)>0.5;
0147 if iter == Niter
0148 xa = zeros(N,1);
0149 end
0150 end
0151
0152 end
0153
0154 xest = xa;
0155
0156