// ------------------------------- mexFunction ---------------------------------------// void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { /* Check for proper number of arguments */ if(nrhs!=5) { Usage(); mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nrhs", "Five inputs required."); } if(nlhs!=4) { Usage(); mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nlhs", "Four output required."); } // Input argument and dimensions const mwSize DataR = mxGetM(prhs[0]), DataC = mxGetN(prhs[0]), MeanIR = mxGetM(prhs[1]), MeanIC = mxGetN(prhs[1]); const double *Data = mxGetPr(prhs[0]), *MeanI = mxGetPr(prhs[1]); const double MaxIter = mxGetScalar(prhs[2]), Thresh = mxGetScalar(prhs[3]); const int Verbose = (int) mxGetScalar(prhs[4]); // Set the number of means elements, dimension and number of data points int NM = MeanIC, ND = DataC, Dim = DataR; // Check appropriate sizes and modes if( Dim != MeanIR ) { Usage(); mexErrMsgIdAndTxt("KMeans:Means","Means matrix dimension"); } // Data elements double *Prior = NULL, *Mean = NULL, *Cov = NULL, *Assign; // ** KM parameters double *SqrTemp = new double[Dim]; double *DistTemp = new double[NM]; double alpha = 1, beta = 1, Scale = 0; double Term, TermP = 0; int AssignTemp; int *AssignNum; AssignNum = new int[NM]; mwSize CSize[] = {Dim,Dim,NM}; int CNumSize = 3; int i,k,j; int inc = 1; int index = 0; Assign = new double[ND]; Prior = new double[NM]; Mean = new double[Dim*NM]; Cov = new double[Dim*Dim*NM]; memcpy(Mean, MeanI, Dim*NM*sizeof(double)); for(int E = 0; E < MaxIter; E++){ // ** Assign features to each center #pragma omp parallel for shared(Data, Mean, Dim, inc, Assign) private(i,j, SqrTemp, DistTemp, AssignTemp) for(i = 0; i < ND; i++){ for(j = 0; j < NM; j++){ // Calculate elements of Data-Mean vdSub( Dim, &Data[i*Dim], &Mean[j*Dim], SqrTemp ); // Square elements vdSqr( Dim, SqrTemp, SqrTemp ); // Sum distances DistTemp[j] = dasum( &Dim, SqrTemp, &inc); } // Find minimum distance index AssignTemp = idamin(&NM, DistTemp, &inc); Assign[i] = double(AssignTemp-1); } // ** Calculate new center values // Zero out centers memset(Mean, 0, Dim*NM*sizeof(double)); memset(AssignNum, 0, NM*sizeof(int)); for(i = 0; i < ND; i++){ index = Assign[i]; daxpy(&Dim, &alpha, &Data[i*Dim], &inc, &Mean[index*Dim], &inc); AssignNum[index] = AssignNum[index] + 1; } for(i = 0; i < NM; i++){ // If no point was assigned to an average, assign a different point if(AssignNum[i] == 0){ memcpy(&Mean[i*Dim], &Data[(rand()%ND)*Dim], Dim*sizeof(double)); Scale = 1; dscal(&Dim, &Scale, &Mean[i*Dim], &inc); } else{ Scale = 1 / double(AssignNum[i]); dscal(&Dim, &Scale, &Mean[i*Dim], &inc); } } // Termination conditions k = Dim*NM; Term = dasum(&k, Mean, &inc); if(Verbose == 1){ printf("Iteration %d - Shift %3.9f\n",E, fabs(Term-TermP)); mexEvalString("pause(.000001);"); } if(fabs(Term-TermP) <= Thresh) break; TermP = Term; } // ** Create and assign output variables double *Out; plhs[0] = mxCreateDoubleMatrix(1, ND ,mxREAL ); plhs[1] = mxCreateDoubleMatrix(Dim, NM, mxREAL ); plhs[2] = mxCreateDoubleMatrix(1, NM, mxREAL ); plhs[3] = mxCreateNumericArray( CNumSize, CSize, mxDOUBLE_CLASS, mxREAL); // Priors k = 0; for(i = 0; i < NM; i++ ) k = k + AssignNum[i]; for(i = 0; i < NM; i++){ Prior[i] = double(AssignNum[i])/k; } // Covariances memset(Cov, 0, Dim*Dim*NM*sizeof(double)); char trans = 'N'; k = 1; for(i = 0; i < ND; i++){ index = Assign[i]; Scale = 1/(Prior[index]*ND); // Calculate elements of Data-Mean vdSub( Dim, &Data[i*Dim], &Mean[index*Dim], SqrTemp ); // covariance dgemm(&trans, &trans, &Dim, &Dim, &k, &Scale, SqrTemp, &Dim, SqrTemp, &k, &beta, &Cov[index*Dim*Dim], &Dim); } Out = mxGetPr(plhs[0]); memcpy(Out, Assign, ND*sizeof(double)); Out = mxGetPr(plhs[1]); memcpy(Out, Mean, Dim*NM*sizeof(double)); Out = mxGetPr(plhs[2]); memcpy(Out, Prior, NM*sizeof(double)); Out = mxGetPr(plhs[3]); memcpy(Out, Cov, Dim*Dim*NM*sizeof(double)); //** Delete dynamic variables delete [] Prior; delete [] Mean; delete [] Cov; delete [] Assign; delete [] AssignNum; }
double KNITRO_EXPORT KTR_dasum (const int n, const double * const x, const int incx) { return( dasum (n, x, incx) ); }
/* Function: mdlOutputs ======================================================= * do the main optimization routine here * no discrete states are considered */ static void mdlOutputs(SimStruct *S, int_T tid) { int_T i, j, Result, qinfeas=0; real_T *z = ssGetOutputPortRealSignal(S,0); real_T *w = ssGetOutputPortRealSignal(S,1); real_T *I = ssGetOutputPortRealSignal(S,2); real_T *exitflag = ssGetOutputPortRealSignal(S,3); real_T *pivots = ssGetOutputPortRealSignal(S,4); real_T *time = ssGetOutputPortRealSignal(S,5); InputRealPtrsType M = ssGetInputPortRealSignalPtrs(S,0); InputRealPtrsType q = ssGetInputPortRealSignalPtrs(S,1); ptrdiff_t pivs, info, m, n, inc=1; char_T T='N'; double total_time, alpha=1.0, tmp=-1.0, *x, *Mn, *qn, *r, *c, s=0.0, sn=0.0; PT_Matrix pA; /* Problem data A = [M -1 q] */ PT_Basis pB; /* The basis */ T_Options options; /* options structure defined in lcp_matrix.h */ /* for RTW we do not need these variables */ #ifdef MATLAB_MEX_FILE const char *fname; mxArray *fval; int_T nfields; clock_t t1,t2; #endif UNUSED_ARG(tid); /* not used in single tasking mode */ /* default options */ options.zerotol = 1e-10; /* zero tolerance */ options.lextol = 1e-10; /* lexicographic tolerance - a small treshold to determine if values are equal */ options.maxpiv = INT_MAX; /* maximum number of pivots */ /* if LUMOD routine is chosen, this options refactorizes the basis after n steps using DGETRF routine from lapack to avoid numerical problems */ options.nstepf = 50; options.clock = 0; /* 0 or 1 - to print computational time */ options.verbose = 0; /* 0 or 1 - verbose output */ /* which routine in Basis_solve should solve a set of linear equations: 0 - corresponds to LUmod package that performs factorization in the form L*A = U. Depending on the change in A factors L, U are updated. This is the fastest method. 1 - corresponds to DGESV simple driver which solves the system AX = B by factorizing A and overwriting B with the solution X 2 - corresponds to DGELS simple driver which solves overdetermined or underdetermined real linear systems min ||b - Ax||_2 involving an M-by-N matrix A, or its transpose, using a QR or LQ factorization of A. */ options.routine = 0; options.timelimit = 3600; /* time limit in seconds to interrupt iterations */ options.normalize = 1; /* 0 or 1 - perform scaling of input matrices M, q */ options.normalizethres = 1e6; /* If the normalize option is on, then the matrix scaling is performed only if 1 norm of matrix M (maximum absolute column sum) is above this threshold. This enforce additional control over normalization since it seems to be more aggressive also for well-conditioned problems. */ #ifdef MATLAB_MEX_FILE /* overwriting default options by the user */ if (ssGetSFcnParamsCount(S)==3) { nfields = mxGetNumberOfFields(ssGetSFcnParam(S,2)); for(i=0; i<nfields; i++){ fname = mxGetFieldNameByNumber(ssGetSFcnParam(S,2), i); fval = mxGetField(ssGetSFcnParam(S,2), 0, fname); if ( strcmp(fname,"zerotol")==0 ) options.zerotol = mxGetScalar(fval); if ( strcmp(fname,"lextol")==0 ) options.lextol = mxGetScalar(fval); if ( strcmp(fname,"maxpiv")==0 ) { if (mxGetScalar(fval)>=(double)INT_MAX) options.maxpiv = INT_MAX; else options.maxpiv = (int_T)mxGetScalar(fval); } if ( strcmp(fname,"nstepf")==0 ) options.nstepf = (int_T)mxGetScalar(fval); if ( strcmp(fname,"timelimit")==0 ) options.timelimit = mxGetScalar(fval); if ( strcmp(fname,"clock")==0 ) options.clock = (int_T)mxGetScalar(fval); if ( strcmp(fname,"verbose")==0 ) options.verbose = (int_T)mxGetScalar(fval); if ( strcmp(fname,"routine")==0 ) options.routine = (int_T)mxGetScalar(fval); if ( strcmp(fname,"normalize")==0 ) options.normalize = (int_T)mxGetScalar(fval); if ( strcmp(fname, "normalizethres")==0 ) options.normalizethres = mxGetScalar(fval); } } #endif /* Normalize M, q to avoid numerical problems if possible Mn = diag(r)*M*diag(c) , qn = diag(r)*q */ /* initialize Mn, qn */ Mn = (double *)ssGetPWork(S)[0]; qn = (double *)ssGetPWork(S)[1]; /* initialize vectors r, c */ r = (double *)ssGetPWork(S)[2]; c = (double *)ssGetPWork(S)[3]; /* initialize auxiliary vector x */ x = (double *)ssGetPWork(S)[4]; /* initialize to ones */ for (i=0; i<NSTATES(S); i++) { r[i] = 1.0; c[i] = 1.0; } m = NSTATES(S); n = m*m; /* write data to Mn = M */ memcpy(Mn, *M, n*sizeof(double)); /* write data to qn = q */ memcpy(qn, *q, m*sizeof(double)); /* check out the 1-norm of matrix M (maximum column sum) */ for (i=0; i<m; i++) { sn = dasum(&m, &Mn[i*m], &inc); if (sn>s) { s = sn; } } /* scale matrix M, q and write scaling factors to r (rows) and c (columns) */ if (options.normalize && s>options.normalizethres) { NormalizeMatrix (m, m, Mn, qn, r, c, options); } /* Setup the problem */ pA = ssGetPWork(S)[5]; /* A(:,1:m) = M */ memcpy(pMAT(pA), Mn, n*sizeof(double)); /* A(:,1:m) = -A(:,1:m) */ dscal(&n, &tmp, pMAT(pA), &inc); /* A(:,m+1) = -1 */ for(i=0;i<m;i++) C_SEL(pA,i,m) = -1.0; /* A(:,m+2) = q */ memcpy(&(C_SEL(pA,0,m+1)),qn,m*sizeof(double)); /* initialize basis */ pB = ssGetPWork(S)[6]; /* check if the problem is not feasible at the beginning */ for (i=0; i<m; i++) { if (qn[i]<-options.zerotol) { qinfeas = 1; break; } } /* Solve the LCP */ if (qinfeas) { #ifdef MATLAB_MEX_FILE t1 = clock(); #endif /* main LCP rouinte */ Result = lcp(pB, pA, &pivs, options); #ifdef MATLAB_MEX_FILE t2 = clock(); total_time = ((double)(t2-t1))/CLOCKS_PER_SEC; #else total_time = -1; #endif } else { pivs = 0; total_time = 0; Result = LCP_FEASIBLE; } #ifdef MATLAB_MEX_FILE if (options.clock) { printf("Time needed to perform pivoting:\n time= %i (%lf seconds)\n", t2-t1,total_time); printf("Pivots: %ld\n", pivs); printf("CLOCKS_PER_SEC = %i\n",CLOCKS_PER_SEC); } #endif /* initialize values to 0 */ for(i=0;i<NSTATES(S);i++) { w[i] = 0.0; z[i] = 0.0; I[i] = 0.0; } /* for a feasible basis, compute the solution */ if ( Result == LCP_FEASIBLE || Result == LCP_PRETERMINATED ) { #ifdef MATLAB_MEX_FILE t1 = clock(); #endif info = Basis_Solve(pB, &(C_SEL(pA,0,m+1)), x, options); for (j=0,i=0;i<Index_Length(pB->pW);i++,j++) { w[Index_Get(pB->pW,i)] = x[j]; /* add 1 due to matlab 1-indexing */ I[j] = Index_Get(pB->pW,i)+1; } for(i=0;i<Index_Length(pB->pZ);i++,j++) { /* take only positive values */ if (x[j] > options.zerotol ) { z[Index_Get(pB->pZ,i)] = x[j]; } /* add 1 due to matlab 1-indexing */ I[j] = Index_Get(pB->pZ, i)+m+1; } #ifdef MATLAB_MEX_FILE t2 = clock(); total_time+=(double)(t2-t1)/(double)CLOCKS_PER_SEC; if (options.clock) { printf("Time in total needed to solve LCP: %lf seconds\n", total_time + (double)(t2-t1)/(double)CLOCKS_PER_SEC); } #endif if (options.normalize) { /* do the backward normalization */ /* z = diag(c)*zn */ for (i=0; i<m; i++) { z[i] = c[i]*z[i]; } /* since the normalization does not compute w properly, we recalculate it from * recovered z */ /* write data to Mn = M */ memcpy(Mn, *M, n*sizeof(double)); /* write data to qn = q */ memcpy(qn, *q, m*sizeof(double)); /* copy w <- q; */ dcopy(&m, qn, &inc, w, &inc); /* compute w = M*z + q */ dgemv(&T, &m, &m, &alpha, Mn, &m, z, &inc, &alpha, w, &inc); /* if w is less than eps, consider it as zero */ for (i=0; i<m; i++) { if (w[i]<options.zerotol) { w[i] = 0.0; } } } } /* outputs */ *exitflag = (real_T )Result; *pivots =(real_T)pivs; *time = (real_T)total_time; /* reset dimensions and values in basis for a recursive call */ Reinitialize_Basis(m, pB); }
/* normalize input matrix M */ void NormalizeMatrix (ptrdiff_t m, ptrdiff_t n, double *A, double *b, double *r, double *c, T_Options options) { /* scales matrix A by finding D1= diag(r) and D2=diag(c) in An = D1*A*D2 such that infinity norm of each row and column approaches 1 find D1, D2 s.t. An = D1*A*D2 Scaling matrix is used in solving A*x=b for badly scaled matrix A as follows: A*x = b D1*A*x = D1*b / multiply from left by D1 D1*A*D2*inv(D2)*x = D1*b / insert D2*inv(D2) D1*A*D2*y = D1*b / substitute y = inv(D2)*x An*y = bn / substitue An = D1*A*D2, v = D1*b First solve An*y = bn, then obtain x = D2*y Details of the method are in file drRAL2001034.ps.gz at http://www.numerical.rl.ac.uk/reports/reports.html. arguments: n - (input), ptrdiff_t (INTEGER) Number of rows for matrix A and dimension of vector r m - (input), ptrdiff_t (INTEGER) Number of cols for matrix A and dimension of vector c A - (input/output) (pointer to DOUBLE) Input matrix is changed at the output with the normalized matrix. It is assumed that matrix A does not contain row or column with all elements zeros. If this not the case, no scaling is performed. b - (inptu/output) (pointer to DOUBLE) Input vector is normalized at the output r - (output) (pointer to DOUBLE) Vector with scaling factors for each row, D1 = diag(r) c - (output) (pointer to DOUBLE) Vector with scaling factors for each column, D2 = diag(c) */ ptrdiff_t i, j, kmax, inc=1,k=0; int flag=1; double row_norm = 1.0, col_norm = 1.0, nrm; double *rn, *rs, *cn, *cs; /* prepare vectors */ rn = (double *)mxCalloc(n,sizeof(double)); rs = (double *)mxCalloc(n,sizeof(double)); cn = (double *)mxCalloc(m,sizeof(double)); cs = (double *)mxCalloc(m,sizeof(double)); /* check if A contains row or cols with all zeros */ /* initialize output vectors */ for (i=0; i<m; i++) { nrm = dasum(&n, &A[i], &m); if (nrm<options.zerotol) { flag = 0; } r[i] = 1.0; } for (i=0; i<n; i++) { nrm = dasum(&m, &A[i*n], &inc); if (nrm<options.zerotol) { flag = 0; } c[i] = 1.0; } if (flag) { while ((row_norm>options.zerotol) && (col_norm>options.zerotol)) { /* loop thru rows */ for (i=0; i<m; i++) { /* compute infinity norm of each row */ /* find index of a maximum value in this row (1-indexed) */ kmax = idamax(&n, &A[i], &m); /* evaluate the norm */ nrm = fabs(A[i+(kmax-1)*m]); rs[i] = 1.0/sqrt(nrm); /* printf("rows: kmax=%ld, nrm=%f\n",kmax,nrm); */ /* to compute row_norm*/ rn[i] = 1 - nrm; } /* loop thru cols */ for (i=0; i<n; i++) { /* compute infinity norm of each column */ /* find maximum value in this column */ kmax = idamax(&m, &A[i*n], &inc); /* evaluate the norm */ nrm = fabs(A[i*m+kmax-1]); cs[i] = 1.0/sqrt(nrm); /* printf("cols: kmax=%ld, nrm=%f\n",kmax,nrm); */ /* set the col_norm */ cn[i] = 1 - nrm; } /* scale matrix A and vector b */ for (i=0; i<m; i++) { for (j=0; j<n; j++) { A[i+j*m] = rs[i]*A[i+j*m]*cs[j]; } b[i] = rs[i]*b[i]; /* update output vector r */ r[i] = r[i]*rs[i]; } /* update output vectors c*/ for (i=0; i<n; i++) { c[i] = c[i]*cs[i]; } /* compute the norm and column norms */ kmax = idamax(&m, rn, &inc); row_norm = fabs(rn[kmax-1]); kmax = idamax(&n, cn, &inc); col_norm = fabs(cn[kmax-1]); if (++k>=1000 ) { /* limit the number of iterations */ break; } } } /* freeing vectors */ mxFree(rn); mxFree(rs); mxFree(cn); mxFree(cs); }
int lmT(double * X, int n, int p, double * y, double nu, int maxIter, double tol, char method, double * logLikelihood, double * coef, double * tau) { // Initialize workspace variables int iter, i; double logLikelihood_tm1, delta; /* * Allocate workspace arrays */ // Allocate workspace matrices double * sqwX, * XTX; sqwX = malloc(n * p * sizeof(double)); if (sqwX==NULL) { fprintf(stderr, "Error -- out of memory\n"); exit(1); } XTX = malloc(p * p * sizeof(double)); if (XTX==NULL) { fprintf(stderr, "Error -- out of memory\n"); exit(1); } // Allocate workspace vectors double * w, * sqw, * sqwy, * resid; w = calloc(n, sizeof(double)); if (w==NULL) { fprintf(stderr, "Error -- out of memory\n"); exit(1); } sqw = calloc(n, sizeof(double)); if (sqw==NULL) { fprintf(stderr, "Error -- out of memory\n"); exit(1); } sqwy = calloc(n, sizeof(double)); if (sqwy==NULL) { fprintf(stderr, "Error -- out of memory\n"); exit(1); } resid = calloc(n, sizeof(double)); if (resid==NULL) { fprintf(stderr, "Error -- out of memory\n"); exit(1); } /* * Initialize quantities before EM iterations */ // Initialize weights for observations for (i=0; i<n; i++) { w[i] = 1; } /* * First iteration */ // Run regression to obtain initial coefficients wls(X, n, p, y, w, method, XTX, sqw, sqwX, sqwy, coef); // Calculate residuals calcResid(X, n, p, y, coef, resid); // Calculate tau (*tau) = 0; for (i=0; i<n; i++) { (*tau) += resid[i] * resid[i] * w[i]; } (*tau) /= dasum(n, w, 1); // Calculate initial log-posterior (*logLikelihood) = dt_log(resid, n, nu, 0, sqrt(*tau)); logLikelihood_tm1 = (*logLikelihood); /* * EM loop */ for (iter=0; iter<maxIter; iter++) { // E step: Update u | beta, tau for (i=0; i<n; i++) { w[i] = (nu + 1) / (nu + resid[i]*resid[i]/(*tau)); } // M step: Update beta, tau | u // Run regression to obtain coefficients wls(X, n, p, y, w, method, XTX, sqw, sqwX, sqwy, coef); // Calculate residuals calcResid(X, n, p, y, coef, resid); // Calculate tau (*tau) = 0; for (i=0; i<n; i++) { (*tau) += resid[i] * resid[i] * w[i]; } (*tau) /= dasum(n, w, 1); // Calculate log-posterior (*logLikelihood) = dt_log(resid, n, nu, 0, sqrt(*tau)); // Check convergence delta = ((*logLikelihood) - logLikelihood_tm1) / fabs((*logLikelihood) + logLikelihood_tm1) * 2; if (delta < tol) { logLikelihood_tm1 = (*logLikelihood); break; } logLikelihood_tm1 = (*logLikelihood); } /* * Free allocated memory */ // Free workspace matrices free(sqwX); sqwX = NULL; free(XTX); XTX = NULL; // Free workspace vectors free(w); w=NULL; free(sqw); sqw=NULL; free(sqwy); sqw=NULL; free(resid); resid=NULL; return iter; }