spectral_simplex.c

最大似然估计算法

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

#define SHFT(a,b,c,d) (a)=(b); (b) = (c); (c) = (d);

double fit_psd(double *f, double *P, int n_data, double fs, double a, double bk, double alpha);

void spectral_simplex(double *f, double *P, int n_data, double Fs, double *spec_index, double *sig_wh, double *sig_pl, int est_index, int verbose) {

	int i, j, k, n;

	int n_vert, ilo, ihi, inhi, n_params, info;
	int nfun_eval, got_answer, index;

	double alpha, delta;
	double mle_try, mle_save, min_mle;
	double fac, fac1, fac2;
	double md1, md2, md3, dist;

	double **p, *mle, *psum, *params, *ptry;
	double *start, *tol;

	index = (est_index >= 1 ? 1 : 0);

	/***************************************/
	/* Get the initial guess for P0 and f0 */
	/***************************************/

	alpha = (*spec_index);

	n_params = 2 + index;
	n_vert   = n_params + 1;

	start  = (double *)  calloc((size_t) n_params, sizeof(double));
	tol    = (double *)  calloc((size_t) 4,        sizeof(double));
	params = (double *)  calloc((size_t) n_params, sizeof(double));
	mle    = (double *)  calloc((size_t) n_vert,   sizeof(double));
	psum   = (double *)  calloc((size_t) n_params, sizeof(double));
	ptry   = (double *)  calloc((size_t) n_params, sizeof(double));

	p      = (double **) calloc((size_t)(n_params*n_vert), sizeof(double *));
	for (j = 0; j < n_vert; j++) p[j] = (double *) calloc((size_t) n_params, sizeof(double));

	start[0] = (*sig_wh);
	start[1] = (*sig_pl);
	if (index) start[2] = (*spec_index);

	delta = 1.5;

	tol[0] = 400.0;
	tol[1] = 0.003;
	tol[2] = 0.00002;
	tol[3] = 0.000001;

	for (k = 0; k < n_vert; k++) {
                for (j = 0; j < n_params; j++) {
                        p[k][j] = start[j];
                        if (k == j+1) p[k][j] = delta * start[j];
                }
        }

	for (k = 0; k < n_vert; k++) {
                for (j = 0; j < n_params; j++) params[j] = p[k][j];
		if (index)  mle[k] = fit_psd(f,P,n_data,Fs,params[0],params[1],params[2]);
		else mle[k] = fit_psd(f,P,n_data,Fs,params[0],params[1],alpha);
	}

	nfun_eval = 0;
	got_answer = 0;
	while ((double) nfun_eval <= tol[0]) {

		if (verbose) {
                	for (k = 0; k < n_vert; k++) {
                        	fprintf(stdout, "%2d %3d %10.4f ", k, nfun_eval,  mle[k]);
                        	for (j = 0; j < n_params; j++) fprintf(stdout, "%12.8f ", p[k][j]);
                        	fprintf(stdout, "\n");
                	}
                	fprintf(stdout, "\n");
		}

		ilo = 0;
		ihi = mle[0] > mle[2] ? (inhi=1,0) : (inhi=0,1);

		for (i = 0; i < n_vert; i++) {
			if (mle[i] <= mle[ilo]) ilo = i;
			if (mle[i] > mle[ihi]) {
				inhi = ihi;
                                ihi = i;
			} else if (mle[i] > mle[inhi] && i != ihi) inhi = i;
		}

		md1 = md2 = md3 = 0.0;

		for (k = 1; k < n_vert; k++) {
			for (j = 0; j < n_params; j++) {
				dist = fabs(p[k][j] - p[0][j]) / p[0][j];
				md1 = dist > md1 ? dist : md1;
				dist = fabs(p[k][j] - p[0][j]);
				md2 = dist > md2 ? dist : md2;
			}
			dist = fabs((mle[k] - mle[0]) / mle[0]);
			md3 = dist > md3 ? dist : md3;
		}

		if ((md1 <= tol[1] || md2 <= tol[3]) && (md3 <= tol[2])) {
			got_answer = 1;
			break;
		}

		nfun_eval += 2;

		/************************************************************/
                /* first extrapolate by a factor -1 through the face of the */
                /* simplex across from the high point                       */
                /************************************************************/

                fac = -1.0;
                fac1 = (1.0 - fac) / (double) n_params;
                fac2 = fac1 - fac;

		for (j = 0 ; j < n_params; j++) ptry[j] = psum[j]*fac1 - p[ihi][j]*fac2;
		if (index)  mle_try = fit_psd(f,P,n_data,Fs,ptry[0],ptry[1],ptry[2]);
		else mle_try = fit_psd(f,P,n_data,Fs,ptry[0],ptry[1],alpha);

		if (mle_try < mle[ihi]) {
                        mle[ihi] = mle_try;
                        for (j = 0; j < n_params; j++) {
                                psum[j] += ptry[j]-p[ihi][j];
                                p[ihi][j] = ptry[j];
                        }
                }

                /************************************************************/

                if (mle_try <= mle[ilo]) {

                        fac = 2.0;
                        fac1 = (1.0 - fac) / (double) n_params;
                        fac2 = fac1 - fac;

                        for (j = 0 ; j < n_params; j++) ptry[j] = psum[j]*fac1 - p[ihi][j]*fac2;
			if (index)  mle_try = fit_psd(f,P,n_data,Fs,ptry[0],ptry[1],ptry[2]);
			else mle_try = fit_psd(f,P,n_data,Fs,ptry[0],ptry[1],alpha);

			if (mle_try < mle[ihi]) {
                                mle[ihi] = mle_try;
                                for (j = 0; j < n_params; j++) {
                                        psum[j] += ptry[j]-p[ihi][j];
                                        p[ihi][j] = ptry[j];
                                }
                        }
                } else if (mle_try >= mle[inhi]) {
                        mle_save = mle[ihi];

                        fac = 0.5;
                        fac1 = (1.0 - fac) / (double) n_params;
                        fac2 = fac1 - fac;

                        for (j = 0 ; j < n_params; j++) ptry[j] = psum[j]*fac1 - p[ihi][j]*fac2;
			if (index)  mle_try = fit_psd(f,P,n_data,Fs,ptry[0],ptry[1],ptry[2]);
			else mle_try = fit_psd(f,P,n_data,Fs,ptry[0],ptry[1],alpha);

                        if (mle_try < mle[ihi]) {
                                mle[ihi] = mle_try;
                                for (j = 0; j < n_params; j++) {
                                        psum[j] += ptry[j]-p[ihi][j];
                                        p[ihi][j] = ptry[j];
                                }
                        }
                        if (mle_try >= mle_save) {
                                for (i = 0; i < n_vert; i++) {
                                        if (i != ilo) {
                                                for (j = 0; j < n_params; j++) {
                                                        p[i][j] = psum[j] = 0.5 * (p[i][j] + p[ilo][j]);
                                                }
                                                for (j = 0; j < n_params; j++) params[j] = p[i][j];
						if (index)  mle[i] = fit_psd(f,P,n_data,Fs,params[0],params[1],params[2]);
						else mle[i] = fit_psd(f,P,n_data,Fs,params[0],params[1],alpha);
                                        }
                                }
                        }
                        nfun_eval += n_params;
                        for (j = 0; j < n_params; j++) {
                                psum[j] = 0.0;
                                for (k = 0; k < n_vert; k++) psum[j] += p[k][j];
                        }
                } else --nfun_eval;
        }

        /**************************/
        /* The end of the simplex */
        /**************************/

	if (got_answer) {
		(*sig_wh) = p[ilo][0];
		(*sig_pl) = p[ilo][1];
		if (index) (*spec_index) = p[ilo][2];
		else (*spec_index) = alpha;
	} else {
		fprintf(stderr, "spectral_simplex : Minimization did not converge!!\n");
		(*sig_wh) = 0.0;
		(*sig_pl) = 0.0;
		(*spec_index) = 0.0;
	}

	free(params);
	free(mle);
	free(psum);
	free(ptry);
	for (j = 0; j < n_vert; j++) free(p[j]);
	free(p);
}  

double fit_psd(double *f, double *P, int n_data, double fs, double a, double bk, double alpha) {

	int j;

	double rms, D, power, residual;

	D = 2.0 * pow(2.0 * M_PI,alpha) * pow(24.0 * 3600.0 * 365.24219,alpha/2.0);

	rms = 0.0;
	for (j = 0; j < n_data; j++) {
		power = 2.0 * a * a / fs + D * bk * bk * pow(f[j],alpha) / pow(fs,alpha/2.0 + 1.0);
		residual = 10.0*log10(power) - 10.0*log10(P[j]);
		rms += (residual*residual);
	}
	rms /= (double)(n_data-1);
	rms = sqrt(rms) * 1000;

	if (a < 0.0 || bk < 0.0) rms = 1e9;

	return(rms);
}


double brent_spectral(double *start_index, double *f, double *P, int n_data, double Fs, double *spec_index, double *sig_wh, double *sig_pl, double rms, int verbose) { 

	int j, jj, k, i, iter;

	int imax = 100;
	int count = 0;
	int got_answer = 0;

	double a, b, d, etemp, fu, fv, fw, fx, p, q, r, tol1, tol2, u, v, w, x, xm;
	double min_pl, wh_mle, MLE, xmin, wh_rms, dummy1, dummy2;
	double m, c, delta, sx, sx2, sy, sxy;
	double wh_sig, pl_sig;


	double e = 0.0;
	double zeps = 1.0e-10;
	double cgold = 0.3819660;
	double tolerance = 0.01;

	double *start;
	double *index, *wh, *pl, *mle;

	if (verbose) fprintf(stdout, " Starting the one-dimensional minimisation : initial index = %5.2f\n", start_index[1]);
	
	a = (start_index[0] < start_index[2] ? start_index[0] : start_index[2]);
	b = (start_index[0] > start_index[2] ? start_index[0] : start_index[2]);
	x = w = v = start_index[1];
	
	start      = (double *) calloc((size_t)(3),       sizeof(double) );
	mle        = (double *) calloc((size_t)(200),     sizeof(double) );
	index      = (double *) calloc((size_t)(200),     sizeof(double) );
	wh         = (double *) calloc((size_t)(200),     sizeof(double) );
	pl         = (double *) calloc((size_t)(200),     sizeof(double) );

	j = 0;

	wh_sig = (*sig_wh);
	pl_sig = (*sig_pl);

	spectral_simplex(f,P,n_data,Fs,&x,&wh_sig,&pl_sig,0,0);
	MLE = fit_psd(f,P,n_data,Fs,wh_sig,pl_sig,x);
	index[j] = x; mle[j] = MLE; wh[j] = wh_sig; pl[j] = pl_sig; j++;
	
	if (verbose == 1) {
		fprintf(stdout, " spectral_index = %9.6f  mle = %13.8f ", x, MLE);
		fprintf(stdout, " wh = %12.6f  pl = %12.6f\n", wh_sig*1000.0, pl_sig*1000.0); 
	}

	fw=fv=fx=MLE;

	for (iter=0; iter < imax; iter++) {
		xm = 0.5*(a+b);
		tol2 = 2.0*(tol1=tolerance*fabs(x)+zeps);
		if (fabs(x-xm) <= (tol2-0.5*(b-a))) {
			got_answer = 1;
			xmin = x;
			break;
		}
		if (fabs(e) > tol1) {
			r = (x-w)*(fx-fv);
			q = (x-v)*(fx-fw);
			p = (x-v)*q-(x-w)*r;
			q = 2.0 * (q-r);
			if (q > 0.0) p = -p;
			q = fabs(q);
			etemp = e;
			e = d;
			if (fabs(p) >= fabs(0.5 * q* etemp) || p <= q*(a-x) || p >= q * (b-x)) {
				d = cgold*(e=(x >= xm ? a-x : b-x));
			} else {
				d = p / q;
				u = x+d;
				if (u-a < tol2 || b-u < tol2) {
					d = (xm-x < 0.0 ? -fabs(tol1) : fabs(tol1) );
				}
			}
		} else {
			d = cgold*(e=(x >= xm ? a-x : b-x));
		}
		u = (fabs(d) >= tol1 ? x+d : x + (d < 0.0 ? -fabs(tol1) : fabs(tol1) ));

		if (verbose) fprintf(stdout, " Next choice of index = %7.4f\n", u);

		/***********************************************************************/
		/* New method 6th Sept 2002                                            */
		/* It looks like cats_empirical is spot on with wh parameter but not   */
		/* so good with pl when the index is wrong. However it looks like the  */
		/* index vs pl in the MLE is quite linear so we can fit a line to      */
		/* obtain the starting parameters                                      */
		/***********************************************************************/

		if (j == 1) {
			m = (pl[0] - rms) / start_index[1];
			c = rms;

			wh_sig = rms;
			pl_sig = m * u + c;
		} else {
			sx = sx2 = sy = sxy = 0.0;
			for (jj = 0; jj < j ; jj++) {
				sx  += index[jj];
				sx2 += (index[jj]*index[jj]);
				sy  += pl[jj];
				sxy += (index[jj]*pl[jj]);
			}
			delta = (double) j * sx2 - sx * sx;
			m = ((double)j * sxy - sx * sy) / delta;
			c = (sx2*sy - sx * sxy) / delta;	

			wh_sig = rms;
			pl_sig = m * u + c;
			
		}

		spectral_simplex(f,P,n_data,Fs,&u,&wh_sig,&pl_sig,0,0);
		MLE = fit_psd(f,P,n_data,Fs,wh_sig,pl_sig,u);

		index[j] = u; mle[j] = MLE; wh[j] = wh_sig; pl[j] = pl_sig; j++;

		if (verbose == 1) {
			fprintf(stdout, " spectral_index = %9.6f  mle = %13.8f  ", u, MLE);
			fprintf(stdout, "wh = %12.6f  pl = %12.6f\n", wh[j-1]*1000, pl[j-1]*1000);   
		}

		fu = MLE;

		if (fu <= fx) {
			if (u >= x) a = x; else b = x;
			SHFT(v,w,x,u)
			SHFT(fv,fw,fx,fu)
		} else {
			if (u < x) a = u; else b = u;
			if (fu <= fw || w == x) {
				v = w;
				w = u;
				fv = fw;
				fw = fu;
			} else if (fu <= fv || v == x || v == w) {	
				v = u;
				fv = fu;
			}
		}
	}

	if (got_answer) {

		if (verbose) fprintf(stdout, " Finished : \n\n");
		for (k = 0; k < j; k++) {
			if (verbose) {
				fprintf(stdout, " Spectral_index = %9.6f  mle = %13.8f  ", index[k], mle[k]);
				fprintf(stdout, "wh = %12.6f  pl = %12.6f\n", wh[k]*1000, pl[k]*1000);   
			}
			if (index[k] == xmin) {
				(*sig_wh)     = wh[k];
				(*sig_pl)     = pl[k];
				(*spec_index) = xmin;
			}
		}
	} else {
		MLE = 0.0;
	}

	free(start);
        free(index);
	free(wh);
	free(pl);
	free(mle);

	return(MLE);
}