*-- Author : S.Burke
SUBROUTINE FKLXY(XY,CXY,DDZ,SP,CP,SF,CF,CHISQ,IERR)
**********************************************************************
* *
* Kalman Filter an x/y point with errors on z *
* *
* ERROR CONDITIONS; *
* IERR = 0 ; normal termination *
* -> IERR = 107 ; failure to invert measurement covariance *
* -> IERR = 111 ; failure to invert filtered covariance *
* IERR = 17 ; tan(theta) > 1 (warning) *
* IERR = 12 ; covariance of filtered residuals n.p.d. *
* IERR = 20 + n ; 2 < n < 10 iterations *
* -> IERR = 130 ; 10 iterations *
* *
* -> Fatal errors *
* *
* NB Error 12 is not considered fatal, but the chi-sq will be zero *
* *
* Note that after an error 130 the output vectors are defined, but *
* are not guaranteed to be correct. *
* *
**********************************************************************
IMPLICIT DOUBLE PRECISION (A-H,O-Z)
PARAMETER (IUTIL=0,IROUT=14)
*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.
REAL XY(2),CXY(2,2),DDZ
DIMENSION SP(5),CP(5,5),SF(5),CF(5,5),SS(5)
DIMENSION WMES(2),CMES(2,2)
LOGICAL LFLAG
*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,FKFTVX.
DOUBLE PRECISION DTHMAX,DPHMAX
COMMON /FKFTVX/ DTHMAX,DPHMAX
*KEND.
**********************************************************************
IERR = 0
NPASS = 0
WMES(1) = XY(1)
WMES(2) = XY(2)
CALL UCOPY(SP,SS,10)
SPHI = DSIN(SP(5))
CPHI = DCOS(SP(5))
TTHDZ = SP(4)*DDZ
TTHDZ = TTHDZ*TTHDZ
100 CONTINUE
CMES(1,1) = CXY(1,1) + CPHI*CPHI*TTHDZ
CMES(2,1) = CXY(2,1) + CPHI*SPHI*TTHDZ
CMES(2,2) = CXY(2,2) + SPHI*SPHI*TTHDZ
CALL FKLFXY(SP,CP,WMES,CMES,SF,CF,CHISQ,IFAIL)
IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IFAIL/100,IFAIL,IERR)
IF (IFAIL.GE.100) RETURN
LFLAG = .FALSE.
IF (DABS(SF(4)-SS(4)).GT.DTHMAX) LFLAG = .TRUE.
IF (DABS(SF(5)-SS(5)).GT.DPHMAX) LFLAG = .TRUE.
IF (LFLAG) THEN
CALL UCOPY(SF,SS,10)
TTHDZ = SF(4)*DDZ
TTHDZ = TTHDZ*TTHDZ
SPHI = DSIN(SF(5))
CPHI = DCOS(SF(5))
NPASS = NPASS + 1
IF (NPASS.LT.10) GOTO 100
CALL FKERR(IUTIL,IROUT,IFATAL,IFREE1,IERR)
RETURN
ENDIF
IF (NPASS.GT.2) CALL FKERR(IUTIL,IROUT,IWARN,IFREE+NPASS,IERR)
RETURN
END
*