*-- 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 *