atfirstturntrajectory

 [
 rclosed,...           1) AT lattice with closed trajectory
 inCOD...              2) 6x1 input coordinate updated
 ]=atfirstturntrajectory(...
     ropen,...         1) AT lattice
     inCOD,...         2) 6x1 input coordinate
     indBPM,...        3) bpm indexes
     indHCor,...       4) h. cor indexed
     indVCor,...       5) v. cor indexed
     lim,...           6) maximum bpm reading (lim+2e-3m for extra search)
                          (default: 4e-3m)
     maxcornum,...     7) maximum correctors to use for correction
                          (only last maxcornum correctors and BPMs are used)
     correctflags,...  8) correct [dpp mean0](default: [true true])
     ModelRM,...(r0)   9) output of getresponsematrice
                          (no default. if no rm has been computed, this
                          argument can be r0, an AT lattice cell array of
                          structures without errors to use for
                          computation of the RM.
     reforbit,...     10) reference trajectory 2xnbpm (default:0)
     steererlimit,... 11) 2x1 limit of steerers abs(steerer)<steererlimit
                          (default: [], no limits)
     printouttext     12) print text ( default: true)
     )

 finds closed trajectory:
 correct trajectory with available BPM and correctors
 if not found, increase amplitude limit for bpm reading
 if still not found scan injection point for maximum number of turns
 if all bpm see signal, close orbit using the last 2 correctors in the
 lattice.

see also: findrespmat findtrajectory6Err getresponsematrices

0001 function [rclosed,inCOD]=atfirstturntrajectory(...
0002     ropen,...
0003     inCOD,...
0004     indBPM,...
0005     indHCor,...
0006     indVCor,...
0007     lim,...
0008     maxcornum,...
0009     correctflags,...
0010     ModelRM,...
0011     reforbit,...
0012     steererlimit,...
0013     printouttext)
0014 % [
0015 % rclosed,...           1) AT lattice with closed trajectory
0016 % inCOD...              2) 6x1 input coordinate updated
0017 % ]=atfirstturntrajectory(...
0018 %     ropen,...         1) AT lattice
0019 %     inCOD,...         2) 6x1 input coordinate
0020 %     indBPM,...        3) bpm indexes
0021 %     indHCor,...       4) h. cor indexed
0022 %     indVCor,...       5) v. cor indexed
0023 %     lim,...           6) maximum bpm reading (lim+2e-3m for extra search)
0024 %                          (default: 4e-3m)
0025 %     maxcornum,...     7) maximum correctors to use for correction
0026 %                          (only last maxcornum correctors and BPMs are used)
0027 %     correctflags,...  8) correct [dpp mean0](default: [true true])
0028 %     ModelRM,...(r0)   9) output of getresponsematrice
0029 %                          (no default. if no rm has been computed, this
0030 %                          argument can be r0, an AT lattice cell array of
0031 %                          structures without errors to use for
0032 %                          computation of the RM.
0033 %     reforbit,...     10) reference trajectory 2xnbpm (default:0)
0034 %     steererlimit,... 11) 2x1 limit of steerers abs(steerer)<steererlimit
0035 %                          (default: [], no limits)
0036 %     printouttext     12) print text ( default: true)
0037 %     )
0038 %
0039 % finds closed trajectory:
0040 % correct trajectory with available BPM and correctors
0041 % if not found, increase amplitude limit for bpm reading
0042 % if still not found scan injection point for maximum number of turns
0043 % if all bpm see signal, close orbit using the last 2 correctors in the
0044 % lattice.
0045 %
0046 %see also: findrespmat findtrajectory6Err getresponsematrices
0047 
0048 % features to implement
0049 % average correctors to zero
0050 % dpp correction
0051 % steerers limits
0052 % reference orbit
0053 % limit number of bpm/correctors to use default to all
0054 
0055 % features implemented
0056 % actions if stuck at a given bpm
0057 %       - scan for optimal inCOD
0058 %       - increase limit of readable bpm signal
0059 %       - reduce number of eigenvectors
0060 %
0061 
0062 % defaults
0063 if nargin<12
0064     printouttext=true;
0065 end
0066 
0067 if nargin<2
0068     inCOD=[0 0 0 0 0 0]';
0069 end
0070 if nargin<3
0071     if printouttext
0072         disp('get BPM and Correctors indexes');end;
0073     indBPM=find(atgetcells(ropen,'Class','Monitor'));
0074 end
0075 if nargin<4
0076     indHCor=find(atgetcells(ropen,'iscorH','H'));
0077 end
0078 if nargin<5
0079     indVCor=find(atgetcells(ropen,'iscorV','V'));
0080 end
0081 if nargin<6
0082     lim=4e-3;
0083 end
0084 if nargin<7
0085     maxcornum=length(indBPM);
0086 end
0087 if nargin<8
0088     correctflags=[true true];
0089 end
0090 
0091 if nargin<10
0092     reforbit=zeros(2,length(indBPM));
0093 end
0094 
0095 if nargin<11
0096     steererlimit=[];
0097 end
0098 
0099 if iscell(ModelRM)
0100     r0=ModelRM; % if RM to be computed, provide r0 lattice without errors
0101 end
0102 
0103 traj=0; % initial correction uses model lattice
0104 % if needed will switch to traj=1 during the loop.
0105 if printouttext
0106     if traj==1
0107         disp('correcting trajectory');
0108     elseif traj==0
0109         disp('correcting trajectory with model rm')
0110     end
0111 end
0112 
0113 % load  response matrices or compute them
0114 if correctflags(1) % dpp correction
0115     rmsel=[4 5 6];
0116 else
0117     rmsel=[4 5];
0118 end
0119 
0120 if iscell(ModelRM)
0121     if printouttext
0122         disp('computing model trajectory RM')
0123     end
0124     
0125     ModelRM=getresponsematrices(...
0126         r0,...          %1 AT lattice
0127         indBPM,...      %2 bpm indexes in at lattice
0128         indHCor,...     %3 h cor indexes
0129         indVCor,...     %4 v cor indexes
0130         [],...     %5 skew cor indexes
0131         [],...     %6 quad cor indexes
0132         [],...
0133         inCOD,...       %7 initial coordinates
0134         rmsel...        %8 specifiy rm to be computed
0135         );
0136     
0137     if ~correctflags(1) % dpp correction
0138         ModelRM.TrajHDPP{1}=[];
0139         ModelRM.TrajVDPP{3}=[];
0140     end
0141     
0142     if printouttext
0143         disp('computed model trajectory RM')
0144     end
0145     
0146 end
0147 
0148 
0149 
0150 RMH=ModelRM.TrajHCor{1};
0151 RMV=ModelRM.TrajVCor{3};
0152 RMHd=ModelRM.TrajHDPP;
0153 RMVd=ModelRM.TrajVDPP;
0154 
0155 
0156 rclosed=[];
0157 
0158 nbpmuprev=0;
0159 nbpmu=0; % bpm used in the correction
0160 
0161 lim1=lim(1);
0162 
0163 countiter=0;
0164 countstack=0;% chack that correction did not get stack
0165 
0166 countatbpm=zeros(size(indBPM));
0167 fraceig=2;
0168 Nforcor=maxcornum;
0169 
0170 
0171 % loop until a closed orbit is not found
0172 while isempty(rclosed)
0173     if printouttext
0174         disp('Search closed orbit');
0175     end
0176     if countstack>6 % 6 iterations are limit to continue.
0177         error(['correction of open trjectory looping at ' num2str(nbpmu) ' bpms'])
0178     end
0179     
0180     % check for closed orbit,
0181     % if found, end loop,
0182     % if not found but all bpm, close with last 2 H/V steerers
0183     if (nbpmu==length(indBPM))
0184         if printouttext
0185             disp(['Trajectory closure with last 2 correctors']);
0186         end
0187         
0188         ropen=MatchLast2CorForFirstBPM(ropen, inCOD,...
0189                                        indBPM, indHCor, indVCor);
0190         
0191         COD=findorbit6Err(ropen,indBPM,inCOD);
0192         
0193         if ~isempty(find(isnan(COD),1))
0194             if printouttext
0195                 disp('no closed orbit, but all bpm see signal. Closing with last 2 correctors.'); end;
0196             
0197             rclosed=MatchLast2CorForFirstBPM(ropen, inCOD,...
0198                                              indBPM, indHCor, indVCor);
0199         end
0200         
0201         if isempty(find(isnan(COD),1))
0202             if printouttext
0203                 disp('Found closed orbit'); end;
0204             rclosed=ropen; % end loop!
0205         end
0206         
0207     else % less then All BPM see signal ( 1 turn not completed)
0208         
0209         countiter=countiter+1;
0210         
0211         % get best input coordinates (septum tuning)
0212         %disp('Scan input coordinates')
0213         %[inCOD,nelpas]=ScanPosAngle(ropen,r0,indBPM,inCOD,51,3e-3,'scanInCOD');
0214         %disp(['Best Input Coordinates: [' num2str(inCOD*1e3,'%.2d, ') '] mm, ' num2str(nelpas) ' elem'])
0215         
0216         % get current orbit
0217         [t]=findtrajectory6Err(ropen,indBPM,inCOD);
0218         
0219         ox=t(1,:)-reforbit(1,:);
0220         oy=t(3,:)-reforbit(2,:);
0221         
0222         % bpm at wich there is a signal (not NaN and less than 10mm)
0223         usebpm=~isnan(ox) & ~isnan(oy) & ox<lim(1) & ox>-lim(1) & oy<lim(1) & oy>-lim(1);
0224         
0225         %  % stop at first bpm giving NAN
0226         %  % usebpm=1:(find(isnan(ox) || isnan(oy),1,'first')-1);
0227         
0228         % restrict to smaller initial region
0229         usebpm=indBPM(usebpm)<=max(indBPM(usebpm))*1;
0230         
0231         if countiter>1
0232             nbpmuprev=nbpmu;% store previous value
0233         end
0234         
0235         nbpmu=length(find(usebpm));
0236         
0237         
0238         if nbpmuprev==nbpmu % no more bpm then previous correction
0239             if countstack>1
0240                 lim1=lim1+0.5e-3; % accept less bpm
0241                 if printouttext
0242                     disp(['no improvement from last step. increasing lim to: ' num2str(lim1)]); end;
0243                 usebpm=~isnan(ox) & ~isnan(oy) & ox<lim1 & ox>-lim1 & oy<lim1 & oy>-lim1;
0244                 
0245                 if lim1>lim+2e-3
0246                     if printouttext
0247                         warning('Could not find closed orbit. Trajectory above 6mm. Aborting trajectory correction'); end;
0248                     warning('Closed orbit NOT FOUND');
0249                     rclosed=ropen;
0250                     return
0251                 end
0252                 
0253                 if lim1>lim+1e-3 && numel(find(usebpm))<320
0254                     if printouttext
0255                         disp('update optimal injection point'); end;
0256                     [inCOD]=Scan2x2DinCOD(ropen,inCOD,101,lim1,[]);
0257                     
0258                 end
0259                 
0260                 
0261             end
0262             countstack=countstack+1;
0263             
0264             
0265             % actions if number of corrector is stuck for >3 iterations
0266             if countstack==3
0267                 traj=0;
0268                 fraceig=5;% 1/5 eigenvectors
0269             end
0270             if countstack==4
0271                 traj=0;
0272                 fraceig=10; % 1/10 eigenvectors
0273             end
0274             if countstack==5
0275                 traj=1;% recompute trajectory rm
0276                 if printouttext
0277                     disp('setting correction to measured trajectory response matrix.');end;
0278                 fraceig=7; % 1/7 eigenvectors
0279             end
0280             if countstack==6
0281                 traj=1;% recompute trajectory rm
0282                 if printouttext
0283                     disp('setting correction to measured trajectory response matrix.'); end;
0284                 fraceig=10;% 1/10 eigenvectors
0285             end
0286             
0287         else
0288             countstack=0;
0289             
0290         end
0291         
0292         if countstack==10
0293             error('Could not find closed orbit. Aborting trajectory correction');
0294         end
0295         
0296         nbpmu=length(find(usebpm));
0297         if nbpmu==0
0298             error('NO BEAM AT ALL!')
0299         end
0300         
0301         % all available correctors before last bpm reading signal
0302         usecorH=indHCor<max(indBPM(usebpm));
0303         usecorV=indVCor<max(indBPM(usebpm));
0304         
0305         nhcu=length(find(usecorH));
0306         nvcu=length(find(usecorV));
0307         
0308         countatbpm(nbpmu)=countatbpm(nbpmu)+1;
0309         
0310         if nhcu==0 || nvcu==0
0311             error('NO BEAM AT ALL!')
0312         end
0313         
0314         if printouttext
0315             disp(['Trajectory correction:'...
0316                 ' nbpms= ' num2str(nbpmu)...
0317                 ' ncor: ' num2str([nhcu nvcu],'%d, ')]);
0318         end
0319         
0320         % limit to last correctors and bpms
0321         
0322         lastbpm=find(usebpm==1,1,'last');
0323         if lastbpm>Nforcor
0324             usebpm(1:lastbpm-Nforcor)=0;
0325         end
0326         
0327         lastcor=find(usecorH==1,1,'last');
0328         if lastcor>Nforcor
0329             usecorH(1:lastcor-Nforcor)=0;
0330         end
0331         
0332         lastcor=find(usecorV==1,1,'last');
0333         if lastcor>Nforcor
0334             usecorV(1:lastcor-Nforcor)=0;
0335         end
0336         
0337         if printouttext
0338             disp('computing ORM for available trajectory');
0339         end
0340         
0341         bpm=indBPM(usebpm);
0342         
0343         X=ox(usebpm)';
0344         Y=oy(usebpm)';
0345         
0346         HK=indHCor(usecorH);
0347         VK=indVCor(usecorV);
0348         
0349         neigSteerer=floor(length(HK)/fraceig);
0350         fracapply=1;
0351         
0352         % ----- CORRECTION ----- %
0353         
0354         if printouttext
0355             disp('H PLANE');
0356         end;
0357         corh0=getcellstruct(ropen,'PolynomB',HK,1,1);
0358         
0359         if traj==1
0360             
0361             ModelRM=getresponsematrices(...
0362                 ropen,...          %1 AT lattice
0363                 bpm,...      %2 bpm indexes in at lattice
0364                 HK,...     %3 h cor indexes
0365                 [],...     %4 v cor indexes
0366                 [],...     %5 skew cor indexes
0367                 [],...     %6 quad cor indexes
0368                 [],...
0369                 inCOD,...       %7 initial coordinates
0370                 rmsel...        %8 specifiy rm to be computed
0371                 );
0372             if ~correctflags(1) % dpp correction
0373                 ModelRM.TrajHDPP{1}=[];
0374             end
0375             RespH=ModelRM.TrajHCor{1};
0376             RespHd=ModelRM.TrajHDPP;
0377         elseif traj==0
0378             RespH=RMH(usebpm,usecorH);
0379             if correctflags(1) % dpp correction
0380                 RespHd=RMHd(usebpm);
0381             end
0382         end
0383         
0384         % build RMs
0385         if      correctflags(1) &&  correctflags(2) % dpp and mean0
0386             FRH=[ [RespH;ones(1,size(RespH,2))] [RespHd';0] ];
0387         elseif  correctflags(1) && ~correctflags(2) % dpp no mean 0
0388             FRH=[ RespH RespHd' ];
0389         elseif ~correctflags(1) &&  correctflags(2) % no dpp mean0
0390             FRH=[RespH;ones(1,size(RespH,2))];
0391         elseif ~correctflags(1) && ~correctflags(2) % no dpp no mean0
0392             FRH=RespH;
0393         end
0394         
0395         % if mean0 correction add 0 to correction vector
0396         if correctflags(2)
0397             dch=qemsvd_mod(FRH,[X;0],neigSteerer)*fracapply;
0398         else
0399             dch=qemsvd_mod(FRH,X,neigSteerer)*fracapply;
0400         end
0401         
0402         % if dpp correction separate dpp from correctors
0403         if correctflags(1)
0404             %    inCOD(5)=inCOD(5)+dch(end);
0405             dch=dch(1:end-1);
0406             deltacor=dch(end);
0407         end
0408         
0409         % limit steerers strengths
0410         if ~isempty(steererlimit)
0411             dch(abs(dch)>steererlimit(1))=steererlimit(1);
0412         end
0413         
0414         ropen=setcellstruct(ropen,'PolynomB',HK,corh0-dch,1,1);
0415         %ropen=atsetfieldvalues(ropen,indrfc,'Frequency',f0-alpha*(deltacor)*f0);%
0416         %alpha not computed since lattice is open.
0417             
0418         if printouttext
0419             disp('correcting available V trajectory');
0420             disp('V PLANE');
0421         end;
0422         
0423         corv0=getcellstruct(ropen,'PolynomA',VK,1,1);
0424         
0425         if traj==1
0426             
0427             ModelRM=getresponsematrices(...
0428                 ropen,...          %1 AT lattice
0429                 bpm,...      %2 bpm indexes in at lattice
0430                 [],...     %3 h cor indexes
0431                 VK,...     %4 v cor indexes
0432                 [],...     %5 skew cor indexes
0433                 [],...     %6 quad cor indexes
0434                 [],...
0435                 inCOD,...       %7 initial coordinates
0436                 rmsel...        %8 specifiy rm to be computed
0437                 );
0438             if ~correctflags(1) % dpp correction
0439                 ModelRM.TrajVDPP{1}=[];
0440             end
0441             RespV=ModelRM.TrajVCor{3};
0442             RespVd=ModelRM.TrajVDPP;
0443         elseif traj==0
0444             RespV=RMV(usebpm,usecorH);
0445             if correctflags(1) % dpp correction
0446                 RespVd=RMVd(usebpm);
0447             end
0448         end
0449         
0450         % build RMs
0451         if  correctflags(2) % no dpp mean0
0452             FRV=[RespV;ones(1,size(RespV,2))];
0453         elseif ~correctflags(2) % no dpp no mean0
0454             FRV=RespV;
0455         end
0456         
0457         % if mean0 correction add 0 to correction vector
0458         if correctflags(2)
0459             dcv=qemsvd_mod(FRV,[Y;0],neigSteerer)*fracapply;
0460         else
0461             dcv=qemsvd_mod(FRV,Y,neigSteerer)*fracapply;
0462         end
0463         
0464         
0465         % limit steerers strengths
0466         if ~isempty(steererlimit)
0467             dcv(abs(dcv)>steererlimit(2))=steererlimit(2);
0468         end
0469         ropen=setcellstruct(ropen,'PolynomA',VK,corv0-dcv,1,1);
0470    
0471         
0472         if printouttext
0473             disp('correcting available V trajectory'); end;
0474         
0475         % get current trajectory
0476         
0477         [t]=findtrajectory6Err(ropen,indBPM,inCOD);
0478         
0479         oxc=t(1,:);
0480         oyc=t(3,:);
0481         
0482         
0483         if printouttext
0484             % display results
0485             disp(['X: ' num2str(std(ox(usebpm))*1e6,'%3.3f') ' -> '...
0486                 num2str(std(oxc(usebpm))*1e6,'%3.3f') ' um'])
0487             disp(['Y: ' num2str(std(oy(usebpm))*1e6,'%3.3f') ' -> '...
0488                 num2str(std(oyc(usebpm))*1e6,'%3.3f') ' um'])
0489         end;
0490         
0491     end
0492     
0493 end
0494 
0495 if printouttext
0496     
0497     [t]=findtrajectory6Err(ropen,indBPM,inCOD);
0498     
0499     ox=t(1,:);
0500     oy=t(3,:);
0501     
0502     [t]=findtrajectory6Err(rclosed,indBPM,inCOD);
0503     
0504     oxc=t(1,:);
0505     oyc=t(3,:);
0506     
0507     
0508     % display results
0509     disp(['X: ' num2str(std(ox)*1e6,'%3.3f') ' -> '...
0510         num2str(std(oxc)*1e6,'%3.3f') ' um'])
0511     disp(['Y: ' num2str(std(oy)*1e6,'%3.3f') ' -> '...
0512         num2str(std(oyc)*1e6,'%3.3f') ' um'])
0513 end;
0514 
0515 % update initial cloed orbit guess
0516 [inCOD]=findorbit6Err(rclosed,1,inCOD);
0517 
0518 
0519 end