*-- Author : S.Burke / J.V. Morris
SUBROUTINE FKLFLT(JPL,IERR)
**********************************************************************
* *
* Kalman Filter at plane JPL using the weighted mean formalism. *
* *
* ERROR CONDITIONS; *
* IERR = 0 ; normal termination *
* -> IERR = 101 ; no projection at JPL - filter terminated *
* IERR = 5 ; filtering at JPL already done - but continue *
* -> IERR = 107 ; failure to invert measurement covariance *
* -> IERR = 111 ; failure to invert filtered covariance *
* IERR = 12 ; covariance of filtered residuals n.p.d. *
* -> IERR = 116 ; theta > pi/2: reset to pi/4 *
* IERR = 17 ; theta > 1 (warning) *
* *
* -> Fatal errors *
* *
* NB Error 12 is not considered fatal, but the chi-sq will be zero *
* *
**********************************************************************
IMPLICIT DOUBLE PRECISION (A-H,O-Z)
PARAMETER (IUTIL=0,IROUT=3)
*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,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,FKRSID.
*KEND.
**********************************************************************
*
* Local arrays ...
*
DIMENSION WT(2,5),GMES(2,2),HGW(2)
**********************************************************************
*
* Initialisation and checks ...
*
IERR = 0
* has projection to JPL been done ...... if not, terminate!
IF (.NOT.LPRO(JPL)) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IINF1,IERR)
RETURN
ENDIF
* Has filtering already been done at this plane? Continue anyway .... !?
IF (LFIL(JPL)) CALL FKERR(IUTIL,IROUT,IWARN,IDONE,IERR)
**********************************************************************
*
* If no measurement has been made at JPL, then the filtered vector and
* its covariance are the same as the projected vector and covariance.
* The residuals, covariances and chi-squared are set to zero. This is
* not strictly necessary, but it makes things neater.
*
IF (.NOT.LMES(JPL)) THEN
CALL UCOPY(SPRO(1,JPL),SFIL(1,JPL),10)
CALL FKCOPY(CPRO(1,1,JPL),CFIL(1,1,JPL))
IF (LRPRO) THEN
CALL VZERO(RPRO(1,JPL),4)
CALL VZERO(CRPRO(1,1,JPL),8)
ENDIF
IF (LRFIL) THEN
CALL VZERO(RFIL(1,JPL),4)
CALL VZERO(CRFIL(1,1,JPL),8)
CHIFIL(JPL) = 0.D0
ENDIF
LFIL(JPL) = .TRUE.
RETURN
ENDIF
**********************************************************************
* Invert CMES
CALL FKINV(MES(JPL),CMES(1,1,JPL),GMES,IFAIL)
IF (IFAIL.NE.0) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IMCV,IERR)
RETURN
ENDIF
* Compute the filtered (weighted average) covariance
IF (MES(JPL).EQ.1) THEN
CALL FKCOVP(CPRO(1,1,JPL),HMES(1,1,JPL),GMES,
& CFIL(1,1,JPL),WT,IFAIL)
ELSE
CALL FKCOVR(CPRO(1,1,JPL),HMES(1,1,JPL),GMES,
& CFIL(1,1,JPL),WT,IFAIL)
ENDIF
IF (IFAIL.NE.0) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IOCV,IERR)
RETURN
ENDIF
* Compute the filtered state vector
CALL FKWMES(MES(JPL),HMES(1,1,JPL),GMES,WMES(1,JPL),HGW)
CALL FKWVEC(SPRO(1,JPL),WT,CFIL(1,1,JPL),HGW,SFIL(1,JPL))
CALL FKNORM(SFIL(1,JPL),IFAIL)
IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IFAIL/100,IFAIL,IERR)
IF (IFAIL.GT.100) RETURN
* Calculate the residuals of the prediction
IF (LRPRO) CALL FKLRSD(JPL,SPRO(1,JPL),CPRO(1,1,JPL),2,
& RPRO(1,JPL),CRPRO(1,1,JPL),CHIPRO,IFAIL)
* Calculate the filtered residuals
IF (LRFIL) THEN
CALL FKLRSD(JPL,SFIL(1,JPL),CFIL(1,1,JPL),-3,
& RFIL(1,JPL),CRFIL(1,1,JPL),CHIFIL(JPL),IFAIL)
IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IWARN,IFAIL,IERR)
ENDIF
* Set the flag to show filter has been done
LFIL(JPL) = .TRUE.
RETURN
END
*