*-- 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 * * * ********************************************************************** *KEEP,FKECODE. *KEND. * * Common block definitions * *KEEP,FKNPL. * * 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. * * /FKPROJ/ * /FKFILT/ * /FKSMTH/ * /FKINT/ * /FKRSID/ * /FKTRUE/ * /FKDBG/ *KEEP,FKMEAS. *KEEP,FKFLAG. *KEEP,FKPROJ. *KEEP,FKFILT. *KEEP,FKRSID. *KEND. ********************************************************************** * * Local arrays ... * ********************************************************************** * * Initialisation and checks ... * * has projection to JPL been done ...... if not, terminate! CALL FKERR(IUTIL,IROUT,IFATAL,IINF1,IERR) * 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. * CALL UCOPY(SPRO(1,JPL),SFIL(1,JPL),10) CALL FKCOPY(CPRO(1,1,JPL),CFIL(1,1,JPL)) CALL VZERO(RPRO(1,JPL),4) CALL VZERO(CRPRO(1,1,JPL),8) CALL VZERO(RFIL(1,JPL),4) CALL VZERO(CRFIL(1,1,JPL),8) ********************************************************************** * Invert CMES CALL FKINV(MES(JPL),CMES(1,1,JPL),GMES,IFAIL) CALL FKERR(IUTIL,IROUT,IFATAL,IMCV,IERR) * Compute the filtered (weighted average) covariance CALL FKCOVP(CPRO(1,1,JPL),HMES(1,1,JPL),GMES, CALL FKCOVR(CPRO(1,1,JPL),HMES(1,1,JPL),GMES, CALL FKERR(IUTIL,IROUT,IFATAL,IOCV,IERR) * 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) * Calculate the residuals of the prediction IF (LRPRO) CALL FKLRSD(JPL,SPRO(1,JPL),CPRO(1,1,JPL),2, * Calculate the filtered residuals CALL FKLRSD(JPL,SFIL(1,JPL),CFIL(1,1,JPL),-3, IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IWARN,IFAIL,IERR) * Set the flag to show filter has been done *