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