hmath.c

隐马尔科夫模型工具箱

         d[i_min+1] = c*d[i_min+1];
         RotRows(V,i_min,i_min+1,c,s);

         /* 2nd Givens' rotation */
         Givens(d[i_min],z, &c, &s);
         d[i_min]   = c*d[i_min] + s*z;
         d_tmp      = c*d[i_min+1] - s*f[i_min];
         f[i_min]   = s*d[i_min+1] + c*f[i_min];
         d[i_min+1] = d_tmp;
         if ( i_min+1 < i_max ) {
            z          = s*f[i_min+1];
            f[i_min+1] = c*f[i_min+1];
         }
         RotRows(U,i_min,i_min+1,c,s);
      
         for ( i = i_min+1; i < i_max; i++ ) {
            /* get Givens' rotation for zeroing z */
            Givens(f[i-1],z, &c, &s);
            f[i-1] = c*f[i-1] + s*z;
            d_tmp  = c*d[i] + s*f[i];
            f[i]   = c*f[i] - s*d[i];
            d[i]   = d_tmp;
            z      = s*d[i+1];
            d[i+1] = c*d[i+1];
            RotRows(V,i,i+1,c,s);

            /* get 2nd Givens' rotation */
            Givens(d[i],z, &c, &s);
            d[i]   = c*d[i] + s*z;
            d_tmp  = c*d[i+1] - s*f[i];
            f[i]   = c*f[i] + s*d[i+1];
            d[i+1] = d_tmp;
            if ( i+1 < i_max ) {
               z      = s*f[i+1];
               f[i+1] = c*f[i+1];
            }
            RotRows(U,i,i+1,c,s);
         }
         /* should matrix be split? */
         for ( i = i_min; i < i_max; i++ )
            if ( fabs(f[i]) <
                 MACHEPS*(fabs(d[i])+fabs(d[i+1])) )
               {
                  split = TRUE;
                  f[i] = 0.0;
               }
            else if ( fabs(d[i]) < MACHEPS*size )
               {
                  split = TRUE;
                  d[i] = 0.0;
               }
      }
   }
}

/* HholdVec -- calulates Householder vector to eliminate all entries after the
   i0 entry of the vector vec. It is returned as out. May be in-situ */
static void HholdVec(DVector tmp, int i0, int size,
                     double *beta, double *newval)
{
   int i;
   double norm = 0.0;

   for (i = i0; i <= size; i++) {
      norm += tmp[i]*tmp[i];
   }
   norm = sqrt(norm);

   if ( norm <= 0.0 ) {
      *beta = 0.0;
   }
   else {
      *beta = 1.0/(norm * (norm+fabs(tmp[i0])));
      if ( tmp[i0] > 0.0 )
         *newval = -norm;
      else
         *newval = norm;
      tmp[i0] -= *newval;
   }

}


/* HholdTrRows -- transform a matrix by a Householder vector by rows
   starting at row i0 from column j0 -- in-situ */
static void HholdTrRows(DMatrix M, int i0, int j0, DVector hh, double beta)
{
   double ip, scale;
   int i, j;
   int m,n;

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

   if ( M==NULL || hh==NULL )
      HError(1,"HholdTrRows: matrix or vector is NULL!");
   if ( DVectorSize(hh) != n )
      HError(1,"HholdTrRows: hh vector size must = number of M columns");
   if ( i0 > m+1 || j0 > n+1 )
      HError(1,"HholdTrRows: Bounds matrix/vec size error i=%d j=%d m=%d n=%d",
             i0, j0, m, n);
  
   if ( beta != 0.0 ) {
      /* for each row ... */
      for ( i = i0; i <= m; i++ )
         {  
            /* compute inner product */
            /* ip = __ip__(&(M->me[i][j0]),&(hh->ve[j0]),(int)(M->n-j0));*/
            ip = 0.0;
            for ( j = j0; j <= n; j++ )
               ip += M[i][j]*hh[j];
            scale = beta*ip;
            if ( scale == 0.0 )
               continue;
            /* __mltadd__(&(M->me[i][j0]),&(hh->ve[j0]),-scale,
               (int)(M->n-j0)); */
            for ( j = j0; j <= n; j++ )
               M[i][j] -= scale*hh[j];
         }
   }
}

/* HholdTrCols -- transform a matrix by a Householder vector by columns
   starting at row i0 from column j0 -- in-situ */
static void HholdTrCols(DMatrix M, int i0, int j0, 
                        DVector hh, double beta, DVector w)
{
   int i, j;
   int n;

   n = NumDRows(M);

   if ( M==NULL || hh==NULL )
      HError(1,"HholdTrCols: matrix or vector is NULL!");
   if ( DVectorSize(hh) != n )
      HError(1,"HholdTrCols: hh vector size must = number of M columns");
   if ( i0 > n+1 || j0 > n+1 )
      HError(1,"HholdTrCols: Bounds matrix/vec size error i=%d j=%d n=%d",
             i0, j0, n);

   ZeroDVector(w);

   if ( beta != 0.0 ) {

      for ( i = i0; i <= n; i++ )
         if ( hh[i] != 0.0 )
            for ( j = j0; j <= n; j++ )
               w[j] += M[i][j]*hh[i];

      for ( i = i0; i <= n; i++ )
         if ( hh[i] != 0.0 )
            for ( j = j0; j <= n; j++ )
               M[i][j] -= w[j]*beta*hh[i];

   }
}


/* copy a row from a matrix into  a vector */
static void CopyDRow(DMatrix M, int k, DVector v) 
{
   int i, size;
   DVector w;

   if (v == NULL)
      HError(1, "CopyDRow: Vector is NULL");

   size = DVectorSize(v);
   w = M[k];

   for (i = 1; i <= size; i++)
      v[i] = w[i];
}

/* copy a column from a matrix into  a vector */
static void CopyDColumn(DMatrix M, int k, DVector v) 
{
   int i, size;

   if (v == NULL)
      HError(1, "CopyDColumn: Vector is NULL");

   size = DVectorSize(v);
   for (i = 1; i <= size; i++)
      v[i] = M[i][k];
}

/* BiFactor -- perform preliminary factorisation for bisvd
   -- updates U and/or V, which ever is not NULL */
static void BiFactor(DMatrix A, DMatrix U, DMatrix V)
{
   int n, k;
   DVector tmp1, tmp2, tmp3;
   double beta;

   n = NumDRows(A);

   tmp1 = CreateDVector(&gstack, n);
   tmp2 = CreateDVector(&gstack, n);
   tmp3 = CreateDVector(&gstack, n);

   for ( k = 1; k <= n; k++ ) {
      CopyDColumn(A,k,tmp1);
      HholdVec(tmp1,k,n,&beta,&(A[k][k]));
      HholdTrCols(A,k,k+1,tmp1,beta,tmp3);
      if ( U )
         HholdTrCols(U,k,1,tmp1,beta,tmp3);
      if ( k+1 > n )
         continue;
      CopyDRow(A,k,tmp2);
      HholdVec(tmp2,k+1,n,&beta,&(A[k][k+1]));
      HholdTrRows(A,k+1,k+1,tmp2,beta);
      if ( V )
         HholdTrCols(V,k+1,1,tmp2,beta,tmp3);
   }

   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 = min(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 small = 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;
         small = TRUE;
      }
   if (trace>0 && small) {
      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);
}


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