      SUBROUTINE NLLSFD(Q,TQ,NI,NF,NFREQ,FREQ,AMP,PHI,FDOT,SIGMAF,
     *                  SIGMAA,SIGMAP,SGFDOT,IPRINT,MD)     
C     
C     THIS ROUTINE CALCULATES THE BEST VALUES OF AMP,FREQ,PHI     
C     AND FDOT FOR NFREQ SINUSOIDS  
C     
C     THE TAYLOR EXPANSION NON-LINEAR LEAST SQUARES TECHNIQUE IS USED   
C     
      double precision work1(40,40)
      DOUBLE PRECISION COV(40,40),COVINV(40,40),Y(40),DEL(40),CM(40,40) 
      DOUBLE PRECISION DERIV(40),UNIT(40,40),PARMS(40),V(2) 
      DIMENSION JUNK(40),Q(NF),iwork2(40)
      DOUBLE PRECISION TQ(NF)
      DIMENSION FREQ(1),AMP(1),PHI(1),SIGMAF(1),SIGMAA(1),SIGMAP(1)     
      DIMENSION FDOT(1),SGFDOT(1)   
      DOUBLE PRECISION TWOPI,ARG,TI,QI,SI,CI,Q0 
      IF(NFREQ.GT.10)THEN
        PRINT 99,NFREQ
   99   FORMAT(//12X,'NUMBER OF FREQUENCIES REQUESTED IS ',I5/
     *           12X,'THIS IS GREATER THAN THE 10 ALLOWED FOR ',
     *           'THIS ROUTINE'/12X,'TALK TO DOD')
        STOP
      END IF
      TWOPI=2.0*3.141592653D0 
      N=NF-NI+1   
      IDEBUG=0    
      IF(MD.LT.0)IDEBUG=1     
      IPRINT=1    
      SIG=0.0     
      NUM=4*NFREQ 
      DO 1 J=1,NFREQ    
        IND=(J-1)*4     
        PARMS(IND+1)=AMP(J)   
        PARMS(IND+2)=FREQ(J)  
        PARMS(IND+3)=PHI(J)   
        PARMS(IND+4)=0.0D0    
    1 CONTINUE    
      ICONV=0     
      ITER=0
      SIG=0.0     
      DO 5 I=NI,NF
        TI=TQ(I)  
        QI=Q(I)   
        Q0=0.0    
        DO 3 J=1,NFREQ  
          IND=(J-1)*4   
          ARG=DMOD(TWOPI*(PARMS(IND+2)+PARMS(IND+4)*TI)*TI+PARMS(IND+3) 
     *            ,TWOPI)     
          CI=DCOS(ARG)  
          Q0=Q0+PARMS(IND+1)*CI     
    3   CONTINUE  
        SIG=SIG+(QI-Q0)**2    
    5 CONTINUE    
      SIG=SIG/(N-NUM)   
      IF(IPRINT.EQ.1)PRINT 8,NI,NF  
    8 FORMAT(12X,'NON-LINEAR LEAST SQUARES FIT OF POINTS ',I5,' TO ',I5)
      IF(IPRINT.EQ.1.AND.NFREQ.GT.0)PRINT 10,NFREQ,   
     *  (FREQ(J),AMP(J),PHI(J),J=1,NFREQ) 
   10 FORMAT(/12X,I2,' SINUSOIDS INCLUDED IN THE FIT WITH'/ 
     *       12X,'STARTING VALUES GIVEN BY :'// 
     *       12X,6X,'FREQUENCY',5X,6X,'AMPLITUDE',15X,'PHASE'/    
     *       12X,55('-')/(12X,3(1PE15.8,5X)))   
      PRINT 12    
   12 FORMAT(12X,55('-')////12X,'ITER',5X,'AVE SIG**2'/12X,79('-'))     
      PRINT 225,ITER,SIG,(PARMS(J),J=1,NUM)     
   15 ITER=ITER+1 
      DO 20 I=1,NUM     
        DEL(I)=0.0
        Y(I)=0.0  
        DO 20 J=1,NUM   
          CM(I,J)=0.0   
          COV(I,J)=0.0  
   20 CONTINUE    
C     
C     CALCULATE THE NORMAL EQUATION MATRIX AND Q VECTOR     
C     
      DO 50 I=NI,NF     
        TI=TQ(I)  
        QI=Q(I)   
        Q0=0.0    
        DO 30 J=1,NFREQ 
          IND=(J-1)*4   
          ARG=DMOD(TWOPI*(PARMS(IND+2)+PARMS(IND+4)*TI)*TI+PARMS(IND+3) 
     *            ,TWOPI)     
          SI=DSIN(ARG)  
          CI=DCOS(ARG)  
          DERIV(IND+1)=CI     
          DERIV(IND+2)=-PARMS(IND+1)*TWOPI*TI*SI
          DERIV(IND+3)=-PARMS(IND+1)*SI   
          DERIV(IND+4)=-PARMS(IND+1)*SI*TWOPI*TI*TI   
          Q0=Q0+PARMS(IND+1)*CI     
   30   CONTINUE  
        DO 40 J=1,NUM   
          DO 35 K=1,NUM 
            COV(J,K)=COV(J,K)+DERIV(J)*DERIV(K) 
   35     CONTINUE
          Y(J)=Y(J)+(QI-Q0)*DERIV(J)
   40   CONTINUE  
   50 CONTINUE    
C     
C     INVERT THE NORMAL MATRIX 
C     
      DO 60 J=1,NUM     
      DO 60 K=1,NUM     
   60 COVINV(J,K)=COV(J,K)    
      V(1)=1.0    

c     Invert covinv; inverse appears in work1

      do 800 i=1,num
        do 801 jj=1,num
 801    work1(i,jj)=0.
 800  work1(i,i)=1.
      call ludcmp(covinv,num,40,iwork2,work2)
      do 802 i=1,num
        call lubksb(covinv,num,40,iwork2,work1(1,i))
 802  continue


c     Put the inverse matrix into COVINV
      do i = 1,num
         do j = 1,num
            covinv(i,j) = work1(i,j)
         enddo
      enddo
   62 continue
      IDEBUG=0
C     
C     FROM HERE DOWN TO ABOUT LABEL 135 ARE DEBUG CALCULATIONS & PRINTS 
C     
   65 IF(IDEBUG.EQ.1)THEN     
        PRINT 70  
   70   FORMAT(//' COVARIANCE MATRIX IS : '/)   
        DO 90 I=1,NUM   
          PRINT 80,(COV(I,J),J=1,NUM)     
   80     FORMAT(1X,1P10E13.4)
          PRINT 80
   90   CONTINUE  
        PRINT 95  
   95   FORMAT(///' INVERSE IS : '/)
        DO 100 I=1,NUM  
          PRINT 80,(COVINV(I,J),J=1,NUM)  
          PRINT 80
  100   CONTINUE  
        PRINT 105 
  105   FORMAT(///' THE Y VECTOR IS : '/) 
        PRINT 80,(Y(J),J=1,NUM)     
        PRINT 80  
C     
C       TEST FOR GOOD MATRIX INVERSION BY PRODUCING A UNIT MATRIX 
C       AND PRINTING IT AND THEN CALCULATE THE CONTRIBUTION TO THE
C       SOLUTIONS BY CALCULATING A MATRIX GIVEN BY COVINV(I,J)*Y(J)     
C     
        PRINT 120 
        DO 125 I=1,NUM  
          DO 115 K=1,NUM
            UNIT(I,K)=0.0     
            DO 110 J=1,NUM    
              UNIT(I,K)=UNIT(I,K)+COVINV(I,J)*COV(J,K)
  110       CONTINUE    
  115     CONTINUE
  120     FORMAT(///' PRODUCT OF COVARIANCE AND ITS INVERSE IS : '/)    
          PRINT 80,(UNIT(I,K),K=1,NUM)    
          PRINT 80
  125   CONTINUE  
        PRINT 128 
  128   FORMAT(///' PRODUCT OF COVARIANCE INVERSE AND Y VECTOR :'/)     
        DO 135 I=1,NUM  
          DO 130 J=1,NUM
            UNIT(I,J)=COVINV(I,J)*Y(J)    
  130     CONTINUE
          PRINT 80,(UNIT(I,J),J=1,NUM)    
          PRINT 80
  135   CONTINUE  
      END IF
C     
C     CALCULATE THE CORRECTIONS TO PARMS AND STORE IN DEL   
C     
      DO 140 I=1,NUM    
        DO 140 J=1,NUM  
          DEL(I)=DEL(I)+COVINV(I,J)*Y(J)  
  140 CONTINUE    
C     
C     APPLY CORRECTIONS 
C     
      DO 145 I=1,NUM    
  145 PARMS(I)=PARMS(I)+DEL(I)
C     
C     CALCULATE THE NEW SIGMA**2 OF THE RESIDUALS     
C     
      SIGNEW=0.0  
      DO 150 I=NI,NF    
        TI=TQ(I)  
        QI=Q(I)   
        Q0=0.0    
        DO 146 J=1,NFREQ
          IND=(J-1)*4   
          ARG=DMOD(TWOPI*(PARMS(IND+2)+PARMS(IND+4)*TI)*TI+PARMS(IND+3) 
     *            ,TWOPI)     
          CI=DCOS(ARG)  
          Q0=Q0+PARMS(IND+1)*CI     
  146   CONTINUE  
        SIGNEW=SIGNEW+(QI-Q0)**2    
  150 CONTINUE    
      SIGNEW=SIGNEW/(N-NUM)   
C     
C     CALCULATE THE CORRELATION MATRIX FOR THE ERRORS 
C     
      DO 180 I=1,NUM    
        CM(I,I)=COVINV(I,I)*SIGNEW  
  180 CONTINUE    
      DO 200 I=1,NUM    
      DO 200 J=1,I-1    
  200 CM(I,J)=COVINV(I,J)/SQRT(COVINV(I,I)*COVINV(J,J))     
C     
C     CHECK FOR CONVERGENCE   
C     
      ERROR=1.0E-7
      DELSIG=ABS(SIGNEW-SIG)/SIG    
      IF(DELSIG.LE.ERROR)ICONV=1    
      SIG=SIGNEW  
C     
C     PRINT RESULTS SECTION   
C     
      IF(IPRINT.EQ.1)THEN     
        IF(ICONV.EQ.1)PRINT 210,ITER
  210   FORMAT(/12X,'*** CONVERGENCE AFTER ',I3,' ITERATIONS ***'/)     
        IF(ITER.GT.49)PRINT 220
  220   FORMAT(/12X,'*** NO CONVERGENCE AFTER 50 ITERATIONS ***'/)
        PRINT 225,ITER,SIG,(PARMS(J),J=1,NUM)   
  225   FORMAT(12X,I4,1P5E15.6/(31X,4E15.6))    
        IF(ICONV.EQ.1.OR.ITER.GT.49)PRINT 230    
  230   FORMAT(12X,79('-')//) 
      END IF
      IF(ICONV.EQ.1.OR.ITER.GT.49)THEN     
        IF(MD.EQ.1)THEN 
C     
C         PREWHITEN THE DATA VIA THE FINAL SOLUTION   
C     
          DO 235 I=NI,NF
            TI=TQ(I)    
            QI=Q(I)     
            Q0=0.0
            DO 232 J=1,NFREQ  
              IND=(J-1)*4     
              ARG=DMOD(TWOPI*(PARMS(IND+2)+PARMS(IND+4)*TI)*TI    
     *                 +PARMS(IND+3),TWOPI)     
              CI=DCOS(ARG)    
              Q0=Q0+PARMS(IND+1)*CI 
  232       CONTINUE    
            Q(I)=Q(I)-Q0
  235     CONTINUE
        END IF    
C     
C       CALCULATE ERRORS ON PARMS   
C     
        DO 240 J=1,NFREQ
          IND=(J-1)*4   
          AMP(J)=PARMS(IND+1) 
          FREQ(J)=PARMS(IND+2)
          PHI(J)=PARMS(IND+3) 
          FDOT(J)=PARMS(IND+4)
          SIGMAA(J)=SQRT(CM(IND+1,IND+1)) 
          SIGMAF(J)=SQRT(CM(IND+2,IND+2)) 
          SIGMAP(J)=SQRT(CM(IND+3,IND+3)) 
          SGFDOT(J)=SQRT(CM(IND+4,IND+4)) 
  240   CONTINUE  
        IF(IPRINT.EQ.1)PRINT 245    
  245   FORMAT(12X,'FINAL VALUES : '//12X,33X,'SIGMA'/12X,38('-'))
        IF(IPRINT.EQ.1.AND.NFREQ.GT.0)PRINT 250,(AMP(J),SIGMAA(J),
     *  FREQ(J),SIGMAF(J),PHI(J),SIGMAP(J),FDOT(J),SGFDOT(J),J=1,NFREQ) 
  250   FORMAT(12X,' AMP',5X,1PE15.8,5X,E9.1/   
     *         12X,'FREQ',5X,E15.8,5X,E9.1/     
     *         12X,' PHI',5X,E15.8,5X,E9.1/     
     *         12X,'FDOT',5X,E15.8,5X,E9.1)     
        PRINT 254,SQRT(SIG)   
  254   FORMAT(12X,38('-')//12X,'STANDARD DEVIATION OF RESIDUALS IS ',  
     *         1PE14.6/)
        IF(IPRINT.EQ.1)THEN   
          PRINT 255     
  255     FORMAT(12X,'CORRELATION COEFFICIENTS ARE :'/)     
          DO 270 I=2,NUM
            PRINT 260,(CM(I,J),J=1,I-1)   
  260       FORMAT(12X,15F8.3)
  270     CONTINUE
        END IF    
        RETURN    
      END IF
C     
C     RETURN FOR THE NEXT ITERATION 
C     
      GO TO 15    
      END   
