*-- 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 * * * ********************************************************************** *KEEP,FKECODE. *KEND. ********************************************************************** * Invert CMES CALL FKERR(IUTIL,IROUT,IFATAL,IMCV,IERR) * Compute the filtered (weighted average) covariance CALL FKCVXY(CPRO,GMES,CFIL,WT,IFAIL) CALL FKERR(IUTIL,IROUT,IFATAL,IOCV,IERR) * Compute the filtered state vector CALL FKWVEC(SPRO,WT,CFIL,GW,SFIL) CALL FKNORM(SFIL,IFAIL) IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IFAIL/100,IFAIL,IERR) * Calculate the chi**2 CHISQ = FKCHXY(WMES,CMES,SFIL,CFIL,IFAIL) IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IWARN,IFAIL,IERR) *