hmath.c

隐马尔科夫模型工具箱

      printf("   ");
      for (j=1;j<=maxj;j++)
         printf("%8.2f ",m[i][j]);
      if (maxj<ncols) printf("...");
      printf("\n");
   }
   if (maxi<nrows)
      printf("   ...\n");
}

/* Export->ShowDMatrix: show the matrix m preceded by title */
void ShowDMatrix(char * title,DMatrix m,int maxCols,int maxRows)
{
   int i,j;
   int maxi,maxj,nrows,ncols;
   
   maxi = nrows = NumDRows(m);
   if (maxi>maxRows) maxi = maxRows;
   maxj = ncols = DVectorSize(m[1]);
   if (maxj>maxCols) maxj = maxCols;
   printf("%s\n",title);
   for (i=1;i<=maxi;i++) {
      printf("   ");
      for (j=1;j<=maxj;j++)
         printf("%10.4f ",m[i][j]);
      if (maxj<ncols) printf("...");
      printf("\n");
   }
   if (maxi<nrows)
      printf("   ...\n");
}

/* Export->ShowTriMat: show the matrix m preceded by title */
void ShowTriMat(char * title,TriMat m,int maxCols,int maxRows)
{
   int i,j;
   int maxi,maxj,size;
   
   size = TriMatSize(m);
   maxi = size;
   if (maxi>maxRows) maxi = maxRows;
   printf("%s\n",title);
   for (i=1;i<=maxi;i++) {
      printf("   ");
      maxj = i;
      if (maxj>maxCols) maxj = maxCols;
      for (j=1;j<=maxj;j++)
         printf("%8.2f ",m[i][j]);
      if (maxj<i) printf("...");
      printf("\n");
   }
   if (maxi<size)
      printf("   ...\n");
}

/* -------------------- Matrix Operations ---------------------- */

/* Choleski: Place lower triangular choleski factor of A in L.*/
/*           Return FALSE if matrix singular or not +definite */
static Boolean Choleski(TriMat A, DMatrix L)
{
   int size,i,j,k;
   double sum;

   size = TriMatSize(A);
   for (i=1; i<=size; i++)
      for (j=1; j<=i; j++) {
         sum=A[i][j];
         for (k=1; k<j; k++)
            sum -= (L[i][k]*L[j][k]);
         if ((i==j)&&(sum<=0.0)) 
            return FALSE;
         else if (i==j)
            sum = sqrt(sum);
         else if (L[j][j]==0.0)
            return FALSE;
         else
            sum /= L[j][j];
         L[i][j] = sum;
      }
   for (i=1; i<=size; i++) 
      for (j=i+1; j<=size; j++) 
         L[i][j] = 0.0;
   return TRUE;
}

/* MSolve: solve Ly=e^i and L^t x = y, where e^i is a unit vector */
static void MSolve(DMatrix L, int i, DVector x, DVector y)
{
   int nr,j,k;
   double sum;
   
   nr=NumDRows(L);
   for (j=1; j<i; j++) y[j] = 0.0;  /* forward sub */
   y[i] = 1.0/L[i][i];
   for (j=i+1; j<=nr; j++){
      sum = 0.0;
      for (k=i; k<j; k++)
         sum -= L[j][k]*y[k];
      y[j] = sum /L[j][j];
   }
   x[nr] = y[nr]/L[nr][nr];         /* backward sub */
   for (j=nr-1; j>=1; j--){
      sum = y[j];
      for (k=j+1; k<=nr; k++)
         sum -= L[k][j]*x[k];
      x[j] = sum / L[j][j];
   }
}

/* EXPORT->CovInvert: puts inverse of c in invc, returns log(Det(c)) */
/*          Note that c must be positive definite */
LogFloat CovInvert(TriMat c, Matrix invc)
{
   DMatrix l;     /* Lower Tri Choleski Matrix */
   DVector x,y;   /* for f/b substitution */
   LogFloat ldet = 0.0;
   int i,j,n;
   Boolean isTri;
   
   n = TriMatSize(c); isTri = IsTriMat(invc);
   l = CreateDMatrix(&gstack,n,n);
   x = CreateDVector(&gstack,n);
   y = CreateDVector(&gstack,n);
   if (Choleski(c,l)){
      for (j=1; j<=n; j++){
         MSolve(l,j,x,y);
         for (i=isTri?j:1; i<=n; i++)
            invc[i][j] = x[i];
         ldet += log(l[j][j]);
      }
   } else
      HError(5220,"CovInvert: [%f ...] not invertible",c[1][1]);
   FreeDMatrix(&gstack,l);    /* cut back stack to entry state */
   return 2.0*ldet;
}

/* EXPORT->CovDet: Returns log(Det(c)), c must be positive definite */
LogFloat CovDet(TriMat c)
{
   DMatrix l;  /* Lower Tri Choleski Matrix */
   LogFloat ldet = 0.0;
   int j,n;
   
   n = TriMatSize(c);
   l = CreateDMatrix(&gstack,n,n);
   if (Choleski(c,l)){
      for (j=1; j<=n; j++)
         ldet += log(l[j][j]);
   } else
      HError(5220,"CovDet: [%f ...] not invertible",c[1][1]);
   FreeDMatrix(&gstack,l);
   return 2.0*ldet;
}

/* Quadratic prod of a full square matrix C and an arbitry full matrix transform A */
void LinTranQuaProd(Matrix Prod, Matrix A, Matrix C)
{
   int i,j,k;
   float tempElem;
   Matrix tempMatrix_A_mult_C;
   
   if (NumRows(C) != NumCols(C)){
      HError(999,"HMath: LinTranQuaProd: Matrix C is not square!\n");
   }
   else {
      tempMatrix_A_mult_C = CreateMatrix(&gstack,NumRows(A),NumCols(C));
      ZeroMatrix(tempMatrix_A_mult_C);
      
      for (i=1;i<=NumRows(tempMatrix_A_mult_C);i++){
         for (j=1;j<=NumCols(tempMatrix_A_mult_C);j++){
            tempElem = 0.0;
            for (k=1;k<=NumCols(A);k++){
               tempElem += A[i][k]*C[j][k];
            }
            tempMatrix_A_mult_C[i][j] = tempElem;
         }
      }
      
      for (i=1;i<=NumRows(Prod);i++){
         for (j=1;j<=i;j++){
            tempElem = 0.0;
            for (k=1;k<=NumCols(tempMatrix_A_mult_C);k++){
               tempElem += tempMatrix_A_mult_C[i][k]*A[j][k];
            }
            Prod[i][j] = tempElem;
         }
      }
      
      for (i=1;i<=NumRows(Prod);i++){
         for (j=1;j>i;j++){
            Prod[i][j] = Prod[j][i];
         }
      }
      
      FreeMatrix(&gstack,tempMatrix_A_mult_C);
   }
}


/* ------------- Singular Value Decomposition --------------- */
/**************************************************************************
 **
 ** Copyright (C) 1993 David E. Steward & Zbigniew Leyk, all rights reserved.
 **
 **                          Meschach Library
 ** 
 ** This Meschach Library is provided "as is" without any express 
 ** or implied warranty of any kind with respect to this software. 
 ** In particular the authors shall not be liable for any direct, 
 ** indirect, special, incidental or consequential damages arising 
 ** in any way from use of the software.
** 
** Everyone is granted permission to copy, modify and redistribute this
** Meschach Library, provided:
**  1.  All copies contain this copyright notice.
**  2.  All modified copies shall carry a notice stating who
**      made the last modification and the date of such modification.
**  3.  No charge is made for this software or works derived from it.  
**      This clause shall not be construed as constraining other software
**      distributed on the same medium as this software, nor is a
**      distribution fee considered a charge.
**
**  Modifications made to conform with HTK formats by 
**  Daniel Kershaw, Entropic Ltd, Cambridge, England.
**
***************************************************************************/
#define MACHEPS 2.22045e-16
#define FZERO 1.0e-6
#define sgn(x)  ((x) >= 0 ? 1 : -1)
#define min(a,b) ((a) > (b) ? (b) : (a))
#define MAX_STACK       100

/* Givens -- returns c,s parameters for Givens rotation to
   eliminate y in the vector [ x y ]' */
static void Givens(double x, double y, double *c, double *s)
{
   double norm;
  
   norm = sqrt(x*x+y*y);
   if ( norm == 0.0 ) {
      *c = 1.0;
      *s = 0.0;       
   }       /* identity */
   else {
      *c = x/norm;
      *s = y/norm;
   }
}


/* RotRows -- premultiply mat by givens rotation described by c,s */
static void RotRows(DMatrix M, int i, int k, 
                    double c, double s)
{
   int   j, n;
   double temp;
  
   n = NumDRows(M);
  
   if (i > n || k > n)
      HError(1, "RotRows: Index tooo big i=%d k=%d\n", i, k);
  
  
   for ( j=1; j<=n; j++ ) {
      temp = c*M[i][j] + s*M[k][j];
      M[k][j] = -s*M[i][j] + c*M[k][j];
      M[i][j] = temp;
   }
  
}

/* FixSVD -- fix minor details about SVD make singular values non-negative
   -- sort singular values in decreasing order */
static void FixSVD(DVector d, DMatrix U, DMatrix V)
{
  
   int  i, j, n;
  
   n = DVectorSize(d);


   /* make singular values non-negative */
   for (i = 1; i <= n; i++) {
      if ( d[i] < 0.0 ) {
         d[i] = - d[i];
         for ( j = 1; j <= NumDRows(U); j++ )
            U[i][j] = - U[i][j];
      }
   }

   return;

#if 0 /* #### ge: what is this code after return supposed to do here? */
   {
      int  k, l, r, stack[MAX_STACK], sp;
      double tmp, v;

   /* sort singular values */
   sp = -1;
   l = 1;       
   r = n;
   for ( ; ; ) {
      while ( r >= l ) {
         /* i = partition(d->ve,l,r) */
         v = d[r];
         i = l-1;           
         j = r;
         for ( ; ; ) {
            /* inequalities are "backwards" for **decreasing** order */
            while ( d[++i] > v );
            while ( d[--j] < v );
            if ( i >= j )
               break;
            /* swap entries in d->ve */
            tmp = d[i];   
            d[i] = d[j];
            d[j] = tmp;
            /* swap rows of U & V as well */
            for ( k = 1; k <= DVectorSize(U[1]); k++ ) {
               tmp = U[i][k];
               U[i][k] = U[j][k];
               U[j][k] = tmp;
            }
            for ( k = 1; k <= DVectorSize(V[1]); k++ ) {
               tmp = V[i][k];
               V[i][k] = V[j][k];
               V[j][k] = tmp;
            }
         }
         tmp = d[i];
         d[i] = d[r];
         d[r] = tmp;
         for ( k = 1; k <= DVectorSize(U[1]); k++ ) {
            tmp = U[i][k];
            U[i][k] = U[r][k];
            U[r][k] = tmp;
         }
         for ( k = 1; k <= DVectorSize(V[1]); k++ ) {
            tmp = V[i][k];
            V[i][k] = V[r][k];
            V[r][k] = tmp;
         }
         /* end i = partition(...) */
         if ( i - l > r - i ) {
            stack[++sp] = l;    
            stack[++sp] = i-1;
            l = i+1;    
         }
         else {
            stack[++sp] = i+1;
            stack[++sp] = r;
            r = i-1;    
         }
      }
      if ( sp < 0 )
         break;
      r = stack[sp--];
      l = stack[sp--];
   }
   }
#endif
}

/* BiSvd -- svd of a bidiagonal m x n matrix represented by d (diagonal) and
   f (super-diagonals) */
static void BiSVD(DVector d, DVector f, DMatrix U, DMatrix V)
{
   int i, j, n;
   int i_min, i_max, split;
   double c, s, shift, size, z;
   double d_tmp, diff, t11, t12, t22;

   if ( ! d || ! f )
      HError(1,"BiSVD: Vectors are null!");
   if ( DVectorSize(d) != DVectorSize(f) + 1 )
      HError(1, "BiSVD: Error with the vector sizes!");
 
   n = DVectorSize(d);
  
   if ( ( U && DVectorSize(U[1]) < n ) || ( V && NumDRows(V) < n ) )
      HError(1, "BiSVD: Error Matrix sizes!");
   if ( ( U && NumDRows(U) != DVectorSize(U[1])) || 
        ( V && NumDRows(V) != DVectorSize(V[1])) )
      HError(1, "BiSVD: One of the matrices must be square");
  
  
   if ( n == 1 )
      return;
    
   s = 0.0;
   for ( i = 1; i <= n; i++)
      s += d[i]*d[i];
   size = sqrt(s);
   s = 0.0;
   for ( i = 1; i < n; i++)
      s += f[i]*f[i];
   size += sqrt(s);
   s = 0.0;
  
   i_min = 1;
   while ( i_min <= n ) {   /* outer while loop */
      /* find i_max to suit;
         submatrix i_min..i_max should be irreducible */
      i_max = n;
      for ( i = i_min; i < n; i++ )
         if ( d[i] == 0.0 || f[i] == 0.0 ) {
            i_max = i;
            if ( f[i] != 0.0 ) {
               /* have to ``chase'' f[i] element out of matrix */
               z = f[i];
               f[i] = 0.0;
               for ( j = i; j < n && z != 0.0; j++ ) {
                  Givens(d[j+1],z, &c, &s);
                  s = -s;
                  d[j+1] =  c*d[j+1] - s*z;
                  if ( j+1 < n ) {
                     z      = s*f[j+1];
                     f[j+1] = c*f[j+1];
                  }
                  RotRows(U,i,j+1,c,s);
               }
            }
            break;
         }    
  

      if ( i_max <= i_min ) {
         i_min = i_max + 1;
         continue;
      }

      split = FALSE;
      while ( ! split ) {
         /* compute shift */
         t11 = d[i_max-1]*d[i_max-1] +
            (i_max > i_min+1 ? f[i_max-2]*f[i_max-2] : 0.0);
         t12 = d[i_max-1]*f[i_max-1];
         t22 = d[i_max]*d[i_max] + f[i_max-1]*f[i_max-1];
         /* use e-val of [[t11,t12],[t12,t22]] matrix
            closest to t22 */
         diff = (t11-t22)/2;
         shift = t22 - t12*t12/(diff +
                                sgn(diff)*sqrt(diff*diff+t12*t12));
      
         /* initial Givens' rotation */
         Givens(d[i_min]*d[i_min]-shift,
                d[i_min]*f[i_min], &c, &s);
      
         /* do initial Givens' rotations */
         d_tmp      = c*d[i_min] + s*f[i_min];
         f[i_min]   = c*f[i_min] - s*d[i_min];
         d[i_min]   = d_tmp;
         z          = s*d[i_min+1];