*-- Author : S.Burke
SUBROUTINE FKLXYZ(XY,CXYZ,SP,CP,SF,CF,CHISQ,IERR)
*-----------------------------------------Updates 02/06/92-------
**: FKLXYZ 50106 SB. Protect against divide by 0.
*-----------------------------------------Updates----------------
**********************************************************************
* *
* 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=15)
*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),CXYZ(3,3)
DIMENSION SP(5),CP(5,5),SF(5),CF(5,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)
SPHI = DSIN(SP(5))
CPHI = DCOS(SP(5))
IF (ABS(CPHI).GT.1.0D-15) THEN
TANPHI = SPHI/CPHI
ELSE
TANPHI = SIGN(1.0D15,CPHI)*SPHI
ENDIF
IF (ABS(TANPHI).GT.1.0D-15) THEN
COTPHI = 1.D0/TANPHI
ELSE
COTPHI = SIGN(1.0D15,TANPHI)
ENDIF
TTHDZ = SP(4)**2*CXYZ(3,3)
100 CONTINUE
CMES(1,1) = CXYZ(1,1) + CPHI*(CPHI*TTHDZ + 2.D0*CXYZ(3,1)
& + 2.D0*COTPHI*CXYZ(3,2))
CMES(2,1) = CXYZ(2,1) + CPHI*SPHI*TTHDZ + 2.D0*SPHI*CXYZ(3,1)
& + 2.D0*CPHI*CXYZ(3,2)
CMES(2,2) = CXYZ(2,2) + SPHI*(SPHI*TTHDZ + 2.D0*CXYZ(3,2)
& + 2.D0*TANPHI*CXYZ(3,1))
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)-SP(4)).GT.DTHMAX) THEN
LFLAG = .TRUE.
TTHDZ = SP(4)**2*CXYZ(3,3)
ENDIF
IF (DABS(SF(5)-SP(5)).GT.DPHMAX) THEN
LFLAG = .TRUE.
SPHI = DSIN(SP(5))
CPHI = DCOS(SP(5))
IF (ABS(CPHI).GT.1.0D-15) THEN
TANPHI = SPHI/CPHI
ELSE
TANPHI = SIGN(1.0D15,CPHI)*SPHI
ENDIF
IF (ABS(TANPHI).GT.1.0D-15) THEN
COTPHI = 1.D0/TANPHI
ELSE
COTPHI = SIGN(1.0D15,TANPHI)
ENDIF
ENDIF
IF (LFLAG) THEN
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
*