*-- Author : S.Burke / J.V. Morris
SUBROUTINE FKLFIT(IERR)
**********************************************************************
* *
* KALMAN Filter + Smoother applied to the full Forward Tracker *
* *
* ERROR CONDITIONS; *
* *
* IERR = 0 ; normal termination *
* -> IERR = 101 ; no starting point was provided *
* -> IERR = 102 ; not enough measurements to fit *
* -> IERR = 103 ; invalid value in MES array *
* -> IERR = 104 ; invalid value of JSTART, JSTOP or JLAST *
* IERR = 20 + n ; 1 < n < 10 iterations in point rejection *
* -> IERR = 130 ; 10 iterations in point rejection *
* -> IERR = 200 + ee ; fatal error ee from FKLPRO *
* -> IERR = 300 + ee ; fatal error ee from FKLFIL *
* -> IERR = 400 + ee ; fatal error ee from FKLSMO *
* -> IERR = 500 + ee ; fatal error ee from FKLPRS *
* -> IERR = 600 + ee ; fatal error ee from FKLPAS *
* -> IERR = 600 + ee ; fatal error ee from FKLPAF *
* *
* -> Fatal errors *
* *
**********************************************************************
IMPLICIT DOUBLE PRECISION (A-H,O-Z)
PARAMETER (IUTIL=0,IROUT=1)
*KEEP,FKECODE.
PARAMETER (IWARN=0,IFATAL=1,IFPRO=2,IFFLT=3,IFSMO=4,IFPRS=5,
& IFPAS=6,IFPAF=7)
PARAMETER (IINF1=1,IINF2=2,IINF3=3,IINV=4,IDONE=5)
PARAMETER (IICV=6,IMCV=7,IOCV=11,IRCV=12,IOVCV=13,
& ITHGP2=16,ITHG1=17)
PARAMETER (IFREE=20,IFREE1=30,IFREE2=40,IFREE3=50)
*KEND.
*
* Common block definitions
*
*KEEP,FKNPL.
CHARACTER*5 CKDBG
PARAMETER (CKDBG='FKDBG')
PARAMETER (NPL=72)
LOGICAL LTRUE,LFIRST,LTRPL,LTRPLD
DOUBLE PRECISION TRUE,RTRUE,CHITRU,SPRO,CPRO,SFIL,CFIL
&, SSMT,CSMT,SSMTR,CSMTR,DPRO,CBPRO,QPRO,QGAIN
&, RPRO,CRPRO,RFIL,CRFIL,RSMT,CRSMT,CHIFIL,CHISMT
*
* Per-track values can go in H1WORK; note that LTRUE and LFIRST must
* be set at least per event.
*
* This is about 36k words long; the remaining common blocks are
* about 3.6k in total. Some of this could be in /H1WORK/, but the
* blocks would have to be reorganised.
*
COMMON /H1WORK/
* /FKPROJ/
& SPRO(5,NPL),CPRO(5,5,NPL)
* /FKFILT/
&, SFIL(5,NPL),CFIL(5,5,NPL)
* /FKSMTH/
&, SSMT(5,NPL),CSMT(5,5,NPL)
&, SSMTR(5,NPL),CSMTR(5,5,NPL)
* /FKINT/
&, DPRO(5,5,NPL),CBPRO(5,5,NPL),QPRO(5,5,NPL)
&, QGAIN(5,5,NPL),IAPROX,LFIRST
* /FKRSID/
&, RPRO(2,NPL),CRPRO(2,2,NPL),RFIL(2,NPL)
&, CRFIL(2,2,NPL),RSMT(2,NPL),CRSMT(2,2,NPL)
&, CHIFIL(NPL),CHISMT(NPL)
* /FKTRUE/
&, TRUE(5,NPL),RTRUE(5,NPL),CHITRU(NPL),LTRUE
* /FKDBG/
&, LTRPL(NPL),LTRPLD(NPL)
*KEEP,FKCNTL.
COMMON /FKCNTL/ LUN,IPR,ITR,IPL,JSTART,JSTOP,JLAST,JSTEP
*KEEP,FKCONS.
DOUBLE PRECISION ZPL,DZPL,RADL
COMMON /FKCONS/ ZPL(NPL),DZPL(NPL),RADL(NPL)
*KEEP,FKMEAS.
DOUBLE PRECISION WMES,CMES,HMES
COMMON /FKMEAS/ WMES(2,NPL),CMES(2,2,NPL),HMES(2,2,NPL),MES(NPL)
*KEEP,FKFLAG.
LOGICAL LPRO,LFIL,LSMT,LMES,LRAD,LRPRO,LRFIL,LRSMT,LPOINT,LBLOCK
COMMON /FKFLAG/ LPRO(NPL),LFIL(NPL),LSMT(NPL),LMES(NPL)
&, LRAD(NPL),LRPRO,LRFIL,LRSMT,LPOINT,LBLOCK
*KEEP,FKPROJ.
*KEEP,FKFILT.
*KEEP,FKSMTH.
*KEEP,FKTRUE.
*KEEP,FKRSID.
*KEEP,FKRJCT.
DOUBLE PRECISION CHITOT,X2PCUT,X2CUTB,X2CUTA,X2CUTN
&, X2PCTI,X2CTBI,X2CTAI,X2CTNI
LOGICAL LWIRE,LPRINI
COMMON /FKRJCT/ X2PCUT,X2CUTB,X2CUTA,X2CUTN
&, X2PCTI,X2CTBI,X2CTAI,X2CTNI
&, CHITOT(NPL),NDF(NPL)
&, NBLOCK(NPL),NBADP(NPL),NBADB(NPL)
&, NFAILP(NPL),NFAILB(NPL),NNEWP(NPL)
&, NUNRJP(NPL),NUNRJB(NPL),NRERJP(NPL)
&, NCPRS,NBPRS,NCPAS,NPASS,IRJCT(NPL)
&, LWIRE(NPL),LPRINI
*KEEP,FKINT.
*KEND.
**********************************************************************
*
* Initialisation and checks .........
*
IERR = 0
*
* Check that the start and stop positions are sensible
*
JMIN = MIN(JSTART,JSTOP,JLAST)
JMAX = MAX(JSTART,JSTOP,JLAST)
IF (JMIN.LE.0 .OR. JMAX.GT.NPL .OR.
& (JLAST.GT.JMIN .AND. JLAST.LT.JMAX)) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IINV,IERR)
RETURN
ENDIF
IF (.NOT.LPRO(JSTART)) THEN
* Starting point does not exist
CALL FKERR(IUTIL,IROUT,IFATAL,IINF1,IERR)
RETURN
ENDIF
*
* Set the steps between planes according to the direction
*
IF (JLAST.GE.JSTART) THEN
JSTEP = 1
ELSE
JSTEP = -1
ENDIF
*
* Are there enough measurements (ignoring the starting point which has
* zero weight) to do the 5 parameter fit?
*
NMES = 0
JHWM = 0
DO 100 JPL=JSTART,JLAST,JSTEP
IF (LMES(JPL)) THEN
IF (MES(JPL).LE.0 .OR. MES(JPL).GT.2) THEN
* Can only deal with 1 or 2 measurements per plane
CALL FKERR(IUTIL,IROUT,IFATAL,IINF3,IERR)
RETURN
ENDIF
NMES = NMES + MES(JPL)
JHWM = JPL
ENDIF
100 CONTINUE
IF (NMES.LT.5) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IINF2,IERR)
RETURN
ENDIF
*
* Calculate plane spacing according to direction.
* Be careful about edge effects!
*
DO 200 JPL=JMIN-(JSTEP-1)/2,JMAX-(JSTEP+1)/2
DZPL(JPL) = ZPL(JPL+JSTEP) - ZPL(JPL)
200 CONTINUE
* If point rejection is on, the smoothed residuals must be calculated
IF (LPOINT .OR. LBLOCK) LRSMT = .TRUE.
NPASS = 0
LSTART = JSTART
LSTOP = JSTOP
* Re-entry point
3000 CONTINUE
NPASS = NPASS + 1
IF (NPASS.GE.10) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IFREE1,IERR)
RETURN
ENDIF
*
* Reset all flags ...
*
LFIL(LSTART) = .FALSE.
DO 300 JPL=LSTART+JSTEP,JLAST,JSTEP
LPRO(JPL) = .FALSE.
LFIL(JPL) = .FALSE.
300 CONTINUE
DO 400 JPL=LSTOP,JLAST,JSTEP
LSMT(JPL) = .FALSE.
400 CONTINUE
*
* End of initialisation and checks.
*
**********************************************************************
*
DO 1000 JPL=LSTART,JLAST,JSTEP
IPL = JPL
* If we're past the last measurement, look for a new one
IF ((LPOINT .OR. LBLOCK) .AND. JSTEP*(JPL-JHWM).GT.0) THEN
CALL FKLPAF(JPL,IFAIL)
IF (IFAIL.GT.100) THEN
CALL FKERR(IUTIL,IROUT,IFPAF,IFAIL,IERR)
RETURN
ENDIF
IF (LMES(JPL)) JHWM = JPL
ENDIF
* Filter at plane JPL
CALL FKLFLT(JPL,IFAIL)
IF (IFAIL.GT.100) THEN
CALL FKERR(IUTIL,IROUT,IFFLT,IFAIL,IERR)
RETURN
ENDIF
IF (JPL.NE.JLAST) THEN
* Project to plane JPL+JSTEP (except if we're at the end)
CALL FKLPRO(JPL,IFAIL)
IF (IFAIL.GT.100) THEN
CALL FKERR(IUTIL,IROUT,IFPRO,IFAIL,IERR)
RETURN
ENDIF
ENDIF
1000 CONTINUE
**********************************************************************
NDROP = 0
LLAST = JLAST
1500 CONTINUE
DO 2000 JPL=LLAST,LSTOP,-JSTEP
IPL = JPL
* Smooth from plane JPL+JSTEP to plane JPL ...
CALL FKLSMO(JPL,IFAIL)
IF (IFAIL.GT.100) THEN
CALL FKERR(IUTIL,IROUT,IFSMO,IFAIL,IERR)
RETURN
ENDIF
* Set the rejection flag to 0 on the first pass
IF (LFIRST .AND. NPASS.EQ.1) IRJCT(JPL) = 0
IF (LPOINT .OR. LBLOCK) THEN
* Reject bad points (do this on all planes, as they may be in a block)
CALL FKLPRS(JPL,NDROP,IFAIL)
IF (IFAIL.GT.100) THEN
CALL FKERR(IUTIL,IROUT,IFPRS,IFAIL,IERR)
RETURN
ENDIF
*
* If there isn't a measurement, look for one.
* FKLPAS can be called even if LMES is .TRUE., but whether
* it's useful depends on FKHUNT, which hasn't been written yet.
* If JPL is above the high water mark FKLPAF will already have
* looked for a new point, so there's no point in doing it again.
*
IF (.NOT.LMES(JPL) .AND. JSTEP*(JPL-JHWM).LE.0) THEN
CALL FKLPAS(JPL,NDROP,IFAIL)
IF (IFAIL.GT.100) THEN
CALL FKERR(IUTIL,IROUT,IFPAS,IFAIL,IERR)
RETURN
ENDIF
ENDIF
ENDIF
2000 CONTINUE
* If we've changed a point on this pass, we must smooth back to JSTOP
IF (LSTOP.NE.JSTOP .AND. NDROP.GT.0) THEN
LLAST = LSTOP - JSTEP
LSTOP = JSTOP
DO 500 JPL=LSTOP,LLAST,JSTEP
LSMT(JPL) = .FALSE.
500 CONTINUE
GOTO 1500
ENDIF
*
* Have any planes been rejected during smoothing?
* If so, re-filter outwards from plane NDROP, and smooth back to NDROP+1
*
IF (NDROP.GT.0) THEN
LSTART = NDROP
LSTOP = NDROP + JSTEP
GOTO 3000
ENDIF
IF (NPASS.GT.2) CALL FKERR(IUTIL,IROUT,IWARN,IFREE+NPASS,IERR)
RETURN
END
*