factork.c

matlab中uwb波形优化算法经常会使用的工具包:SeDuMi_1_1R3.

  return 0; /* success */
}

/* ************************************************************
   PROCEDURE prpicholpivot - U'*U factorization for nxn matrix,
     with column pivotting. Yields U with 1 >= \|U(i,i+1:n)\| forall i.
   INPUT
     n - order of matrix to be factored
     x,xpi - Full nxn. Matrix to be factored.
   OUTPUT
     u,upi - Full nxn, Cholesky factor: X=U'*U with U(:,perm)
       upper triangular.
     perm - Column permutation for maximal stability.
   WORK
     d - length n vector; the positive diagonal diag(U(:,perm)).^2,
         has decreasing order.
   RETURNS:
     0 = "success", 1 = "X is NOT positive definite"
   ************************************************************ */
int prpicholpivot(double *u, double *upi, int *perm, const double *x,
                  const double *xpi, const int n, double *d)
{
  int i,j,k,imax, icol;
  double *uk, *rowuk, *ukpi, *rowukpi;
  double dk, uki;
  const double *xk;
/* ------------------------------------------------------------
   Initialize: d = diag(x), perm = 0:n-1.
   ------------------------------------------------------------ */
  for(xk = x, k = 0; k < n; xk += n, k++)
    d[k] = xk[k];
  for(j = 0; j < n; j++)
    perm[j] = j;
/* ------------------------------------------------------------
   Pivot in step k=0:n-1 on imax:
   ------------------------------------------------------------ */
  for(k = 0; k < n; k++){
/* ------------------------------------------------------------
   Let [imax,dk] = max(d(k:m))
   ------------------------------------------------------------ */
    dk = d[k]; imax = k;
    for(i = k + 1; i < n; i++)
      if(d[i] > dk){
        imax = i;
        dk = d[i];
      }
/* ------------------------------------------------------------
   k-th pivot is j=perm[imax].
   ------------------------------------------------------------ */
    d[imax] = d[k];
    j = perm[imax];                     /* original node number */
    uk = u + j * n;
    rowuk = u + k;
    ukpi = upi + j * n;
    rowukpi = upi + k;
    perm[imax] = perm[k];
    perm[k] = j;
/* ------------------------------------------------------------
    Let uk[k] = (dk := sqrt(dk))
    ------------------------------------------------------------ */
    if(dk <= 0.0)
      return 1;      /* Matrix is not positive definite */
    dk = sqrt(dk);
    uk[k] = dk;
/* ------------------------------------------------------------
   Let  u(k,:) = (x(k,:)-uk'*u(0:k-1,:)) / dk,
   then d(k+1:n) -= u(k,:).^2.
   ------------------------------------------------------------ */
    for(i = k + 1; i < n; i++){
      icol = perm[i] * n;
      uki = x[icol + j];                                 /* real part */
      uki -= realdot(uk, u+icol, k) + realdot(ukpi, upi+icol,k);
      rowuk[icol] = (uki /= dk);
      d[i] -= SQR(uki);                               /* d(:) -= u(k,:).^2 */
      uki = xpi[icol + j];                               /* imaginary part */
      uki += realdot(ukpi, u+icol, k) - realdot(uk, upi+icol,k);
      rowukpi[icol] = (uki /= dk);
      d[i] -= SQR(uki);                               /* d(:) -= u(k,:).^2 */
    }
  }
  return 0; /* success */
}

/* ============================================================
   MAIN: MEXFUNCTION
   ============================================================ */
/* ************************************************************
   PROCEDURE mexFunction - Entry for Matlab
   [qdetx,ux,ispos,perm] = factorK(x,K);
   ************************************************************ */
void mexFunction(const int nlhs, mxArray *plhs[],
  const int nrhs, const mxArray *prhs[])
{
  mxArray *myplhs[NPAROUT];
  coneK cK;
  int i,k,nk,nksqr, sdplen,sdpdim,lenfull, fwsiz, ispos;
  const double *x;
  double *ux, *fwork, *permPr, *qdetx, *up, *uppi;
  int *iwork, *perm;
  double uxk;
  char use_pivot;
/* ------------------------------------------------------------
   Check for proper number of arguments
   ------------------------------------------------------------ */
  mxAssert(nrhs >= NPARIN, "factorK requires more input arguments");
  mxAssert(nlhs <= NPAROUT, "factorK produces less output arguments");
  use_pivot = (nlhs == NPAROUT);
/* ------------------------------------------------------------
   Disassemble cone K structure
   ------------------------------------------------------------ */
  conepars(K_IN, &cK);
/* ------------------------------------------------------------
   Compute statistics: sdpdim = rdim+hdim, sdplen = sum(K.s).
   ------------------------------------------------------------ */
  lenfull = cK.lpN +  cK.qDim + cK.rDim + cK.hDim;
  sdpdim = cK.rDim + cK.hDim;
  sdplen = cK.rLen + cK.hLen;
/* ------------------------------------------------------------
   Get input vector x, skip LP part
   ------------------------------------------------------------ */
  mxAssert(mxGetM(X_IN) * mxGetN(X_IN) == lenfull, "x size mismatch.");
  x = mxGetPr(X_IN) + cK.lpN;
/* ------------------------------------------------------------
   Allocate output qdetx(lorN), UX(sdpdim), perm(sdplen), ispos(1).
   ------------------------------------------------------------ */
  QDETX_OUT = mxCreateDoubleMatrix(cK.lorN, 1, mxREAL);
  qdetx = mxGetPr(QDETX_OUT);
  UX_OUT = mxCreateDoubleMatrix(sdpdim, 1, mxREAL);
  ux = mxGetPr(UX_OUT);
  ISPOS_OUT = mxCreateDoubleMatrix(1,1,mxREAL);
  PERM_OUT =  mxCreateDoubleMatrix(sdplen, 1, mxREAL);
  permPr = mxGetPr(PERM_OUT);
/* ------------------------------------------------------------
   Allocate working arrays iwork(sdplen),
   fwork(MAX(rmaxn^2,2*hmaxn^2) + MAX(rmaxn,hmaxn))
   ------------------------------------------------------------ */
  iwork = (int *) mxCalloc(sdplen, sizeof(int));
  perm = iwork;
  fwsiz = MAX(cK.rMaxn,cK.hMaxn);
  fwork = (double *) mxCalloc(fwsiz + MAX(SQR(cK.rMaxn),2*SQR(cK.hMaxn)),
                              sizeof(double));
  up = fwork + fwsiz;
  uppi = up + SQR(cK.hMaxn);
/* ------------------------------------------------------------
   LORENTZ:  qdetx = sqrt(qdet(x))
   ------------------------------------------------------------ */
  ispos = 1;
  for(k = 0; k < cK.lorN; k++){
    nk = cK.lorNL[k];
    if( (uxk = qdet(x,nk)) < 0.0){
      ispos = 0;
      break;
    }
    else
      qdetx[k] = sqrt(uxk);
    x += nk;
  }
/* ------------------------------------------------------------
   PSD: Cholesky factorization. If use_pivot, then do pivoting.
   ------------------------------------------------------------ */
  if(use_pivot){
    if(ispos)
      for(k = 0; k < cK.rsdpN; k++){                /* real symmetric */
        nk = cK.sdpNL[k];
        if(cholpivot(up,perm, x,nk, fwork)){
          ispos = 0;
          break;
        }
        uperm(ux, up, perm, nk);
        triu2sym(ux,nk);
        nksqr = SQR(nk);
        x += nksqr; ux += nksqr;
        perm += nk;
      }
/* ------------------------------------------------------------
   Complex Hermitian PSD pivoted Cholesky factorization
   ------------------------------------------------------------ */
    if(ispos)
      for(; k < cK.sdpN; k++){                    /* complex Hermitian */
        nk = cK.sdpNL[k];
        nksqr = SQR(nk);
        if(prpicholpivot(up,uppi,perm, x,x+nksqr,nk, fwork)){
          ispos = 0;
          break;
        }
        uperm(ux, up, perm, nk);                  /* real part */
        uperm(ux+nksqr, uppi, perm, nk);          /* imaginary part */
        triu2herm(ux,ux+nksqr,nk);
        nksqr += nksqr;                           /* 2*n^2 for real+imag */
        x += nksqr; ux += nksqr;
        perm += nk;
      }
/* ------------------------------------------------------------
   Convert "perm" to Fortran-index in doubles.
   ------------------------------------------------------------ */
    for(i = 0; i < sdplen; i++)
      permPr[i] = 1.0 + iwork[i];
  }
/* ------------------------------------------------------------
   PSD, !use_pivot: Cholesky without pivoting.
   First let ux = x, then ux=chol(ux).
   ------------------------------------------------------------ */
  else{           /* Cholesky real sym PSD without pivoting */
    if(ispos){
      memcpy(ux, x, sdpdim * sizeof(double));       /* copy real + complex */
      for(k = 0; k < cK.rsdpN; k++){                /* real symmetric */
        nk = cK.sdpNL[k];
        if(cholnopiv(ux,nk)){
          ispos = 0;
          break;
        }
        triu2sym(ux,nk);
        ux += SQR(nk);
      }
    }
/* ------------------------------------------------------------
   Complex Hermitian PSD Cholesky factorization, no pivoting.
   ------------------------------------------------------------ */
    if(ispos)
      for(; k < cK.sdpN; k++){                    /* complex Hermitian */
        nk = cK.sdpNL[k];
        nksqr = SQR(nk);
        if(prpicholnopiv(ux,ux+nksqr,nk)){
          ispos = 0;
         break;
        }
        triu2herm(ux,ux+nksqr,nk);
        ux += 2 * nksqr;
      }
  } /* !use_pivot */
/* ------------------------------------------------------------
   Return parameter ispos
   ------------------------------------------------------------ */
  *mxGetPr(ISPOS_OUT) = ispos;
/* ------------------------------------------------------------
   Release working arrays
   ------------------------------------------------------------ */
  mxFree(iwork);
  mxFree(fwork);
/* ------------------------------------------------------------
   Copy requested output parameters (at least 1), release others.
   ------------------------------------------------------------ */
  i = MAX(nlhs, 1);
  memcpy(plhs,myplhs, i * sizeof(mxArray *));
  for(; i < NPAROUT; i++)
    mxDestroyArray(myplhs[i]);
}