--- DarthVader/TrackerLevel2/src/F77/mini.f 2007/05/14 11:03:06 1.16 +++ DarthVader/TrackerLevel2/src/F77/mini.f 2007/05/24 13:29:09 1.17 @@ -46,7 +46,7 @@ DATA ALMAX/dinf,dinf,1.,dinf,dinf/ !limits on alpha vector components DATA ALMIN/-dinf,-dinf,-1.,-dinf,-dinf/ !" - DIMENSION DAL(5) !increment of vector alfa +c$$$ DIMENSION DAL(5) !increment of vector alfa DIMENSION CHI2DD_R(4,4),CHI2D_R(4) !hessiano e gradiente di chi2 c elena-------- @@ -67,9 +67,17 @@ c LOGICAL TRKDEBUG,TRKVERBOSE c COMMON/TRKD/TRKDEBUG,TRKVERBOSE - LOGICAL TRKDEBUG,TRKVERBOSE + LOGICAL TRKDEBUG,TRKVERBOSE,STUDENT COMMON/TRKD/TRKDEBUG,TRKVERBOSE + DIMENSION AL0(5) + LOGICAL SUCCESS_NEW,SUCCESS_OLD +* +* define kind of minimization (0x=chi2+gaussian or 1x=likelihood+student) +* + STUDENT = .false. + IF(MOD(INT(TRACKMODE/10),10).EQ.1) STUDENT = .true. + IF(IPRINT.EQ.1) THEN TRKVERBOSE = .TRUE. TRKDEBUG = .FALSE. @@ -132,97 +140,225 @@ * ----------------------- 10 ISTEP=ISTEP+1 !<<<<<<<<<<<<<< NEW STEP !! - CALL CHISQ(IFLAG,JFAIL) !chi^2 and its derivatives - IF(JFAIL.NE.0) THEN - IFAIL=1 - CHI2=-9999. - if(TRKVERBOSE) - $ PRINT *,'*** ERROR in mini *** wrong CHISQ' - RETURN - ENDIF - - COST=1e-5 - DO I=1,5 - DO J=1,5 - CHI2DD(I,J)=CHI2DD(I,J)*COST +* ------------------------------- +* **** Chi2+gaussian minimization +* ------------------------------- + + IF(.NOT.STUDENT) THEN + + CALL CHISQ(IFLAG,JFAIL) !chi^2 and its derivatives + IF(JFAIL.NE.0) THEN + IFAIL=1 + CHI2=-9999. + if(TRKVERBOSE) + $ PRINT *,'*** ERROR in mini *** wrong CHISQ' + RETURN + ENDIF + +c COST=1e-5 + COST=1. + DO I=1,5 + IF(CHI2DD(I,I).NE.0.)COST=COST/DABS(CHI2DD(I,I))**0.2 + ENDDO + DO I=1,5 + DO J=1,5 + CHI2DD(I,J)=CHI2DD(I,J)*COST + ENDDO +c$$$ CHI2D(I)=CHI2D(I)*COST ENDDO - CHI2D(I)=CHI2D(I)*COST - ENDDO - IF(PFIXED.EQ.0.) THEN + IF(PFIXED.EQ.0.) THEN *------------------------------------------------------------* * track fitting with FREE deflection *------------------------------------------------------------* - CALL DSFACT(5,CHI2DD,5,IFA,DET,JFA) !CHI2DD matrix determinant - IF(IFA.NE.0) THEN !not positive-defined - if(TRKVERBOSE)then - PRINT *, - $ '*** ERROR in mini ***'// - $ 'on matrix inversion (not pos-def)' - $ ,DET - endif - IF(CHI2.EQ.0) CHI2=-9999. - IF(CHI2.GT.0) CHI2=-CHI2 - IFAIL=1 - RETURN - ENDIF - CALL DSFINV(5,CHI2DD,5) !CHI2DD matrix inversion + CALL DSFACT(5,CHI2DD,5,IFA,DET,JFA) !CHI2DD matrix determinant + IF(IFA.NE.0) THEN !not positive-defined + if(TRKVERBOSE)then + PRINT *, + $ '*** ERROR in mini ***'// + $ 'on matrix inversion (not pos-def)' + $ ,DET + endif + IF(CHI2.EQ.0) CHI2=-9999. + IF(CHI2.GT.0) CHI2=-CHI2 + IFAIL=1 + RETURN + ENDIF + CALL DSFINV(5,CHI2DD,5) !CHI2DD matrix inversion * ******************************************* * find new value of AL-pha * ******************************************* - DO I=1,5 - DAL(I)=0. - DO J=1,5 - DAL(I)=DAL(I)-CHI2DD(I,J)*CHI2D(J) - COV(I,J)=2.*COST*CHI2DD(I,J) - ENDDO - ENDDO - DO I=1,5 - AL(I)=AL(I)+DAL(I) - ENDDO + DO I=1,5 + DAL(I)=0. + DO J=1,5 + DAL(I)=DAL(I)-CHI2DD(I,J)*CHI2D(J) *COST + COV(I,J)=2.*COST*CHI2DD(I,J) + ENDDO + ENDDO + DO I=1,5 + AL(I)=AL(I)+DAL(I) + ENDDO *------------------------------------------------------------* * track fitting with FIXED deflection *------------------------------------------------------------* - ELSE - AL(5)=1./PFIXED - DO I=1,4 - CHI2D_R(I)=CHI2D(I) - DO J=1,4 - CHI2DD_R(I,J)=CHI2DD(I,J) + ELSE + AL(5)=1./PFIXED + DO I=1,4 + CHI2D_R(I)=CHI2D(I) + DO J=1,4 + CHI2DD_R(I,J)=CHI2DD(I,J) + ENDDO ENDDO - ENDDO - CALL DSFACT(4,CHI2DD_R,4,IFA,DET,JFA) - IF(IFA.NE.0) THEN - if(TRKVERBOSE)then - PRINT *, - $ '*** ERROR in mini ***'// - $ 'on matrix inversion (not pos-def)' - $ ,DET - endif - IF(CHI2.EQ.0) CHI2=-9999. - IF(CHI2.GT.0) CHI2=-CHI2 - IFAIL=1 - RETURN - ENDIF - CALL DSFINV(4,CHI2DD_R,4) + CALL DSFACT(4,CHI2DD_R,4,IFA,DET,JFA) + IF(IFA.NE.0) THEN + if(TRKVERBOSE)then + PRINT *, + $ '*** ERROR in mini ***'// + $ 'on matrix inversion (not pos-def)' + $ ,DET + endif + IF(CHI2.EQ.0) CHI2=-9999. + IF(CHI2.GT.0) CHI2=-CHI2 + IFAIL=1 + RETURN + ENDIF + CALL DSFINV(4,CHI2DD_R,4) * ******************************************* * find new value of AL-pha * ******************************************* - DO I=1,4 - DAL(I)=0. - DO J=1,4 - DAL(I)=DAL(I)-CHI2DD_R(I,J)*CHI2D_R(J) - COV(I,J)=2.*COST*CHI2DD_R(I,J) + DO I=1,4 + DAL(I)=0. + DO J=1,4 + DAL(I)=DAL(I)-CHI2DD_R(I,J)*CHI2D_R(J) *COST + COV(I,J)=2.*COST*CHI2DD_R(I,J) + ENDDO + ENDDO + DAL(5)=0. + DO I=1,4 + AL(I)=AL(I)+DAL(I) ENDDO + ENDIF + + if(TRKDEBUG) print*,'mini2: step ',istep,chi2,1./AL(5) + +c$$$ PRINT*,'DAL ',(DAL(K),K=1,5) +c$$$ PRINT*,'CHI2DOLD ',(CHI2DOLD(K),K=1,5) + + + ENDIF + +* ------------------------------- +* **** Likelihood+Student minimization +* ------------------------------- + + IF(STUDENT) THEN + CALL CHISQSTT(1,JFAIL) + DO I=1,5 + DAL(I)=0. + DO J=1,5 + DAL(I)=DAL(I)-CHI2DD(I,J)*CHI2D(J) + ENDDO + ENDDO + + DO I=1,5 + DO j=1,5 + COV(I,J) = 2.*CHI2DD(I,J) + ENDDO + ENDDO + + CHI2TOLL = 1.E-3 + ALPHA = 3.0 + BETA = -0.4 + E=1. + EA=1. + EB=1. + EC=1. + FA=1. + FB=1. + FC=1. + SUCCESS_OLD = .FALSE. + SUCCESS_NEW = .FALSE. + + CALL CHISQSTT(0,JFAIL) +c$$$ PRINT*,CHI2 + CHI2_NEW = CHI2 + FC = CHI2 + EC = 0. + + 100 CONTINUE + DO I=1,5 + AL0(I)=AL(I) + ENDDO + DO I=1,5 + AL(I)=AL(I)+E*DAL(I) ENDDO - DAL(5)=0. - DO I=1,4 - AL(I)=AL(I)+DAL(I) + CALL CHISQSTT(0,JFAIL) + CHI2_OLD = CHI2_NEW + CHI2_NEW = CHI2 + FA = FB + FB = FC + FC = CHI2 + EA = EB + EB = EC + EC = E + +c$$$ PRINT*,E,CHI2_NEW + + IF(CHI2_NEW.LE.CHI2_OLD) THEN ! success + IF(DABS(CHI2_NEW-CHI2_OLD).LT.CHI2TOLL) GOTO 101 + SUCCESS_OLD = SUCCESS_NEW + SUCCESS_NEW = .TRUE. + E = E*ALPHA + ELSE ! failure + SUCCESS_OLD = SUCCESS_NEW + SUCCESS_NEW = .FALSE. + CHI2_NEW = CHI2_OLD + DO I=1,5 + AL(I)=AL0(I) + ENDDO + IF(SUCCESS_OLD) THEN + DENOM = (EB-EA)*(FB-FC) - (EB-EC)*(FB-FA) + IF(DENOM.NE.0.) THEN + E = EB - 0.5*( (EB-EA)**2*(FB-FC) + $ - (EB-EC)**2*(FB-FA) ) / DENOM + ELSE + E = BETA*E + ENDIF + ELSE + E = BETA*E + ENDIF +c$$$ E = BETA*E + ENDIF + GOTO 100 + + 101 CONTINUE + + DO I=1,5 + DAL(I)=E*DAL(I) ENDDO + +c$$$ print*,' ' +c$$$ PRINT*,'DAL ',(DAL(K),K=1,5) +c$$$ PRINT*,'CHI2DOLD ',(CHI2DOLD(K),K=1,5) +c$$$ print*,'==== CHI2 ====' +c$$$ print*,chi2 +c$$$ print*,'==== CHI2d ====' +c$$$ print*,(chi2d(i),i=1,5) +c$$$ print*,'==== CHI2dd ====' +c$$$ do j=1,5 +c$$$ print*,(chi2dd(j,i),i=1,5) +c$$$ enddo +c$$$ print*,'================' +c$$$ print*,' ' + +*========= FIN QUI ============= + ENDIF - if(TRKDEBUG) print*,'mini2: step ',istep,chi2,1./AL(5) + + + *------------------------------------------------------------* * ---------------------------------------------------- * @@ -280,74 +416,102 @@ IF(ABS(DAL(I)).GT.ALTOL(I))GOTO 10 !>>>> new step! ENDDO -* new estimate of chi^2: - JFAIL=0 !error flag - CALL CHISQ(IFLAG,JFAIL) !chi^2 and its derivatives - IF(JFAIL.NE.0) THEN - IFAIL=1 - if(TRKVERBOSE)THEN - CHI2=-9999. - if(TRKVERBOSE) - $ PRINT *,'*** ERROR in mini *** wrong CHISQ' - ENDIF - RETURN - ENDIF - COST=1e-7 - DO I=1,5 - DO J=1,5 - CHI2DD(I,J)=CHI2DD(I,J)*COST - ENDDO - CHI2D(I)=CHI2D(I)*COST - ENDDO - IF(PFIXED.EQ.0.) THEN - CALL DSFACT(5,CHI2DD,5,IFA,DET,JFA) !CHI2DD matrix determinant - IF(IFA.NE.0) THEN !not positive-defined - if(TRKVERBOSE)then - PRINT *, - $ '*** ERROR in mini ***'// - $ 'on matrix inversion (not pos-def)' - $ ,DET - endif - IF(CHI2.EQ.0) CHI2=-9999. - IF(CHI2.GT.0) CHI2=-CHI2 +***************************** +* final estimate of chi^2 +***************************** + +* ------------------------------- +* **** Chi2+gaussian minimization +* ------------------------------- + + IF(.NOT.STUDENT) THEN + + JFAIL=0 !error flag + CALL CHISQ(IFLAG,JFAIL) !chi^2 and its derivatives + IF(JFAIL.NE.0) THEN IFAIL=1 - RETURN + if(TRKVERBOSE)THEN + CHI2=-9999. + if(TRKVERBOSE) + $ PRINT *,'*** ERROR in mini *** wrong CHISQ' + ENDIF + RETURN ENDIF - CALL DSFINV(5,CHI2DD,5) !CHI2DD matrix inversion - DO I=1,5 - DAL(I)=0. - DO J=1,5 - COV(I,J)=2.*COST*CHI2DD(I,J) - ENDDO - ENDDO - ELSE - DO I=1,4 - CHI2D_R(I)=CHI2D(I) - DO J=1,4 - CHI2DD_R(I,J)=CHI2DD(I,J) +c COST=1e-7 + COST=1. + DO I=1,5 + IF(CHI2DD(I,I).NE.0.)COST=COST/DABS(CHI2DD(I,I))**0.2 + ENDDO + DO I=1,5 + DO J=1,5 + CHI2DD(I,J)=CHI2DD(I,J)*COST ENDDO ENDDO - CALL DSFACT(4,CHI2DD_R,4,IFA,DET,JFA) - IF(IFA.NE.0) THEN - if(TRKVERBOSE)then - PRINT *, - $ '*** ERROR in mini ***'// - $ 'on matrix inversion (not pos-def)' - $ ,DET - endif - IF(CHI2.EQ.0) CHI2=-9999. - IF(CHI2.GT.0) CHI2=-CHI2 - IFAIL=1 - RETURN + IF(PFIXED.EQ.0.) THEN + CALL DSFACT(5,CHI2DD,5,IFA,DET,JFA) !CHI2DD matrix determinant + IF(IFA.NE.0) THEN !not positive-defined + if(TRKVERBOSE)then + PRINT *, + $ '*** ERROR in mini ***'// + $ 'on matrix inversion (not pos-def)' + $ ,DET + endif + IF(CHI2.EQ.0) CHI2=-9999. + IF(CHI2.GT.0) CHI2=-CHI2 + IFAIL=1 + RETURN + ENDIF + CALL DSFINV(5,CHI2DD,5) !CHI2DD matrix inversion + DO I=1,5 +c$$$ DAL(I)=0. + DO J=1,5 + COV(I,J)=2.*COST*CHI2DD(I,J) + ENDDO + ENDDO + ELSE + DO I=1,4 + CHI2D_R(I)=CHI2D(I) + DO J=1,4 + CHI2DD_R(I,J)=CHI2DD(I,J) + ENDDO + ENDDO + CALL DSFACT(4,CHI2DD_R,4,IFA,DET,JFA) + IF(IFA.NE.0) THEN + if(TRKVERBOSE)then + PRINT *, + $ '*** ERROR in mini ***'// + $ 'on matrix inversion (not pos-def)' + $ ,DET + endif + IF(CHI2.EQ.0) CHI2=-9999. + IF(CHI2.GT.0) CHI2=-CHI2 + IFAIL=1 + RETURN + ENDIF + CALL DSFINV(4,CHI2DD_R,4) + DO I=1,4 +c$$$ DAL(I)=0. + DO J=1,4 + COV(I,J)=2.*COST*CHI2DD_R(I,J) + ENDDO + ENDDO ENDIF - CALL DSFINV(4,CHI2DD_R,4) - DO I=1,4 - DAL(I)=0. - DO J=1,4 - COV(I,J)=2.*COST*CHI2DD_R(I,J) + + ENDIF + +* ------------------------------- +* **** Likelihood+student minimization +* ------------------------------- + + IF(STUDENT) THEN + CALL CHISQSTT(1,JFAIL) + DO I=1,5 + DO j=1,5 + COV(I,J) = 2.*CHI2DD(I,J) ENDDO ENDDO ENDIF + ***************************** * ------------------------------------ @@ -601,7 +765,162 @@ RETURN END +****************************************************************************** +* +* routine to compute Likelihodd+Student and its derivatives +* +* (modified in respect to the previous one in order to include +* single clusters. In this case the residual is evaluated by +* calculating the distance between the track intersection and the +* segment AB associated to the single cluster) +* +****************************************************************************** + + SUBROUTINE CHISQSTT(IFLAG,JFAIL) + + IMPLICIT DOUBLE PRECISION (A-H,O-Z) + + include 'commontracker.f' !tracker general common + include 'common_mini_2.f' !common for the tracking procedure + + LOGICAL TRKDEBUG,TRKVERBOSE + COMMON/TRKD/TRKDEBUG,TRKVERBOSE + + DIMENSION AL_P(5) + DIMENSION VECTEMP(5) +c$$$ DIMENSION U(5) ! BFGS + + DO I=1,5 + AL_P(I)=AL(I) + ENDDO + JFAIL=0 !error flag + CALL POSXYZ(AL_P,JFAIL) !track intersection with tracking planes + IF(JFAIL.NE.0) THEN + IF(TRKVERBOSE) + $ PRINT *,'CHISQSTT ==> error from trk routine POSXYZ !!' + IFAIL=1 + RETURN + ENDIF + + DO I=1,nplanes + DXDAL(I,1)=1. + DYDAL(I,1)=0. + DXDAL(I,2)=0. + DYDAL(I,2)=1. + COSTHE=DSQRT(1.-AL(3)**2) + IF(COSTHE.EQ.0.) THEN + IF(TRKVERBOSE)PRINT *,'=== WARNING ===> COSTHE=0' + IFAIL=1 + RETURN + ENDIF + DXDAL(I,3)=(ZINI-ZM(I))*DCOS(AL(4))/COSTHE**3 + DYDAL(I,3)=(ZINI-ZM(I))*DSIN(AL(4))/COSTHE**3 + DXDAL(I,4)=-AL(3)*(ZINI-ZM(I))*DSIN(AL(4))/COSTHE + DYDAL(I,4)=AL(3)*(ZINI-ZM(I))*DCOS(AL(4))/COSTHE + IF(AL(5).NE.0.) THEN + DXDAL(I,5)= + + (XV(I)-(AL(1)+AL(3)/COSTHE*(ZINI-ZM(I)) + + *DCOS(AL(4))))/AL(5) + DYDAL(I,5)= + + (YV(I)-(AL(2)+AL(3)/COSTHE*(ZINI-ZM(I)) + + *DSIN(AL(4))))/AL(5) + ELSE + DXDAL(I,5)=100.*( 0.25 *0.3*0.4*(0.01*(ZINI-ZM(I)))**2 ) + DYDAL(I,5)=0. + ENDIF + ENDDO + + IF(IFLAG.EQ.0) THEN ! function calulation + CHI2=0. + DO I=1,nplanes + IF(XGOOD(I).EQ.1.AND.YGOOD(I).EQ.0)THEN !X-cl + BETA = (XM_B(I)-XM_A(I))/(YM_B(I)-YM_A(I)) + ALFA = XM_A(I) - BETA * YM_A(I) + YM(I) = ( YV(I) + BETA*XV(I) - BETA*ALFA )/(1+BETA**2) + if(YM(I).lt.dmin1(YM_A(I),YM_B(I))) + $ YM(I)=dmin1(YM_A(I),YM_B(I)) + if(YM(I).gt.dmax1(YM_A(I),YM_B(I))) + $ YM(I)=dmax1(YM_A(I),YM_B(I)) + XM(I) = ALFA + BETA * YM(I) !<<<< measured coordinates + ELSEIF(XGOOD(I).EQ.0.AND.YGOOD(I).EQ.1)THEN !Y-cl + BETA = (YM_B(I)-YM_A(I))/(XM_B(I)-XM_A(I)) + ALFA = YM_A(I) - BETA * XM_A(I) + XM(I) = ( XV(I) + BETA*YV(I) - BETA*ALFA )/(1+BETA**2) + if(XM(I).lt.dmin1(XM_A(I),XM_B(I))) + $ XM(I)=dmin1(XM_A(I),XM_B(I)) + if(XM(I).gt.dmax1(XM_A(I),XM_B(I))) + $ XM(I)=dmax1(XM_A(I),XM_B(I)) + YM(I) = ALFA + BETA * XM(I) !<<<< measured coordinates + ENDIF + TERMX = DLOG( (TAILX(I)*RESX(I)**2+(XV(I)-XM(I))**2)/ + $ (TAILX(I)*RESX(I)**2) ) + TERMY = DLOG( (TAILY(I)*RESY(I)**2+(YV(I)-YM(I))**2)/ + $ (TAILY(I)*RESY(I)**2) ) + CHI2=CHI2 + $ +(TAILX(I)+1.0)*TERMX *( XGOOD(I) ) + $ +(TAILY(I)+1.0)*TERMY *( YGOOD(I) ) + ENDDO + ENDIF + + IF(IFLAG.EQ.1) THEN ! derivative calulation + DO I=1,5 + CHI2DOLD(I)=CHI2D(I) + ENDDO + DO J=1,5 + CHI2D(J)=0. + DO I=1,nplanes + CHI2D(J)=CHI2D(J) + $ +2.*(TAILX(I)+1.0)*(XV(I)-XM(I))/ + $ (TAILX(I)*RESX(I)**2+(XV(I)-XM(I))**2)* + $ DXDAL(I,J) *XGOOD(I) + $ +2.*(TAILY(I)+1.0)*(YV(I)-YM(I))/ + $ (TAILY(I)*RESY(I)**2+(YV(I)-YM(I))**2)* + $ DYDAL(I,J) *YGOOD(I) + ENDDO + ENDDO + DO K=1,5 + VECTEMP(K)=0. + DO M=1,5 + VECTEMP(K) = VECTEMP(K) + + $ COV(K,M)/2.*(CHI2D(M)-CHI2DOLD(M)) + ENDDO + ENDDO + DOWN1 = 0. + DO K=1,5 + DOWN1 = DOWN1 + DAL(K)*(CHI2D(K)-CHI2DOLD(K)) + ENDDO + IF(DOWN1.EQ.0.) THEN + PRINT*,'WARNING IN MATRIX CALULATION (STUDENT), DOWN1 = 0' + IFAIL=1 + RETURN + ENDIF + DOWN2 = 0. + DO K=1,5 + DO M=1,5 + DOWN2 = DOWN2 + (CHI2D(K)-CHI2DOLD(K))*VECTEMP(K) + ENDDO + ENDDO + IF(DOWN2.EQ.0.) THEN + PRINT*,'WARNING IN MATRIX CALULATION (STUDENT), DOWN2 = 0' + IFAIL=1 + RETURN + ENDIF +c$$$ DO K=1,5 ! BFGS +c$$$ U(K) = DAL(K)/DOWN1 - VECTEMP(K)/DOWN2 +c$$$ ENDDO + DO I=1,5 + DO J=1,5 + CHI2DD(I,J) = COV(I,J)/2. + $ +DAL(I)*DAL(J)/DOWN1 + $ -VECTEMP(I)*VECTEMP(J)/DOWN2 +c$$$ $ +DOWN2*U(I)*U(J) ! BFGS + ENDDO + ENDDO + ENDIF + RETURN + END + ***************************************************************** * * Routine to compute the track intersection points