hmath.c

实现HMM算法

   FreeDVector(&gstack, tmp1);
}

/* mat_id -- set A to being closest to identity matrix as possible
        -- i.e. A[i][j] == 1 if i == j and 0 otherwise */
static void InitIdentity(DMatrix A) 
{
   int     i, size;
  
   ZeroDMatrix(A);
   size = minab(NumDRows(A), DVectorSize(A[1]));
   for ( i = 1; i <= size; i++ )
      A[i][i] = 1.0;
}

/* EXPORT->SVD: Calculate the decompostion of matrix A.
   NOTE: on return that U and V hold U' and V' respectively! */
void SVD(DMatrix A, DMatrix U, DMatrix V, DVector d)
{
   DVector f=NULL;
   int i, n;
   DMatrix A_tmp;

   /* do initial size checks! */
   if ( A == NULL )
      HError(1,"svd: Matrix A is null");

   n = NumDRows(A);

   if (U == NULL || V == NULL || d == NULL)
      HError(1, "SVD: The svd matrices and vector must be initialised b4 call");
 
   A_tmp = CreateDMatrix(&gstack, n, n);
   CopyDMatrix(A, A_tmp);
   InitIdentity(U);
   InitIdentity(V);
   f = CreateDVector(&gstack,n-1);

   BiFactor(A_tmp,U,V);
   for ( i = 1; i <= n; i++ ) {
      d[i] = A_tmp[i][i];
      if ( i+1 <= n )
         f[i] = A_tmp[i][i+1];
   }

   BiSVD(d,f,U,V);
   FixSVD(d,U,V);
   FreeDMatrix(&gstack, A_tmp);
}

/* EXPORT->InvSVD: Inverted Singular Value Decomposition (calls SVD)
   and inverse of A is returned in Result */
void InvSVD(DMatrix A, DMatrix U, DVector W, DMatrix V, DMatrix Result)
{
   int m, n, i, j, k;
   double wmax, wmin;
   Boolean isSmall = FALSE;
   DMatrix tmp1;

   m = NumDRows(U);
   n = DVectorSize(U[1]);

   if (m != n)
      HError(1, "InvSVD: Matrix inversion only for symmetric matrices!\n");

   SVD(A, U, V, W);
   /* NOTE U and V actually now hold U' and V' ! */

   tmp1 = CreateDMatrix(&gstack,m, n);

   wmax = 0.0;
   for (k = 1; k <= n; k ++)
      if (W[k] > wmax)
         wmax = W[k];
   wmin = wmax * 1.0e-8;
   for (k = 1; k <= n; k ++)
      if (W[k] < wmin) {
         /* A component of the diag matrix 'w' of the SVD of 'a'
            was smaller than 1.0e-6 and consequently set to zero. */
         if (trace>0) {
            printf("%d (%e) ", k, W[k]); 
            fflush(stdout);
         }
         W[k] = 0.0;
         isSmall = TRUE;
      }
   if (trace>0 && isSmall) {
      printf("\n"); 
      fflush(stdout);
   }
   /* tmp1 will be the product of matrix v and the diagonal 
      matrix of singular values stored in vector w. tmp1 is then
      multiplied by the transpose of matrix u to produce the 
      inverse which is returned */
   for (j = 1; j <= m; j++)
      for (k = 1; k <= n; k ++)
         if (W[k] > 0.0)
            /* Only non-zero elements of diag matrix w are 
               used to compute the inverse. */
            tmp1[j][k] = V[k][j] / W[k];
         else
            tmp1[j][k] = 0.0;

   ZeroDMatrix(Result);
   for (i=1;i<=m;i++)
      for (j=1;j<=m;j++)
         for (k=1;k<=n;k++)
            Result[i][j] += tmp1[i][k] * U[k][j];
   FreeDMatrix(&gstack,tmp1);
}

/* LUDecompose: perform LU decomposition on Matrix a, the permutation
       of the rows is returned in perm and sign is returned as +/-1
       depending on whether there was an even/odd number of row 
       interchanges */
static Boolean LUDecompose(Matrix a, int *perm, int *sign)
{
   int i,imax,j,k,n;
   double scale,sum,xx,yy;
   Vector vv,tmp;
   
   n = NumRows(a); imax = 0;
   vv = CreateVector(&gstack,n);
   *sign = 1;
   for (i=1; i<=n; i++) {
      scale = 0.0;
      for (j=1; j<=n; j++)
         if ((xx = fabs(a[i][j])) > scale )
            scale = xx;
      if (scale == 0.0) {
         HError(-1,"LUDecompose: Matrix is Singular");
	 return(FALSE);
      }
      vv[i] = 1.0/scale;
   }
   for (j=1; j<=n; j++) {
      for (i=1; i<j; i++) {
         sum = a[i][j];
         for (k=1; k<i; k++) sum -= a[i][k]*a[k][j];
         a[i][j]=sum;
      }
      scale=0.0;
      for (i=j; i<=n; i++) {
         sum = a[i][j];
         for (k=1; k<j; k++) sum -= a[i][k]*a[k][j];
         a[i][j]=sum;
         if ( (yy=vv[i]*fabs(sum)) >=scale) {
            scale = yy; imax = i;
         }
      }
      if (j != imax) {
         tmp = a[imax]; a[imax] = a[j]; a[j] = tmp;
         *sign = -(*sign);
         vv[imax]=vv[j];
      }
      perm[j]=imax;
      if (a[j][j] == 0.0) {
         HError(-1,"LUDecompose: Matrix is Singular");
	 return(FALSE);
      }
      if (j != n) {
         yy = 1.0/a[j][j];
         for (i=j+1; i<=n;i++) a[i][j] *= yy;
      }
   }
   return(TRUE);
}


/* EXPORT->MatDet: determinant of a matrix */
float MatDet(Matrix c)
{
   Matrix a;
   float det;
   int n,perm[1600],i,sign;
   
   n=NumRows(c);
   a=CreateMatrix(&gstack,n,n);
   CopyMatrix(c,a);                /* Make a copy of c */
   LUDecompose(a,perm,&sign);      /* Do LU Decomposition */
   det = sign;                     /* Calc Det(c) */
   for (i=1; i<=n; i++) {
      det *= a[i][i];
   }
   FreeMatrix(&gstack,a);
   return det;
}


/* DLUDecompose: perform LU decomposition on Matrix a, the permutation
       of the rows is returned in perm and sign is returned as +/-1
       depending on whether there was an even/odd number of row 
       interchanges */
static Boolean DLUDecompose(DMatrix a, int *perm, int *sign)
{
   int i,imax,j,k,n;
   double scale,sum,xx,yy;
   DVector vv,tmp;
   
   n = NumDRows(a); imax = 0;
   vv = CreateDVector(&gstack,n);
   *sign = 1;
   for (i=1; i<=n; i++) {
      scale = 0.0;
      for (j=1; j<=n; j++)
         if ((xx = fabs(a[i][j])) > scale )
            scale = xx;
      if (scale == 0.0) {
         HError(-1,"LUDecompose: Matrix is Singular");
         return(FALSE);
      }
      vv[i] = 1.0/scale;
   }
   for (j=1; j<=n; j++) {
      for (i=1; i<j; i++) {
         sum = a[i][j];
         for (k=1; k<i; k++) sum -= a[i][k]*a[k][j];
         a[i][j]=sum;
      }
      scale=0.0;
      for (i=j; i<=n; i++) {
         sum = a[i][j];
         for (k=1; k<j; k++) sum -= a[i][k]*a[k][j];
         a[i][j]=sum;
         if ( (yy=vv[i]*fabs(sum)) >=scale) {
            scale = yy; imax = i;
         }
      }
      if (j != imax) {
         tmp = a[imax]; a[imax] = a[j]; a[j] = tmp;
         *sign = -(*sign);
         vv[imax]=vv[j];
      }
      perm[j]=imax;
      if (a[j][j] == 0.0) {
         HError(-1,"LUDecompose: Matrix is Singular");
         return(FALSE);
      }
      if (j != n) {
         yy = 1.0/a[j][j];
         for (i=j+1; i<=n;i++) a[i][j] *= yy;
      }
   }
   return(TRUE);
}


/* EXPORT->DMatDet: determinant of a double matrix */
double DMatDet(DMatrix c)
{
   DMatrix a;
   double det;
   int n,perm[1600],i,sign;
   
   n=NumDRows(c);
   a=CreateDMatrix(&gstack,n,n);
   CopyDMatrix(c,a);                /* Make a copy of c */
   DLUDecompose(a,perm,&sign);      /* Do LU Decomposition */
   det = sign;                     /* Calc Det(c) */
   for (i=1; i<=n; i++) {
      det *= a[i][i];
   }
   FreeDMatrix(&gstack,a);
   return det;
}



/* LinSolve: solve the set of linear equations Ax = b, returning
        the result x in  b */
static void LinSolve(Matrix a, int *perm, float *b)
{
   int i,ii=0,ip,j,n;
   double sum;
   
   n=NumRows(a);
   for (i=1;i<=n;i++) {
      ip=perm[i]; sum=b[ip]; b[ip]=b[i];
      if (ii)
         for (j=ii;j<=i-1;j++) sum -=a[i][j]*b[j];
      else
         if (sum) ii=i;
      b[i]=sum;
   }
   for (i=n; i>=1; i--) {
      sum=b[i];
      for (j=i+1; j<=n; j++)
         sum -=a[i][j]*b[j];
      b[i]=sum/a[i][i];
   }
}        


/* EXPORT-> MatInvert: puts inverse of c in invc, returns Det(c) */
float MatInvert(Matrix c, Matrix invc)
{
   Matrix a;
   float col[100];
   float det;
   int sign;
   int n,i,j,perm[100];
   
   n=NumRows(c);
   a=CreateMatrix(&gstack,n,n);
   CopyMatrix(c,a);           /* Make a copy of c */
   LUDecompose(a,perm,&sign);      /* Do LU Decomposition */
   for (j=1; j<=n; j++) {     /* Invert matrix */
      for (i=1; i<=n; i++)
         col[i]=0.0;
      col[j]=1.0;
      LinSolve(a,perm,col);
      for (i=1; i<=n; i++)
         invc[i][j] = col[i];
   }  
   det = sign;                /* Calc log(det(c)) */
   for (i=1; i<=n; i++) {
      det *= a[i][i];
   }
   FreeMatrix(&gstack,a);
   return det;
}                
 
/* DLinSolve: solve the set of linear equations Ax = b, returning
        the result x in  b */
static void DLinSolve(DMatrix a, int *perm, double *b)
{
   int i,ii=0,ip,j,n;
   double sum;
   
   n=NumDRows(a);
   for (i=1;i<=n;i++) {
      ip=perm[i]; sum=b[ip]; b[ip]=b[i];
      if (ii)
         for (j=ii;j<=i-1;j++) sum -=a[i][j]*b[j];
      else
         if (sum) ii=i;
      b[i]=sum;
   }
   for (i=n; i>=1; i--) {
      sum=b[i];
      for (j=i+1; j<=n; j++)
         sum -=a[i][j]*b[j];
      b[i]=sum/a[i][i];
   }
}       

/* Inverting a double matrix */
double DMatInvert(DMatrix c, DMatrix invc)
{
   DMatrix a;
   double col[100];
   double det;
   int sign;
   int n,i,j,perm[100];
   
   n=NumDRows(c);
   a=CreateDMatrix(&gstack,n,n);
   CopyDMatrix(c,a);           /* Make a copy of c */
   DLUDecompose(a,perm,&sign);      /* Do LU Decomposition */
   for (j=1; j<=n; j++) {     /* Invert matrix */
      for (i=1; i<=n; i++)
         col[i]=0.0;
      col[j]=1.0;
      DLinSolve(a,perm,col);
      for (i=1; i<=n; i++)
         invc[i][j] = col[i];
   }  
   det = sign;                /* Calc log(det(c)) */
   for (i=1; i<=n; i++) {
      det *= a[i][i];
   }
   FreeDMatrix(&gstack,a);
   return det;
}

/* EXPORT-> DMatCofact: generates the cofactors of row r of matrix c */
double DMatCofact(DMatrix c, int r, DVector cofact)
{
   DMatrix a;
   double col[100];
   double det;
   int sign;
   int n,i,perm[100];
   
   n=NumDRows(c);
   a=CreateDMatrix(&gstack,n,n);
   CopyDMatrix(c,a);                      /* Make a copy of c */
   if (! DLUDecompose(a,perm,&sign))      /* Do LU Decomposition */
     return 0;
   det = sign;                         /* Calc det(c) */
   for (i=1; i<=n; i++) {
      det *= a[i][i];
   }
   for (i=1; i<=n; i++)
     col[i]=0.0;
   col[r]=1.0;
   DLinSolve(a,perm,col);
   for (i=1; i<=n; i++)
     cofact[i] = col[i]*det;
   FreeDMatrix(&gstack,a);
   return det;
}

/* EXPORT-> MatCofact: generates the cofactors of row r of matrix c */
double MatCofact(Matrix c, int r, Vector cofact)
{
   DMatrix a;
   DMatrix b;
   double col[100];
   float det;
   int sign;
   int n,i,perm[100];
 
   n=NumRows(c);
   a=CreateDMatrix(&gstack,n,n);
   b=CreateDMatrix(&gstack,n,n);
   Mat2DMat(c,b);
   CopyDMatrix(b,a);                      /* Make a copy of c */
   if (! DLUDecompose(a,perm,&sign))      /* Do LU Decomposition */
     return 0;
   det = sign;                         /* Calc det(c) */
   for (i=1; i<=n; i++) {
      det *= a[i][i];
   }
   for (i=1; i<=n; i++)
     col[i]=0.0;
   col[r]=1.0;
   DLinSolve(a,perm,col);
   for (i=1; i<=n; i++)
     cofact[i] = col[i]*det;
   
   FreeDMatrix(&gstack,b);
   FreeDMatrix(&gstack,a);
   return det;
}

/* -------------------- Log Arithmetic ---------------------- */

/*
  The types LogFloat and LogDouble are used for representing
  real numbers on a log scale.  LZERO is used for log(0) 
  in log arithmetic, any log real value <= LSMALL is 
  considered to be zero.
*/

static LogDouble minLogExp;

/* EXPORT->LAdd: Return sum x + y on log scale, 
                sum < LSMALL is floored to LZERO */
LogDouble LAdd(LogDouble x, LogDouble y)
{
   LogDouble temp,diff,z;
   
   if (x<y) {
      temp = x; x = y; y = temp;
   }
   diff = y-x;
   if (diff<minLogExp) 
      return  (x<LSMALL)?LZERO:x;
   else {
      z = exp(diff);
      return x+log(1.0+z);
   }
}

/* EXPORT->LSub: Return diff x - y on log scale, 
                 diff < LSMALL is floored to LZERO */
LogDouble LSub(LogDouble x, LogDouble y)
{
   LogDouble diff,z;
   
   if (x<y)    
      HError(5271,"LSub: result -ve");
   diff = y-x;
   if (diff<minLogExp) 
      return  (x<LSMALL)?LZERO:x;
   else {
      z = 1.0 - exp(diff);
      return (z<MINLARG) ? LZERO : x+log(z);
   }
}

/* EXPORT->L2F: Convert log(x) to double, result is
                floored to 0.0 if x < LSMALL */
double   L2F(LogDouble x)
{
   return (x<LSMALL) ? 0.0 : exp(x);
}

/* -------------------- Random Numbers ---------------------- */


#ifdef UNIX
/* Prototype for C Library functions drand48 and srand48 */
double drand48(void);
void srand48(long);
#define RANDF() drand48()
#define SRAND(x) srand48(x)
#else
/* if not unix use ANSI C defaults */
#define RANDF() ((float)rand()/RAND_MAX)
#define SRAND(x) srand(x)
#endif

/* EXPORT->RandInit: Initialise random number generators 
           if seed is -ve, then system clock is used */
void RandInit(int seed)
{
   if (seed<0) seed = (int)time(NULL)%257;
   SRAND(seed);
}

/* EXPORT->RandomValue:  */
float RandomValue(void)
{
   return RANDF();
}

/* EXPORT->GaussDeviate: random number with a N(mu,sigma) distribution */
float GaussDeviate(float mu, float sigma)
{
   double fac,r,v1,v2,x;
   static int gaussSaved = 0; /* GaussDeviate generates numbers in pairs */
   static float gaussSave;    /* 2nd of pair is remembered here */


   if (gaussSaved) {
      x = gaussSave; gaussSaved = 0;
   }
   else {
      do {
         v1 = 2.0*(float)RANDF() - 1.0;
         v2 = 2.0*(float)RANDF() - 1.0;
         r = v1*v1 + v2*v2;
      }
      while (r>=1.0);
      fac = sqrt(-2.0*log(r)/r);
      gaussSaved = 1;
      gaussSave = v1*fac;
      x = v2*fac;
   }
   return x*sigma+mu;
}

/* --------------------- Initialisation ---------------------- */

/* EXPORT->InitMath: initialise this module */
void InitMath(void)
{
   int i;

   Register(hmath_version,hmath_vc_id);
   RandInit(-1);
   minLogExp = -log(-LZERO);
   numParm = GetConfig("HMATH", TRUE, cParm, MAXGLOBS);
   if (numParm>0){
      if (GetConfInt(cParm,numParm,"TRACE",&i)) trace = i;
   }
}

/* ------------------------- End of HMath.c ------------------------- */