jns.c

独立成分分析的批数据处理算法JADE,计算量虽然大些

/* ================================================================== */
/* 

   Implements the Jade and the Shibbs algorithms

   Copyright: JF Cardoso.  [e-mail]

   This is essentially my first C program.  Educated comments are more
   than welcome. 

   version 1.2   Jun. 05, 2002.
   Version 1.1   Jan. 20, 1999.

   Changes wrt 1.1
     o Minor fix for new versions of mex (see History)

   Changes wrt 1.0
     o Switched to Matlab-wise vectorization of matrices 
     o Merged a few subroutines into their callers
     o Implemented more C tricks to make the code more unscrutable
     o Changed the moment estimating routines to prepare for a
       read-from-file operation (the sensor loops are nested inside
       the sample loops)
     o Limited facility to control verbosity levels.  Messages directed
       to sterr.


 To do: 
   o Address the convergence problem of Shibbs on (e.g.) Gaussian data.
   o Control of convergence may/should be based on the variation of the objective
     rather than one the size of the rotations (see above item).   
   o Smarter use of floating types: short for the data, long when 
     during moment estimation (issue of error accumulation).
   o An `out of memory' should return an error code rather than exiting.

*/
/* ================================================================== */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "Matutil.h"


#define VERBOSITY 0

#define RELATIVE_JD_THRESHOLD  1.0e-4
/* A `null' Jacobi rotation for joint diagonalization is smaller than
   RELATIVE_JD_THRESHOLD/sqrt(T) where T is the number of samples */

#define RELATIVE_W_THRESHOLD  1.0e-12
/* A null Jacobi rotation for the whitening is smaller than
   RELATIVE_W_THRESHOLD/sqrt(T) where T is the number of samples */


void OutOfMemory() 
{
  printf("Out of memory, sorry...\n");
  exit(EXIT_FAILURE) ;
}


#define SPACE_PER_LEVEL 3

void Message0(int level, char *mess) {
  int count ;
  if (level < VERBOSITY) {
    for (count=0; count<level*SPACE_PER_LEVEL; count++) fprintf(stderr," ");
    fprintf(stderr, mess);
  }
}
void MessageF(int level, char *mess, double value) {
  int count ;
  if (level < VERBOSITY) {
    for (count=0; count<level*SPACE_PER_LEVEL; count++) fprintf(stderr," ");
    fprintf(stderr, mess, value);
  }
}
void MessageI(int level, char *mess, int value) {
  int count ;
  if (level < VERBOSITY) {
    for (count=0; count<level*SPACE_PER_LEVEL; count++) fprintf(stderr," ");
    fprintf(stderr, mess, value);
  }
}

void Identity (double *Mat, int p)
{
  int i;
  int p2 = p*p ;
  for (i=0;i<p2;i++) Mat[i]     = 0.0 ;
  for (i=0;i<p ;i++) Mat[i+i*p] = 1.0 ;
}


/* How far from identity ? Ad hoc answer */
double NonIdentity (double *Mat, int p)
{
  int i,j;
  double point ;
  double sum  = 0.0 ;
  for (i=0;i<p;i++)
    for (j=0;j<p;j++) {
      point = Mat[i*p+j] ;
      if (i!=j) sum += point*point ; 
      else      sum += (point-1.0)*(point-1.0) ; 
    }
  return sum ;
}


/* X=Trans*X : computes IN PLACE the transformation X=Trans*X.  X: nxT, Trans: nxn */
void Transform (double *X, double *Trans, int n, int T)  
{
  double *Tx ; /* buffer for a column vector */
  int i,s,t ;
  int Xind, Xstart, Xstop ;
  double sum ;

  Tx = (double *) calloc(n, sizeof(double)) ; 
  if (Tx == NULL) OutOfMemory() ;

  for (t=0; t<T; t++)
    {
      Xstart = t * n ;
      Xstop  = Xstart + n ;

      /* stores in Tx the t-th colum of X transformed by Trans */
      for (i=0; i<n ; i++) {
	sum = 0.0 ;
	for (s=i, Xind=Xstart; Xind<Xstop; s+=n, Xind++)
	  sum += Trans[s]*X[Xind] ;
	Tx[i]=sum ;
	}

      /* plugs the transformed vector back in the orignal matrix */
      for (i=0, Xind=Xstart; i< n; i++, Xind++) 
	X[Xind]=Tx[i] ;
    }
  free(Tx) ;
}


void EstCovMat(double *R, double *A, int m, int T)
{
  int i, j, t ;
  double *x ;
  double ust = 1.0 / (double) T ;

  for (i=0; i<m; i++)
    for (j=i; j<m; j++)
      R[i+j*m] = 0.0 ;
  
  for (t=0, x=A; t<T; t++, x+=m)
    for (i=0; i<m; i++)
      for (j=i; j<m; j++)
	R[i+j*m] += x[i]* x[j]; 
  
  for (i=0; i<m; i++)
    for (j=i; j<m; j++) {
      R[i+j*m] = ust * R[i+j*m] ;
      R[j+i*m] = R[i+j*m] ;
    }
}  


/* rem: does not depend on n,of course: remove the argument */
/* A(mxn) --> A(mxn) x R where R=[ c s ; -s c ]  rotates the (p,q) columns of R */
void RightRotSimple(double *A, int m, int n, int p, int q, double c, double s )
{
  double nx, ny ;
  int ix = p*m ;
  int iy = q*m ;
  int i ;
  
  for (i=0; i<m; i++) {
    nx = A[ix] ;
    ny = A[iy] ;
    A[ix++] = c*nx - s*ny ;
    A[iy++] = s*nx + c*ny ;
  }
}

/* Ak(mxn) --> Ak(mxn) x R where R rotates the (p,q) columns R =[ c s ; -s c ] 
   and Ak is the k-th M*N matrix in the stack */
void RightRotStack(double *A, int M, int N, int K, int p, int q, double c, double s ) 
{ 
  int k, ix, iy, cpt, kMN ; 
  int pM = p*M ;
  int qM = q*M ;
  double nx, ny ; 

  for (k=0, kMN=0; k<K; k++, kMN+=M*N)
    for ( cpt=0, ix=pM+kMN, iy=qM+kMN; cpt<M; cpt++) { 
      nx = A[ix] ; 
      ny = A[iy] ; 
      A[ix++] = c*nx - s*ny ; 
      A[iy++] = s*nx + c*ny ; 
    } 
}


/* 
   A(mxn) --> R * A(mxn) where R=[ c -s ; s c ]   rotates the (p,q) rows of R 
*/
void LeftRotSimple(double *A, int m, int n, int p, int q, double c, double s )
{
  int ix = p ;
  int iy = q ;
  double nx, ny ;
  int j ;
  
  for (j=0; j<n; j++, ix+=m, iy+=m) {
    nx = A[ix] ;
    ny = A[iy] ;
    A[ix] = c*nx - s*ny ;
    A[iy] = s*nx + c*ny ;
  }
}


/* 
   Ak(mxn) --> R * Ak(mxn) where R rotates the (p,q) rows R =[ c -s ; s c ]  
   and Ak is the k-th matrix in the stack
*/
void LeftRotStack(double *A, int M, int N, int K, int p, int q, double c, double s )
{
  int k, ix, iy, cpt ;
  int MN = M*N ;
  int kMN ;
  double nx, ny ;

  for (k=0, kMN=0; k<K; k++, kMN+=MN)
    for (cpt=0, ix=p+kMN, iy=q+kMN; cpt<N; cpt++, ix+=M, iy+=M) {
      nx = A[ix] ;
      ny = A[iy] ;
      A[ix] = c*nx - s*ny ;
      A[iy] = s*nx + c*ny ;
    }
}


/* Givens angle for the pair (p,q) of an mxm matrix A   */
double Givens(double *A, int m, int p, int q)
{
  double pp = A[p+m*p] ;
  double qq = A[q+m*q] ;
  double pq = A[p+m*q] ;
  double qp = A[q+m*p] ;

  if (pp>qq)
    return 0.5 * atan2(-pq-qp, pp-qq) ;
  else
    return 0.5 * atan2(pq+qp, qq-pp) ;

}

/* Givens angle for the pair (p,q) of a stack of K M*M matrices */
double GivensStack(double *A, int M, int K, int p, int q)
{
  int k ;
  double diff_on, sum_off, ton, toff ;
  double *cm ; /* A cumulant matrix  */
  double G11 = 0.0 ;
  double G12 = 0.0 ;
  double G22 = 0.0 ;
  
  int M2 = M*M ;
  int pp = p+p*M ;
  int pq = p+q*M ;
  int qp = q+p*M ;
  int qq = q+q*M ;

  for (k=0, cm=A; k<K; k++, cm+=M2) {
    diff_on = cm[pp] - cm[qq] ;
    sum_off = cm[pq] + cm[qp] ;
    
    G11 += diff_on * diff_on ;
    G22 += sum_off * sum_off ;
    G12 += diff_on * sum_off ;
  }
  ton  = G11 - G22 ;
  toff = 2.0 * G12  ;
  
  return -0.5 * atan2 ( toff , ton+sqrt(ton*ton+toff*toff) ); 

  /* there is no final minus sign in the matlab code because the
     convention for c/s in the Givens rotations is the opposite ??? */
}


/* 
   Diagonalization of an mxm matrix A by a rotation R.
*/
int Diago (double *A, double *R, int m, double threshold) 
{
  int encore = 1 ;
  int rots = 0 ;
  int p, q ;
  double theta,c,s ;

  Identity(R, m) ;

  /* Sweeps until no pair gets updated  */
  while (encore>0)  { 
    encore = 0 ;
    for (p=0; p<m; p++)
      for (q=p+1; q<m; q++) {