*-- Author : S.Burke / J.V. Morris
SUBROUTINE FKCOVP(CPRO,H,GMES,CFIL,WT,IERR)
**********************************************************************
* *
* Calculate filtered covariance for a `planar' (i.e. only one *
* measurement) *
* *
* ERROR CONDITIONS; *
* IERR = 0 ; normal termination *
* -> IERR = 111 ; filtered covariance not positive definite *
* *
* -> Fatal error *
* *
**********************************************************************
IMPLICIT DOUBLE PRECISION (A-H,O-Z)
PARAMETER (IUTIL=1,IROUT=1)
*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 CPRO(5,5),H(2,2),GMES(2,2),CFIL(5,5),WT(2,5)
**********************************************************************
IERR = 0
IF (H(2,1).EQ.1.D0) GOTO 1000
IF (H(2,2).EQ.1.D0) GOTO 2000
* Rotate the top left corner of CPRO into measurement space ...
CP11 = (H(1,1)*CPRO(1,1) + 2.*H(1,2)*CPRO(2,1))*H(1,1)
& + H(1,2)*CPRO(2,2) *H(1,2)
CP12 = (H(2,1)*CPRO(1,1) + H(2,2)*CPRO(2,1))*H(1,1) +
& (H(2,1)*CPRO(2,1) + H(2,2)*CPRO(2,2))*H(1,2)
CP13 = H(1,1)*CPRO(3,1) + H(1,2)*CPRO(3,2)
CP14 = H(1,1)*CPRO(4,1) + H(1,2)*CPRO(4,2)
CP15 = H(1,1)*CPRO(5,1) + H(1,2)*CPRO(5,2)
* Premultiply by GMES, add the unit matrix and invert - all trivial ...
DET = GMES(1,1)*CP11 + 1.D0
IF (DET.LE.0.D0) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IOCV,IERR)
RETURN
ENDIF
DET = 1.D0/DET
GDET = GMES(1,1)*DET
GCP12 = -CP12*GDET
GCP13 = -CP13*GDET
GCP14 = -CP14*GDET
GCP15 = -CP15*GDET
* ... except for rotating the result back into state vector space ...
GH11 = DET*H(1,1) + GCP12*H(2,1)
GH12 = DET*H(1,2) + GCP12*H(2,2)
WT(1,1) = H(1,1)*GH11 + H(2,1)*H(2,1)
WT(1,2) = H(1,1)*GH12 + H(2,1)*H(2,2)
WT(1,3) = H(1,1)*GCP13
WT(1,4) = H(1,1)*GCP14
WT(1,5) = H(1,1)*GCP15
WT(2,1) = H(1,2)*GH11 + H(2,2)*H(2,1)
WT(2,2) = H(1,2)*GH12 + H(2,2)*H(2,2)
WT(2,3) = H(1,2)*GCP13
WT(2,4) = H(1,2)*GCP14
WT(2,5) = H(1,2)*GCP15
* ... and finally, premultiply by CPRO
CFIL(1,1) = CPRO(1,1)*WT(1,1) + CPRO(2,1)*WT(2,1)
CFIL(2,1) = CPRO(2,1)*WT(1,1) + CPRO(2,2)*WT(2,1)
CFIL(2,2) = CPRO(2,1)*WT(1,2) + CPRO(2,2)*WT(2,2)
CFIL(3,1) = CPRO(3,1)*WT(1,1) + CPRO(3,2)*WT(2,1)
CFIL(3,2) = CPRO(3,1)*WT(1,2) + CPRO(3,2)*WT(2,2)
CFIL(3,3) = CPRO(3,1)*WT(1,3) + CPRO(3,2)*WT(2,3) + CPRO(3,3)
CFIL(4,1) = CPRO(4,1)*WT(1,1) + CPRO(4,2)*WT(2,1)
CFIL(4,2) = CPRO(4,1)*WT(1,2) + CPRO(4,2)*WT(2,2)
CFIL(4,3) = CPRO(4,1)*WT(1,3) + CPRO(4,2)*WT(2,3) + CPRO(4,3)
CFIL(4,4) = CPRO(4,1)*WT(1,4) + CPRO(4,2)*WT(2,4) + CPRO(4,4)
CFIL(5,1) = CPRO(5,1)*WT(1,1) + CPRO(5,2)*WT(2,1)
CFIL(5,2) = CPRO(5,1)*WT(1,2) + CPRO(5,2)*WT(2,2)
CFIL(5,3) = CPRO(5,1)*WT(1,3) + CPRO(5,2)*WT(2,3) + CPRO(5,3)
CFIL(5,4) = CPRO(5,1)*WT(1,4) + CPRO(5,2)*WT(2,4) + CPRO(5,4)
CFIL(5,5) = CPRO(5,1)*WT(1,5) + CPRO(5,2)*WT(2,5) + CPRO(5,5)
RETURN
1000 CONTINUE
* For 1/3 of the planars there is no rotation, and the rest is trivial
DET = GMES(1,1)*CPRO(2,2) + 1.D0
IF (DET.LE.0.D0) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IOCV,IERR)
RETURN
ENDIF
DET = 1.D0/DET
GDET = GMES(1,1)*DET
WT(1,1) = 1.D0
WT(1,2) = 0.D0
WT(1,3) = 0.D0
WT(1,4) = 0.D0
WT(1,5) = 0.D0
WT(2,1) = -GDET*CPRO(2,1)
WT(2,2) = DET
WT(2,3) = -GDET*CPRO(3,2)
WT(2,4) = -GDET*CPRO(4,2)
WT(2,5) = -GDET*CPRO(5,2)
CFIL(1,1) = CPRO(1,1) + CPRO(2,1)*WT(2,1)
CFIL(2,1) = CPRO(2,1) + CPRO(2,2)*WT(2,1)
CFIL(2,2) = CPRO(2,2)*WT(2,2)
CFIL(3,1) = CPRO(3,1) + CPRO(3,2)*WT(2,1)
CFIL(3,2) = CPRO(3,2)*WT(2,2)
CFIL(3,3) = CPRO(3,2)*WT(2,3) + CPRO(3,3)
CFIL(4,1) = CPRO(4,1) + CPRO(4,2)*WT(2,1)
CFIL(4,2) = CPRO(4,2)*WT(2,2)
CFIL(4,3) = CPRO(4,2)*WT(2,3) + CPRO(4,3)
CFIL(4,4) = CPRO(4,2)*WT(2,4) + CPRO(4,4)
CFIL(5,1) = CPRO(5,1) + CPRO(5,2)*WT(2,1)
CFIL(5,2) = CPRO(5,2)*WT(2,2)
CFIL(5,3) = CPRO(5,2)*WT(2,3) + CPRO(5,3)
CFIL(5,4) = CPRO(5,2)*WT(2,4) + CPRO(5,4)
CFIL(5,5) = CPRO(5,2)*WT(2,5) + CPRO(5,5)
RETURN
2000 CONTINUE
*
* Same as the above, for a different wire alignment (I think this
* is the one which actually occurs).
*
DET = GMES(1,1)*CPRO(1,1) + 1.D0
IF (DET.LE.0.D0) THEN
CALL FKERR(IUTIL,IROUT,IFATAL,IOCV,IERR)
RETURN
ENDIF
DET = 1.D0/DET
GDET = GMES(1,1)*DET
WT(1,1) = DET
WT(1,2) = -CPRO(2,1)*GDET
WT(1,3) = -CPRO(3,1)*GDET
WT(1,4) = -CPRO(4,1)*GDET
WT(1,5) = -CPRO(5,1)*GDET
WT(2,1) = 0.D0
WT(2,2) = 1.D0
WT(2,3) = 0.D0
WT(2,4) = 0.D0
WT(2,5) = 0.D0
* ... and finally, premultiply by CPRO
CFIL(1,1) = CPRO(1,1)*WT(1,1)
CFIL(2,1) = CPRO(2,1)*WT(1,1)
CFIL(2,2) = CPRO(2,1)*WT(1,2) + CPRO(2,2)
CFIL(3,1) = CPRO(3,1)*WT(1,1)
CFIL(3,2) = CPRO(3,1)*WT(1,2) + CPRO(3,2)
CFIL(3,3) = CPRO(3,1)*WT(1,3) + CPRO(3,3)
CFIL(4,1) = CPRO(4,1)*WT(1,1)
CFIL(4,2) = CPRO(4,1)*WT(1,2) + CPRO(4,2)
CFIL(4,3) = CPRO(4,1)*WT(1,3) + CPRO(4,3)
CFIL(4,4) = CPRO(4,1)*WT(1,4) + CPRO(4,4)
CFIL(5,1) = CPRO(5,1)*WT(1,1)
CFIL(5,2) = CPRO(5,1)*WT(1,2) + CPRO(5,2)
CFIL(5,3) = CPRO(5,1)*WT(1,3) + CPRO(5,3)
CFIL(5,4) = CPRO(5,1)*WT(1,4) + CPRO(5,4)
CFIL(5,5) = CPRO(5,1)*WT(1,5) + CPRO(5,5)
RETURN
END
*