*-- Author : S.Burke / J.V. Morris
SUBROUTINE FKLRFL(JPL,IFLAG,IERR)
**********************************************************************
* *
* Reverse Kalman Filter used to add or remove a measurement *
* at plane JPL *
* *
* ERROR CONDITIONS; *
* IERR = 0 ; normal termination *
* -> IERR = 101 ; no smoothed vector at JPL *
* -> IERR = 103 ; no measurement at JPL *
* -> IERR = 104 ; invalid value of IFLAG *
* -> IERR = 107 ; failure to invert measurement covariance *
* -> IERR = 111 ; failure to invert filtered covariance *
* -> IERR = 116 ; theta > pi/2 (reset to pi/4) *
* IERR = 17 ; theta > 1 (warning) *
* *
* -> Fatal errors *
* *
* NB All fatal errors result in the update not being made, and are *
* therefore recoverable *
* *
**********************************************************************
IMPLICIT DOUBLE PRECISION (A-H,O-Z)
PARAMETER (IUTIL=0,IROUT=5)
*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,FKSMTH.
*KEND.
**********************************************************************
*
* Local arrays ...
*
DIMENSION HGW(2),WT(2,5),GMES(2,2)
**********************************************************************
*
* Initialisation and checks ...
*
IERR = 0
* Has smoothing to JPL been done ...... if not, terminate!
IF (.NOT.LSMT(JPL)) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IINF1,IERR)
RETURN
ENDIF
* If no measurement has been made at JPL, then terminate
IF (.NOT.LMES(JPL)) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IINF3,IERR)
RETURN
ENDIF
* IFLAG should not be zero
IF (IFLAG.EQ.0) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IINV,IERR)
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
*
* If IFLAG is negative the point is being filtered out, so
* negate the (inverse of the) measurement covariance
*
IF (IFLAG.LT.0) THEN
GMES(1,1) = -GMES(1,1)
GMES(2,1) = -GMES(2,1)
GMES(2,2) = -GMES(2,1)
ENDIF
*
* Compute the covariance
*
IF (MES(JPL).EQ.1) THEN
CALL FKCOVP(CSMT(1,1,JPL),HMES(1,1,JPL),GMES,
& CSMTR(1,1,JPL),WT,IFAIL)
ELSE
CALL FKCOVR(CSMT(1,1,JPL),HMES(1,1,JPL),GMES,
& CSMTR(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(SSMT(1,JPL),WT,CSMTR(1,1,JPL),HGW,SSMTR(1,JPL))
CALL FKNORM(SSMTR(1,JPL),IFAIL)
IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IFAIL/100,IFAIL,IERR)
IF (IFAIL.GE.100) RETURN
IF (ABS(IFLAG).GT.1) THEN
* Copy the filtered vector and its covariance back into /FKSMTH/
CALL UCOPY(SSMTR(1,JPL),SSMT(1,JPL),10)
CALL FKCOPY(CSMTR(1,1,JPL),CSMT(1,1,JPL))
ENDIF
RETURN
END
*