著名的JADE算法

/* ================================================================== */ /* Implements the Jade and the Shibbs algorithms Copyright: JF Cardoso. [email protected] 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++) { theta = Givens(A,m,p,q) ; if ( fabs(theta) > threshold ) { c = cos(theta); s = sin(theta); LeftRotSimple (A, m, m, p, q, c, s) ; RightRotSimple(A, m, m, p, q, c, s) ; LeftRotSimple (R, m, m, p, q, c, s) ; encore = 1 ; rots++ ; } } } return rots ; } /* Joint diagonalization of a stack K of symmetric M*M matrices by Jacobi */ /* returns the number of plane rotations executed */ int JointDiago (double *A, double *R, int M, int K, double threshold) { int rots = 0 ; int more = 1 ; int