*-- Author : S.Burke
SUBROUTINE FKLFXY(SPRO,CPRO,WMES,CMES,SFIL,CFIL,CHISQ,IERR)
**********************************************************************
* *
* Kalman Filter an x/y point *
* *
* ERROR CONDITIONS; *
* IERR = 0 ; normal termination *
* -> 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 ; x, y or theta has silly value *
* IERR = 17 ; tan(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=12)
*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.
DIMENSION SPRO(5),CPRO(5,5),WMES(2),CMES(2,2),SFIL(5),CFIL(5,5)
DIMENSION GMES(2,2),WT(2,5),GW(2)
**********************************************************************
IERR = 0
* Invert CMES
DET = (CMES(1,1)*CMES(2,2) - CMES(2,1)*CMES(2,1))
IF (DET.LE.0.D0) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IMCV,IERR)
RETURN
ENDIF
DET = 1.D0/DET
GMES(1,1) = CMES(2,2)*DET
GMES(2,1) = -CMES(2,1)*DET
GMES(2,2) = CMES(1,1)*DET
* Compute the filtered (weighted average) covariance
CALL FKCVXY(CPRO,GMES,CFIL,WT,IFAIL)
IF (IFAIL.NE.0) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IOCV,IERR)
RETURN
ENDIF
* Compute the filtered state vector
GW(1) = GMES(1,1)*WMES(1) + GMES(2,1)*WMES(2)
GW(2) = GMES(2,1)*WMES(1) + GMES(2,2)*WMES(2)
CALL FKWVEC(SPRO,WT,CFIL,GW,SFIL)
CALL FKNORM(SFIL,IFAIL)
IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IFAIL/100,IFAIL,IERR)
IF (IFAIL.GE.100) RETURN
* Calculate the chi**2
CHISQ = FKCHXY(WMES,CMES,SFIL,CFIL,IFAIL)
IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IWARN,IFAIL,IERR)
RETURN
END
*