*-- Author : Stephen Burke
SUBROUTINE FKLSSM(Z,DZ,SVEC,CVEC,SPRO,QGAIN,QPRO,SSMT,CSMT,IERR)
*-----------------------------------------Updates 07/02/92-------
**: FKLSSM 30205.SB. Remove unused FKECODE sequence.
*-----------------------------------------Updates----------------
**********************************************************************
* *
* Simple Kalman Smoothing *
* *
* ERROR CONDITIONS; *
* IERR = 0 ; normal termination *
* IERR = 116 ; x, y or theta has silly value *
* IERR = 17 ; tan(theta) > 1 (warning) *
* *
**********************************************************************
**********************************************************************
* Adjust the old smoothed vector to allow for MS ...
CALL FKADJ(SVEC,SPRO,QGAIN,SADJ)
CALL FKNORM(SADJ,IFAIL)
IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IFAIL/100,IFAIL,IERR)
* ... translate to the new position ...
CALL FKTRAN(DZ,Z,SADJ,SSMT,DINV)
CALL FKNORM(SSMT,IFAIL)
IF (IFAIL.NE.0) CALL FKERR(IUTIL,IROUT,IFAIL/100,IFAIL,IERR)
* ... and calculate the new covariance
CALL FKDMQD(DINV,QGAIN,AGAIN)
CALL FKMXM(CVEC,AGAIN,CSMT)
CALL FKDQA(DINV,QPRO,AGAIN,CSMT)
*