DynamicSeq_DS_function

 Dantzig Selector with sequential measurements using homotopy.
 
 Solves  
 min_x  ||x||_1 s.t. ||A'(Ax-y)+b'(bx-w)||_\infty <= tau

 using homotopy update method
 [xp_h, lambda_h, gamma_xh, gamma_lh, iter, th] =
 DynamicSeq_DS_function(A, b, Q_glgx, R_glgx, y, w, xp, lame, gamma_x, gamma_lambda, pk, ak, tau, maxiter)

 A: mxn matrix
 b: new rows in measurement matrix
 [Q_glgx, R_glgx] = qr factors for A_glgx
 y: old set of measurements
 w: new measurements
 xp: old primal solution
 lame: old dual solution
 gamma_x: old primal support
 gamma_lambda: old dual support
 pk: old primal constraints
 ak: old dual constraints
 tau: threshold parameter.

0001 % Dantzig Selector with sequential measurements using homotopy.
0002 %
0003 % Solves
0004 % min_x  ||x||_1 s.t. ||A'(Ax-y)+b'(bx-w)||_\infty <= tau
0005 %
0006 % using homotopy update method
0007 % [xp_h, lambda_h, gamma_xh, gamma_lh, iter, th] =
0008 % DynamicSeq_DS_function(A, b, Q_glgx, R_glgx, y, w, xp, lame, gamma_x, gamma_lambda, pk, ak, tau, maxiter)
0009 %
0010 % A: mxn matrix
0011 % b: new rows in measurement matrix
0012 % [Q_glgx, R_glgx] = qr factors for A_glgx
0013 % y: old set of measurements
0014 % w: new measurements
0015 % xp: old primal solution
0016 % lame: old dual solution
0017 % gamma_x: old primal support
0018 % gamma_lambda: old dual support
0019 % pk: old primal constraints
0020 % ak: old dual constraints
0021 % tau: threshold parameter.
0022 
0023 % Dantzig selector solution dynamica update with sequential measurements
0024 % Rank one update is based on qr factorization.
0025 
0026 % Author: Salman Asif, Georgia Tech.
0027 % Email: sasif@ece.gatech.edu
0028 % Created: June 2009
0029 %
0030 %-------------------------------------------+
0031 % Copyright (c) 2009.  Muhammad Salman Asif
0032 %-------------------------------------------+
0033 
0034 function [xp_h, lambda_h, gamma_xh, gamma_lh, iter, th] = DynamicSeq_DS_function(A, b, Q_glgx, R_glgx, y, w, xp, lame, gamma_x, gamma_lambda, pk, ak, tau, maxiter)
0035 
0036 t0 = cputime;
0037 N = length(xp);
0038 e0 = 0;
0039 gamma_xh = gamma_x;
0040 gamma_lh = gamma_lambda;
0041 xp_h = xp;
0042 lambda_h = lame;
0043 idelta = [];
0044 itheta = [];
0045 epsilon = 0;
0046 pk_old = pk;
0047 pk_old(gamma_lh) = sign(pk_old(gamma_lh))*tau;
0048 ak_old = ak;
0049 ak_old(gamma_xh) = sign(ak_old(gamma_xh));
0050 out_lambda = [];
0051 out_x = [];
0052 in_x = [];
0053 in_lambda = [];
0054 
0055 iter = 0;
0056 
0057 while iter < maxiter
0058     iter = iter+1;
0059    
0060     e0h = e0; % making e0h==1 workes with BPDN but NOT with DS.
0061     % The reason for this is because the value of step size is not
0062     % guaranteed to stay positive.
0063     
0064     dx = b(:,gamma_lh)'*(b(:,gamma_xh)*xp_h(gamma_xh)-w);
0065     
0066     %%% NEED FAST UPDATE
0067     % iAglgx = inv(A(:,gamma_lh)'*A(:,gamma_xh)+e0h*b(:,gamma_lh)'*b(:,gamma_xh));
0068     % ux = (b(:,gamma_xh)*iAglgx*b(:,gamma_lh)');
0069     % delx = -iAglgx*dx;
0070     
0071     iAb_vec = R_glgx\(Q_glgx'*b(:,gamma_lh)');
0072     ux = (b(:,gamma_xh)*iAb_vec);
0073     delx = -R_glgx\(Q_glgx'*dx);
0074 
0075     pk = pk_old; % A'*(A*xp_h-y)+e0*b'*(b*xp_h-w);
0076     dk = b'*(b*xp_h-w)+(A'*(A(:,gamma_xh)*delx)+e0h*b'*(b(:,gamma_xh)*delx));
0077     delx_vec = zeros(N,1);
0078     delx_vec(gamma_xh) = delx;
0079 
0080     dl = b(:,gamma_xh)'*(b(:,gamma_lh)*lambda_h(gamma_lh));
0081     ul = ux;
0082    
0083     %%% NEED FAST UPDATE
0084     % iAgxgl = iAglgx';
0085     % del_lam = -iAgxgl*dl;
0086      
0087     del_lam = -Q_glgx*(R_glgx'\dl);
0088         
0089     ak = ak_old; %A'*A*lambda_h+e0*b'*b*lambda_h;
0090     bk = b'*(b*lambda_h)+(A'*(A(:,gamma_lh)*del_lam)+e0h*b'*(b(:,gamma_lh)*del_lam));
0091     del_lam_vec = zeros(N,1);
0092     del_lam_vec(gamma_lh) = del_lam;
0093 
0094     % gamma_lc = setdiff([1:N],[gamma_lh]);
0095     temp_gamma = zeros(N,1);
0096     temp_gamma(gamma_lh) = gamma_lh;
0097     gamma_lc = find([1:N]' ~= temp_gamma);
0098 
0099     d_constr1 = (tau-pk(gamma_lc))./dk(gamma_lc);
0100     d_constr2 = (tau+pk(gamma_lc))./-dk(gamma_lc);
0101     d_constr3 = (-xp_h(gamma_xh)./delx_vec(gamma_xh));
0102     idelta_1 = find(d_constr1>2*eps);
0103     idelta_2 = find(d_constr2>2*eps);
0104     idelta_3 = find(d_constr3>2*eps);
0105     delta1 = min(d_constr1(idelta_1));
0106     delta2 = min(d_constr2(idelta_2));
0107     delta3 = min(d_constr3(idelta_3));
0108     if isempty(delta1)
0109         delta1 = inf;
0110     end
0111     if isempty(delta2)
0112         delta2 = inf;
0113     end
0114     if isempty(delta3)
0115         delta3 = inf;
0116     end
0117 
0118     if delta1>delta2
0119         delta = delta2;
0120         idelta = gamma_lc(find(d_constr2==delta2));
0121         flag_delta = 1;
0122     else
0123         delta = delta1;
0124         idelta = gamma_lc(find(d_constr1==delta1));
0125         flag_delta = 1;
0126     end
0127     if delta3 < delta
0128         delta = delta3;
0129         idelta = gamma_xh(find(d_constr3==delta3));
0130         flag_delta = 0;
0131     end
0132     
0133     % gamma_xc = setdiff([1:N],[gamma_xh]);
0134     temp_gamma = zeros(N,1);
0135     temp_gamma(gamma_xh) = gamma_xh;
0136     gamma_xc = find([1:N]' ~= temp_gamma);
0137 
0138     b_constr1 = (1-ak(gamma_xc))./bk(gamma_xc);
0139     b_constr2 = (1+ak(gamma_xc))./-bk(gamma_xc);
0140     b_constr3 = (-lambda_h(gamma_lh)./del_lam_vec(gamma_lh));
0141     itheta_1 = find(b_constr1>2*eps);
0142     itheta_2 = find(b_constr2>2*eps);
0143     itheta_3 = find(b_constr3>2*eps);
0144     theta1 = min(b_constr1(itheta_1));
0145     theta2 = min(b_constr2(itheta_2));
0146     theta3 = min(b_constr3(itheta_3));
0147     if isempty(theta1)
0148         theta1 = inf;
0149     end
0150     if isempty(theta2)
0151         theta2 = inf;
0152     end
0153     if isempty(theta3)
0154         theta3 = inf;
0155     end
0156 
0157     if theta1>theta2
0158         theta = theta2;
0159         itheta = gamma_xc(find(b_constr2==theta2));
0160         flag_theta = 1;
0161     else
0162         theta = theta1;
0163         itheta = gamma_xc(find(b_constr1==theta1));
0164         flag_theta = 1;
0165     end
0166     if theta3 < theta
0167         theta = theta3;
0168         itheta = gamma_lh(find(b_constr3==theta3));
0169         flag_theta = 0;
0170     end
0171 
0172     if 1-theta*ul <=0 & ul >0
0173         stp = 1;
0174     end
0175     if 1-delta*ux <=0 & ux >0
0176         stp = 1;
0177     end
0178 
0179     epsilon_x = (delta+e0-e0h*delta*ux)/(1-delta*ux);
0180     epsilon_l = (theta+e0-e0h*theta*ul)/(1-theta*ul);
0181 
0182     dteta = min(delta, theta);
0183 
0184     if epsilon_l <0
0185         epsilon_l = inf;
0186     end
0187     if epsilon_x < 0
0188         epsilon_x = inf;
0189     end
0190 
0191     epsilon = min(epsilon_x,epsilon_l);
0192 
0193     if epsilon >=1
0194         dteta_end = ((1-e0)/(1+ux*(1-e0h)));
0195         xp_h = xp_h + dteta_end*delx_vec;
0196         lambda_h = lambda_h + dteta_end*del_lam_vec;
0197         break;
0198     end
0199 
0200     gamma_xh_old = gamma_xh;
0201     gamma_lh_old = gamma_lh;
0202     xp_h_old = xp_h;
0203     xp_h = xp_h+dteta*delx_vec;
0204     lambda_h_old = lambda_h;
0205     lambda_h = lambda_h+dteta*del_lam_vec;
0206     
0207     pk_t = pk+dteta*dk;
0208     ak_t = ak+dteta*bk;
0209 
0210     in_lambda = [];
0211     out_lambda = [];
0212     in_x = [];
0213     out_x = [];
0214     if epsilon_x > epsilon_l
0215         %%%%%%%%%%%%%%%%%%%%%
0216         %%% x post-update %%%
0217         %%%%%%%%%%%%%%%%%%%%%
0218         if flag_theta == 1
0219             % when an element is added to Gamma_x by selected
0220             % epsilon_dual
0221             new_x = itheta;
0222             gamma_xh_app = [gamma_xh; new_x];
0223             in_x = itheta;
0224             
0225             %%% UPDATE THE MATRIX AND ITS INVERSE HERE
0226             % just update with new epsilon
0227             Q0 = Q_glgx;
0228             R0 = R_glgx;
0229             [Q_glgx R_glgx] = qrupdate(Q0, R0, (epsilon-e0)*b(:,gamma_lh)',b(:,gamma_xh)');
0230         else
0231             % when an element is removed from Gamma_lambda by selected
0232             % epsilon_dual
0233             outl_index = find(gamma_lh == itheta);
0234             % gamma_lh = setdiff(gamma_lh,itheta);
0235             gamma_lh = gamma_lh([1:outl_index-1 outl_index+1:length(gamma_lh)]);
0236             
0237             % [max_val outx_index] = max(abs(iAglgx(:,outl_index)));
0238             outx_index = length(gamma_xh);
0239             new_x = gamma_xh(outx_index);
0240             % gamma_xh = setdiff(gamma_xh,new_x);
0241             gamma_xh = gamma_xh([1:outx_index-1 outx_index+1:length(gamma_xh)]);
0242             gamma_xh_app = [gamma_xh; new_x];
0243             
0244             lambda_h(itheta) = 0;
0245             out_lambda = itheta;
0246             
0247             %%% UPDATE THE MATRIX AND ITS INVERSE HERE
0248             % out_lambda and new_x removed, and new epsilon
0249             Q0 = Q_glgx;
0250             R0 = R_glgx;
0251             [Q1 R1] = qrupdate(Q0, R0, (epsilon-e0)*b(:,gamma_lh_old)',b(:,gamma_xh_old)');
0252             loc_vector = zeros(length(gamma_lh_old),1);
0253             loc_vector(outl_index) = 1;
0254             rep_row = (A(:,out_lambda)'*A(:,gamma_xh_old)+epsilon*b(:,out_lambda)'*b(:,gamma_xh_old))';
0255             [Q2t R2t] = qrupdate(Q1, R1, loc_vector,-rep_row);
0256             loc_vector = zeros(length(gamma_xh_old),1);
0257             loc_vector(outx_index) = 1;
0258             rep_vec = (A(:,gamma_lh_old)'*A(:,new_x)+epsilon*b(:,gamma_lh_old)'*b(:,new_x));
0259             rep_vec(outl_index) = 1;
0260             [Q3t R3t] = qrupdate(Q2t, R2t, -rep_vec, loc_vector);
0261             Q_glgx = Q3t([1:outl_index-1 outl_index+1:length(gamma_lh_old)],[1:outx_index-1 outx_index+1:length(gamma_xh_old)]);
0262             R_glgx = R3t([1:outx_index-1 outx_index+1:length(gamma_xh_old)],[1:outx_index-1 outx_index+1:length(gamma_xh_old)]);
0263         
0264             % [Q2 R2] = qrdelete(Q1, R1, outl_index,'row');
0265             % [Q_glgx R_glgx] = qrdelete(Q2, R2, outx_index, 'col');
0266         end
0267         z_x = -sign(ak_t(new_x));
0268     
0269         dx = ((A(:,gamma_lh)'*A(:,new_x)+epsilon*b(:,gamma_lh)'*b(:,new_x))*z_x);
0270         
0271         %%% NEED FAST UPDATE
0272         % iAglgx_btb = inv(A(:,gamma_lh)'*A(:,gamma_xh)+epsilon*b(:,gamma_lh)'*b(:,gamma_xh));
0273         % del_x_hat = -iAglgx_btb*dx;
0274 
0275         del_x_hat = -R_glgx\(Q_glgx'*dx);
0276 
0277         del_xhat_vec = zeros(N,1);
0278         del_xhat_vec(gamma_xh) = del_x_hat;
0279         del_xhat_vec(new_x) = z_x;
0280 
0281         dk_t = A'*(A*del_xhat_vec)+epsilon*b'*(b*del_xhat_vec);
0282         if flag_theta == 0
0283             if sign(pk_t(itheta))== sign(dk_t(itheta))
0284                 z_x = -z_x;
0285                 del_xhat_vec = -del_xhat_vec;
0286                 dk_t = -dk_t;
0287             end
0288         end
0289         pk_t(out_lambda) = sign(pk_t(out_lambda))*tau;
0290         % gamma_lc = setdiff([1:N],[gamma_lh]);
0291         temp_gamma = zeros(N,1);
0292         temp_gamma(gamma_lh) = gamma_lh;
0293         gamma_lc = find([1:N]' ~= temp_gamma);
0294 
0295         d_constr1 = (tau-pk_t(gamma_lc))./dk_t(gamma_lc);
0296         d_constr2 = (tau+pk_t(gamma_lc))./-dk_t(gamma_lc);
0297         d_constr3 = (-xp_h(gamma_xh_app)./del_xhat_vec(gamma_xh_app));
0298         idelta_1 = find(d_constr1>2*eps);
0299         idelta_2 = find(d_constr2>2*eps);
0300         idelta_3 = find(d_constr3>2*eps);
0301         delta1 = min(d_constr1(idelta_1));
0302         delta2 = min(d_constr2(idelta_2));
0303         delta3 = min(d_constr3(idelta_3));
0304 
0305         if delta1>delta2
0306             delta = delta2;
0307             idelta = gamma_lc(find(d_constr2==delta2));
0308             flag_delta = 1;
0309         else
0310             delta = delta1;
0311             idelta = gamma_lc(find(d_constr1==delta1));
0312             flag_delta = 1;
0313         end
0314         if delta3 < delta
0315             delta = delta3;
0316             idelta = gamma_xh_app(find(d_constr3==delta3));
0317             flag_delta = 0;
0318         end
0319         
0320         xp_h_old = xp_h;
0321         xp_h = xp_h + delta*del_xhat_vec;
0322 
0323         if flag_delta == 1
0324             gamma_xh_old = gamma_xh;
0325             gamma_lh_old = gamma_lh;
0326             gamma_lh = [gamma_lh; idelta];
0327             gamma_xh = gamma_xh_app;
0328             in_lambda = idelta;
0329             
0330             %%% UPDATE THE MATRIX AND ITS INVERSE HERE FOR
0331             % new_x and new_lambda added;
0332             Q0 = Q_glgx;
0333             R0 = R_glgx;
0334             Q0t = [Q0 zeros(length(gamma_xh_old),1); zeros(1,length(gamma_lh_old)) 1];
0335             R0t = [R0 zeros(length(gamma_xh_old),1); zeros(1,length(gamma_lh_old)) 1];
0336             loc_vector = zeros(length(gamma_lh),1);
0337             loc_vector(end) = 1;
0338             rep_row = (A(:,in_lambda)'*A(:,gamma_xh)+epsilon*b(:,in_lambda)'*b(:,gamma_xh))';
0339             [Q2t R2t] = qrupdate(Q0t, R0t, loc_vector, rep_row);
0340             rep_vec = A(:,gamma_lh)'*A(:,new_x)+epsilon*b(:,gamma_lh)'*b(:,new_x);
0341             rep_vec(end) = -1;
0342             [Q_glgx R_glgx] = qrupdate(Q2t, R2t, rep_vec, loc_vector);
0343 
0344             % [Q1, R1] = qrinsert(Q0, R0, length(gamma_lh), A(:,gamma_lh_old)'*A(:,new_x)+epsilon*b(:,gamma_lh_old)'*b(:,new_x),'col');
0345             % [Q_glgx, R_glgx] = qrinsert(Q1,R1, length(gamma_lh), A(:,in_lambda)'*A(:,gamma_xh)+epsilon*b(:,in_lambda)'*b(:,gamma_xh),'row');
0346         else
0347             gamma_xh_old = gamma_xh;
0348             gamma_lh_old = gamma_lh;
0349             
0350             % gamma_xh = setdiff(gamma_xh_app,idelta);
0351             outx_index = find(gamma_xh==idelta);
0352             out_x = idelta;
0353             xp_h(out_x) = 0;
0354             
0355             if ~isempty(outx_index) % Because otherwise new_x is removed
0356                 gamma_xh(outx_index) = new_x;
0357     
0358                 %%% UPDATE THE MATRIX AND ITS INVERSE HERE
0359                 % new_x and out_x swapped
0360                 Q0 = Q_glgx;
0361                 R0 = R_glgx;
0362                 loc_vector = zeros(length(gamma_xh),1);
0363                 loc_vector(outx_index) = 1;
0364                 [Q_glgx, R_glgx] = qrupdate(Q0,R0, A(:,gamma_lh)'*A(:,new_x)+epsilon*b(:,gamma_lh)'*b(:,new_x)-(A(:,gamma_lh)'*A(:,out_x)+epsilon*b(:,gamma_lh)'*b(:,out_x)),loc_vector);
0365             end
0366         end
0367 
0368         pk_old = pk_t+delta*dk_t;
0369         pk_old(gamma_lh) = sign(pk_old(gamma_lh))*tau;
0370         ak_old = ak_t;
0371         ak_old(gamma_xh_app) = sign(ak_old(gamma_xh_app));
0372     else 
0373         %%%%%%%%%%%%%%%%%%%%%%%%%%
0374         %%% lambda post-update %%%
0375         %%%%%%%%%%%%%%%%%%%%%%%%%%
0376         if flag_delta == 1
0377             % when an element is added to Gamma_lambda by selected
0378             % epsilon_primal
0379             new_lambda = idelta;
0380             gamma_lh_app = [gamma_lh; new_lambda];
0381             in_lambda = idelta;
0382             %%% UPDATE THE MATRIX AND ITS INVERSE HERE
0383             % just update the epsilon
0384             Q0 = Q_glgx;
0385             R0 = R_glgx;
0386             [Q_glgx R_glgx] = qrupdate(Q0, R0, (epsilon-e0)*b(:,gamma_lh)',b(:,gamma_xh)');
0387         else
0388             gamma_lh_old = gamma_lh;
0389             gamma_xh_old = gamma_xh;
0390             
0391             % when an element is removed from Gamma_x by selected
0392             % epsilon_primal
0393             outx_index = find(gamma_xh==idelta);
0394             % gamma_xh = setdiff(gamma_xh,idelta);
0395             gamma_xh = gamma_xh([1:outx_index-1 outx_index+1:length(gamma_xh)]);
0396             
0397             % [max_val outl_index] = max(abs(iAgxgl(:,outx_index)));
0398             outl_index = length(gamma_lh);
0399             new_lambda = gamma_lh(outl_index);
0400             % gamma_lh = setdiff(gamma_lh,new_lambda);
0401             gamma_lh = gamma_lh([1:outl_index-1 outl_index+1:length(gamma_lh)]);
0402             gamma_lh_app = [gamma_lh; new_lambda];
0403             xp_h(idelta) = 0;
0404             out_x = idelta;
0405             
0406             %%% UPDATE THE MATRIX AND ITS INVERSE HERE
0407             % out_x and new_lambda removed and new epsilon
0408             Q0 = Q_glgx;
0409             R0 = R_glgx;
0410             [Q1 R1] = qrupdate(Q0, R0, (epsilon-e0)*b(:,gamma_lh_old)',b(:,gamma_xh_old)');
0411             loc_vector = zeros(length(gamma_lh_old),1);
0412             loc_vector(outl_index) = 1;
0413             rep_row = (A(:,new_lambda)'*A(:,gamma_xh_old)+epsilon*b(:,new_lambda)'*b(:,gamma_xh_old))';
0414             [Q2t R2t] = qrupdate(Q1, R1, loc_vector,-rep_row);
0415             loc_vector = zeros(length(gamma_xh_old),1);
0416             loc_vector(outx_index) = 1;
0417             rep_vec = (A(:,gamma_lh_old)'*A(:,out_x)+epsilon*b(:,gamma_lh_old)'*b(:,out_x));
0418             rep_vec(outl_index) = 1; 
0419             [Q3t R3t] = qrupdate(Q2t, R2t, -rep_vec, loc_vector);
0420             Q_glgx = Q3t([1:outl_index-1 outl_index+1:length(gamma_lh_old)],[1:outx_index-1 outx_index+1:length(gamma_xh_old)]);
0421             R_glgx = R3t([1:outx_index-1 outx_index+1:length(gamma_xh_old)],[1:outx_index-1 outx_index+1:length(gamma_xh_old)]);
0422             
0423             % [Q2 R2] = qrdelete(Q1, R1, outl_index,'row');
0424             % [Q_glgx R_glgx] = qrdelete(Q2, R2, outx_index, 'col');
0425         end
0426         z_l = sign(pk_t(new_lambda));
0427         
0428         dl = ((A(:,gamma_xh)'*A(:,new_lambda)+epsilon*b(:,gamma_xh)'*b(:,new_lambda))*z_l);
0429         
0430         %%% NEED FAST UPDATE
0431         % iAgxgl_btb = inv(A(:,gamma_xh)'*A(:,gamma_lh)+epsilon*b(:,gamma_xh)'*b(:,gamma_lh));
0432         % del_lam_hat = -iAgxgl_btb*dl;
0433          
0434         del_lam_hat = -Q_glgx*(R_glgx'\dl);
0435         
0436         del_lhat_vec = zeros(N,1);
0437         del_lhat_vec(gamma_lh) = del_lam_hat;
0438         del_lhat_vec(new_lambda) = z_l;
0439 
0440         bk_t = A'*(A*del_lhat_vec)+epsilon*b'*(b*del_lhat_vec);
0441         if flag_delta == 0
0442             if sign(ak_t(idelta))==sign(bk_t(idelta))
0443                 z_l = -z_l;
0444                 del_lhat_vec = -del_lhat_vec;
0445                 bk_t = -bk_t;
0446             end
0447         end
0448         ak_t(out_x) = sign(ak_t(out_x));
0449         % gamma_xc = setdiff([1:N],[gamma_xh]);
0450         temp_gamma = zeros(N,1);
0451         temp_gamma(gamma_xh) = gamma_xh;
0452         gamma_xc = find([1:N]' ~= temp_gamma);
0453 
0454         b_constr1 = (1-ak_t(gamma_xc))./bk_t(gamma_xc);
0455         b_constr2 = (1+ak_t(gamma_xc))./-bk_t(gamma_xc);
0456         b_constr3 = (-lambda_h(gamma_lh_app)./del_lhat_vec(gamma_lh_app));
0457         itheta_1 = find(b_constr1>2*eps);
0458         itheta_2 = find(b_constr2>2*eps);
0459         itheta_3 = find(b_constr3>2*eps);
0460         theta1 = min(b_constr1(itheta_1));
0461         theta2 = min(b_constr2(itheta_2));
0462         theta3 = min(b_constr3(itheta_3));
0463 
0464         if theta1>theta2
0465             theta = theta2;
0466             itheta = gamma_xc(find(b_constr2==theta2));
0467             flag_theta = 1;
0468         else
0469             theta = theta1;
0470             itheta = gamma_xc(find(b_constr1==theta1));
0471             flag_theta = 1;
0472         end
0473         if theta3 < theta
0474             theta = theta3;
0475             itheta = gamma_lh_app(find(b_constr3==theta3));
0476             flag_theta = 0;
0477         end
0478 
0479         lambda_h_old = lambda_h;
0480         lambda_h = lambda_h + theta*del_lhat_vec;
0481         if flag_theta == 1
0482             gamma_lh_old = gamma_lh;
0483             gamma_xh_old = gamma_xh;
0484             gamma_xh = [gamma_xh; itheta];
0485             gamma_lh = gamma_lh_app;
0486             in_x = itheta;
0487             
0488             %%% UPDATE THE MATRIX AND ITS INVERSE HERE
0489             % new_lambda and in_x added
0490             Q0 = Q_glgx;
0491             R0 = R_glgx;
0492             Q0t = [Q0 zeros(length(gamma_xh_old),1); zeros(1,length(gamma_lh_old)) 1];
0493             R0t = [R0 zeros(length(gamma_xh_old),1); zeros(1,length(gamma_lh_old)) 1];
0494             loc_vector = zeros(length(gamma_lh),1);
0495             loc_vector(end) = 1;
0496             rep_row = (A(:,new_lambda)'*A(:,gamma_xh)+epsilon*b(:,new_lambda)'*b(:,gamma_xh))';
0497             [Q2t R2t] = qrupdate(Q0t, R0t, loc_vector, rep_row);
0498             rep_vec = A(:,gamma_lh)'*A(:,in_x)+epsilon*b(:,gamma_lh)'*b(:,in_x);
0499             rep_vec(end) = -1;
0500             [Q_glgx R_glgx] = qrupdate(Q2t, R2t, rep_vec, loc_vector);
0501 
0502             % [Q1, R1] = qrinsert(Q0, R0, length(gamma_xh), A(:,gamma_lh_old)'*A(:,in_x)+epsilon*b(:,gamma_lh_old)'*b(:,in_x),'col');
0503             % [Q_glgx, R_glgx] = qrinsert(Q1,R1, length(gamma_xh), A(:,new_lambda)'*A(:,gamma_xh)+epsilon*b(:,new_lambda)'*b(:,gamma_xh),'row');
0504         else
0505             % gamma_lh = setdiff(gamma_lh_app,itheta);
0506             outl_index = find(gamma_lh==itheta);
0507             out_lambda = itheta;
0508             
0509             out_lambda = itheta;
0510             lambda_h(itheta) = 0;
0511             if ~isempty(outl_index) % Because otherwise new_lambda is removed
0512                 gamma_lh_old = gamma_lh;
0513                 gamma_xh_old = gamma_xh;
0514                 gamma_lh(outl_index) = new_lambda; 
0515 
0516                 %%% UPDATE THE MATRIX AND ITS INVERSE HERE
0517                 % new_lambda and out_lambda swapped
0518                 Q0 = Q_glgx;
0519                 R0 = R_glgx;
0520                 loc_vector = zeros(length(gamma_xh),1);
0521                 loc_vector(outl_index) = 1;
0522                 row_replaced =  (A(:,new_lambda)'*A(:,gamma_xh)+epsilon*b(:,new_lambda)'*b(:,gamma_xh)-(A(:,out_lambda)'*A(:,gamma_xh)+epsilon*b(:,out_lambda)'*b(:,gamma_xh)));
0523                 [Q_glgx, R_glgx] = qrupdate(Q0,R0, loc_vector,row_replaced');
0524             end
0525 
0526         end
0527         ak_old = ak_t+theta*bk_t;
0528         ak_old(gamma_xh) = sign(ak_old(gamma_xh));
0529         pk_old = pk_t;
0530         pk_old(gamma_lh_app) = sign(pk_old(gamma_lh_app))*tau;
0531     end
0532     e0 = epsilon;
0533 end
0534 th = cputime-t0;