예제 #1
0
파일: setup_aster.c 프로젝트: bitursa/maos
/**
  Estimate wavefront error propagated from measurement error. pgm is the reconstructor. ineam is the
  error inverse.
  trace(Mcc*(pgm*neam*pgm'))
*/
static dmat *calc_recon_error(const dmat *pgm,   /**<[in] the reconstructor*/
			      const dmat *neam,/**<[in] the inverse of error covariance matrix*/
			      const dmat *mcc   /**<[in] NGS mode covariance matrix.*/
			      ){
    dmat *psp=NULL;
    dmat *tmp=NULL;
    dcp(&tmp, pgm);
    dmuldiag(tmp, neam);
    dmm(&psp, 0, tmp, pgm, "nt", 1);
    PDMAT(psp,ppsp); 
    PDMAT(mcc,pmcc);
    /*It is right for both ix, iy to stop at ib.*/
    double all[mcc->nx];
    for(int ib=0; ib<mcc->ny; ib++){
	all[ib]=0;
	for(int iy=0; iy<=ib; iy++){
	    for(int ix=0; ix<=ib; ix++){
		all[ib]+=ppsp[iy][ix]*pmcc[iy][ix];
	    }
	}
    }
    dfree(psp);
    dmat *res=dnew(3,1);
    res->p[0]=all[5];//total error
    res->p[1]=all[1];//total TT error
    if(mcc->nx>5){
	res->p[2]=all[5]-all[4];//focus alone
	if(res->p[2]<0){
	    res->p[2]=0;//due to coupling, this may be negative.
	}
    }
    return res;
}
예제 #2
0
파일: save.c 프로젝트: bitursa/maos
void save_gradol(SIM_T *simu) {
    const PARMS_T *parms=simu->parms;
    const POWFS_T *powfs=simu->powfs;
    for(int iwfs=0; iwfs<parms->nwfsr; iwfs++) {
        int ipowfs=parms->wfsr[iwfs].powfs;
        const int nsa=powfs[ipowfs].saloc->nloc;
        if(!parms->powfs[ipowfs].psol || !simu->gradlastol->p[iwfs]) continue;
        if(parms->plot.run) {
            drawopd("Gpolx",powfs[ipowfs].saloc, simu->gradlastol->p[iwfs]->p,NULL,
                    "WFS Pseudo Openloop Gradients (x)","x (m)", "y (m)", "x %d",  iwfs);
            drawopd("Gpoly",powfs[ipowfs].saloc, simu->gradlastol->p[iwfs]->p+nsa, NULL,
                    "WFS Pseudo Openloop Gradients (y)","x (m)", "y (m)", "y %d",  iwfs);
        }
        if(simu->save->gradol[iwfs] && (simu->reconisim+1) % parms->powfs[ipowfs].dtrat == 0) {
            cellarr_dmat(simu->save->gradol[iwfs], simu->reconisim/parms->powfs[ipowfs].dtrat, simu->gradlastol->p[iwfs]);
        }
    }
    if(parms->save.ngcov>0) {
        /*Outputing psol gradient covariance. */
        for(int igcov=0; igcov<parms->save.ngcov; igcov++) {
            int iwfs1=parms->save.gcov->p[igcov*2];
            int iwfs2=parms->save.gcov->p[igcov*2+1];
            info("Computing covariance between wfs %d and %d\n",iwfs1,iwfs2);
            dmm(&simu->gcov->p[igcov], 1, simu->gradlastol->p[iwfs1], simu->gradlastol->p[iwfs2],"nt",1);
        }
    }
}
예제 #3
0
static void test_part(){/**Compute the covariance of 4 points with various separation.*/
    rand_t rstat;
    int seed=4;
    double r0=0.2;
    double dx=1./64.;
    long N=1+2;
    long ofx=0;
    long ofy=0;
    long nx=N;
    long ny=N;
    long nframe=1000000;
    seed_rand(&rstat, seed);
    map_t *atm=mapnew(nx, ny, dx,dx, NULL);
    dmat *vec=dnew(4,1);
    dmat *cov=NULL;
    dmat* pp=(dmat*)atm;
    for(long i=0; i<nframe; i++){
	info("%ld of %ld\n", i, nframe);
	for(long j=0; j<nx*ny; j++){
	    atm->p[j]=randn(&rstat);
	}
	fractal_do((dmat*)atm, dx, r0,L0,ninit);
	vec->p[0]=IND(pp,ofx+0,ofy+0);
	vec->p[1]=IND(pp,ofx+1,ofy+0);
	vec->p[2]=IND(pp,ofx+0,ofy+1);
	vec->p[3]=IND(pp,ofx+1,ofy+1);
	dmm(&cov, 1, vec, vec, "nt", 1);
    }
    dscale(cov, 1./nframe);
    writebin(cov,"cov.bin");
}
예제 #4
0
    void DocumentSourceSort::populate() {
        /* make sure we've got a sort key */
        verify(vSortKey.size());

        /* track and warn about how much physical memory has been used */
        DocMemMonitor dmm(this);

        /* pull everything from the underlying source */
        for(bool hasNext = !pSource->eof(); hasNext;
            hasNext = pSource->advance()) {
            intrusive_ptr<Document> pDocument(pSource->getCurrent());
            documents.push_back(pDocument);

            dmm.addToTotal(pDocument->getApproximateSize());
        }

        /* sort the list */
        Comparator comparator(this);
        sort(documents.begin(), documents.end(), comparator);

        /* start the sort iterator */
        docIterator = documents.begin();

        if (docIterator != documents.end())
            pCurrent = *docIterator;
        populated = true;
    }
예제 #5
0
파일: zdraw.c 프로젝트: kshirley/Rainfall
// inputs b and c are placeholders, a is inverted, and the output is c:
void chol_inverse(double **a, int n, double *p, double **b, double **c){
  
  int i, j, k;
  double sum=0.0;
	
  // Cholesky decomposition of Sigma:
  for (i=1; i<=n; i++){
    for (j=i; j<=n; j++){
      for (sum=a[i-1][j-1], k=i-1; k>=1; k--) sum -= a[i-1][k-1]*a[j-1][k-1];
      if (i == j){
	p[i-1]=sqrt(sum);
      } else a[j-1][i-1]=sum/p[i-1];
    }
  }
	
  // Invert the lower triangle:
  for (i=1; i<=n; i++){
    a[i-1][i-1] = 1.0/p[i-1];
    for (j=i+1; j<=n; j++){
      sum = 0.0;
      for (k=i; k<j; k++) sum -= a[j-1][k-1]*a[k-1][i-1];
      a[j-1][i-1] = sum/p[j-1];
    }
  }
  
  // set upper triangular elements to zero:
  for (i=0; i<n; i++){
    for (j=i+1; j<n; j++) a[i][j] = 0.0;
  }
	
  dtp(a,n,n,b); // b becomes transpose of a
  dmm(b,n,n,a,n,n,c);	// a-transpose %*% a = c, where c is the inverse of a
}
예제 #6
0
    void DocumentSourceSort::populateAll() {
        /* track and warn about how much physical memory has been used */
        DocMemMonitor dmm(this);

        /* pull everything from the underlying source */
        for (bool hasNext = !pSource->eof(); hasNext; hasNext = pSource->advance()) {
            documents.push_back(KeyAndDoc(pSource->getCurrent(), vSortKey));
            dmm.addToTotal(documents.back().doc.getApproximateSize());
        }

        /* sort the list */
        Comparator comparator(*this);
        sort(documents.begin(), documents.end(), comparator);
    }
예제 #7
0
파일: servo.c 프로젝트: bitursa/maos
/**
   test type I/II filter with ideal measurement to make sure it is implemented correctly.
*/
dmat* servo_test(dmat *input, double dt, int dtrat, dmat *sigma2n, dmat *gain){
    if(input->ny==1){/*single mode. each column is for a mode.*/
	input->ny=input->nx;
	input->nx=1;
    }
    int nmod=input->nx;
    PDMAT(input,pinput);
    dmat *merr=dnew(nmod,1);
    dcell *mreal=cellnew(1,1);
    dmat *mres=dnew(nmod,input->ny);
    dmat *sigman=NULL;
    if(dnorm(sigma2n)>0){
	sigman=dchol(sigma2n);
    }
    dcell *meas=cellnew(1,1);
    dmat *noise=dnew(nmod, 1);
    SERVO_T *st2t=servo_new(NULL, NULL, 0, dt*dtrat, gain);
    rand_t rstat;
    seed_rand(&rstat, 1);
    PDMAT(mres,pmres);
    /*two step delay is ensured with the order of using, copy, acc*/
    for(int istep=0; istep<input->ny; istep++){
	memcpy(merr->p, pinput[istep], nmod*sizeof(double));
	dadd(&merr, 1, mreal->p[0], -1);
	memcpy(pmres[istep],merr->p,sizeof(double)*nmod);
	if(istep % dtrat == 0){
	    dzero(meas->p[0]);
	}
	dadd(&meas->p[0], 1, merr, 1);/*average the error. */
	dcellcp(&mreal, st2t->mint->p[0]);
	if((istep+1) % dtrat == 0){
	    if(dtrat!=1) dscale(meas->p[0], 1./dtrat);
	    if(sigman){
		drandn(noise, 1, &rstat);
		if(sigman->nx>0){
		    dmm(&meas->p[0], 1, sigman, noise, "nn", 1);
		}else{
		    dadd(&meas->p[0], 1, noise, sigman->p[0]);
		}
	    }
	    servo_filter(st2t, meas);
	}
    }
    dfree(sigman);
    dfree(merr);
    dcellfree(mreal);
    dcellfree(meas);
    servo_free(st2t);
    return mres;
}
예제 #8
0
static void test_corner(){/*Compute the covariance of 4 corner points*/
    rand_t rstat;
    int seed=4;
    double r0=0.2;
    double dx=32;
    long N=1+1;
    long nx=N;
    long ny=N;
    long nframe=1000000;
    seed_rand(&rstat, seed);
    map_t *atm=mapnew(nx, ny, dx, dx,NULL);
    dmat *vec=dref_reshape((dmat*)atm, N*N, 1);
    dmat *cov=NULL;
    for(long i=0; i<nframe; i++){
	info("%ld of %ld\n", i, nframe);
	for(long j=0; j<nx*ny; j++){
	    atm->p[j]=randn(&rstat);
	}
	fractal_do((dmat*)atm, dx, r0,L0,ninit);
	dmm(&cov, 1, vec, vec, "nt", 1);
    }
    dscale(cov, 1./nframe);
    writebin(cov,"cov.bin");
}
예제 #9
0
파일: zdraw.c 프로젝트: kshirley/Rainfall
void zdraw(int *params, double *Smu_in, double *Pxb_in, double *Sig_in, double *Prior_in){
	
  int i, j, s, t, S=params[0], T=params[1];
  double **Smu, **Pxb, **Sig, **Prior; // These are input from R and will be read back out
  double **Sigma, *p, **B, **C, **z_right, **z_mean, **z, **x; // Just variables for the C code
	
  GetRNGstate();	

  Smu = dmatrix(S,T);
  fill_dmatrix(Smu_in, S, T, Smu);

  Pxb = dmatrix(S, T);
  fill_dmatrix(Pxb_in, S, T, Pxb);

  Sig = dmatrix(S, T);
  fill_dmatrix(Sig_in, S, T, Sig);

  Prior = dmatrix(S, S);
  fill_dmatrix(Prior_in, S, S, Prior);

  Sigma = dmatrix(S, S); // Used to create covariance matrix for each time point
  p = dvector(S); // empty vector of length S for cholesky decomposition diagonal
  B = dmatrix(S, S); // placeholder for transpose in cholesky inverse
  C = dmatrix(S, S); // placeholder for result of cholesky inverse
  z_right = dmatrix(S, 1); // right-hand matrix for the mean vector for each time point
  z_mean = dmatrix(S, 1); // mean vector for each time point
  z = dmatrix(S, 1); // set up the vector of length S to generate independent random normals  
  x = dmatrix(S ,1);	// place holder for matrix of correlated random normal draws

	
  for (t = 0; t < T; t++){

    // Construct the covariance matrix for this value of t from Prior (S x S) and Sig (S x T)
    // Note: Each column of Sig will form a diagonal (S x S) matrix that is added to Prior
    for (i = 0; i < S; i++){
      for (j = i; j < S; j++){
	if (i == j) Sigma[i][j] = Prior[i][j] + Sig[i][t];
	else {
	  Sigma[i][j] = Prior[i][j];
	  Sigma[j][i] = Prior[j][i];
	}
      }
    }				
				
    // C becomes inverse of Sigma, via Cholesky decomposition:
    chol_inverse(Sigma, S, p, B, C); // Now Sigma is changed to L^(-1), where L is the lower cholesky decomp of Sigma
	  
    // compute the right-hand (S x 1) matrix in the mean of z_t:
    for (s = 0; s < S; s++) z_right[s][0] = Smu[s][t] + Pxb[s][t];
		
    // compute the mean of z_t
    dmm(C, S, S, z_right, S, 1, z_mean);
		
    // Now C is the lower triangle Cholesky decomp of the original Sigma^(-1)
    chol_lower(C, S, p);
	
    // draw S independent normal RVs:
    for (s = 0; s < S; s++) z[s][0] = rnorm(0, 1);
	
    // Multiply the lower triangular cholesky square root by the independent normal vector:
    dmm(C, S, S, z, S, 1, x); // x is the (S x 1) matrix of correlated random normals
		
    // Write over one of the mean matrices to send output back to R:
    for (s = 0; s < S; s++) Smu[s][t] = x[s][0] + z_mean[s][0]; // Smu will hold the (S x T) matrix of new draws
	  
  }

  
  // read the result back to input vector z_in:
  unfill_dmatrix(Smu, S, T, Smu_in);

  // Free memory:
  free_dmatrix(Smu, S);
  free_dmatrix(Pxb, S);
  free_dmatrix(Sig, S);
  free_dmatrix(Prior, S);
  free_dmatrix(Sigma, S);
  free_dvector(p);
  free_dmatrix(B, S);
  free_dmatrix(C, S);
  free_dmatrix(z_right, S);
  free_dmatrix(z_mean, S);
  free_dmatrix(z, S);
  free_dmatrix(x, S);
	
  PutRNGstate();
	
}
예제 #10
0
/*
  Compute cxx on atm to compare against L2, invpsd, fractal.
*/
static void test_cxx(){
    rand_t rstat;
    int seed=4;
    double r0=0.2;
    double dx=1./4;
    long N=16;
    long nx=N;
    long ny=N;
    long nframe=40960;
    seed_rand(&rstat, seed);
    {
	dmat *cxx=dnew(N*N,N*N);
	map_t *atm=mapnew(nx+1, ny+1, dx, dx,NULL);
	for(long i=0; i<nframe; i++){
	    info("%ld of %ld\n", i, nframe);
	    for(long j=0; j<(nx+1)*(ny+1); j++){
		atm->p[j]=randn(&rstat);
	    }
	    fractal_do((dmat*)atm, dx, r0, L0, ninit);
	    dmat *sec=dsub((dmat*)atm, 0, nx, 0, ny);
	    dmat *atmvec=dref_reshape(sec, nx*ny, 1);
	    dmm(&cxx,1, atmvec,atmvec,"nt",1);
	    dfree(atmvec);
	    dfree(sec);
	}
	dscale(cxx, 1./nframe);
	writebin(cxx, "cxx_fractal");
	dfree(cxx);
	mapfree(atm);
    }
    {
	dmat *cxx=dnew(N*N,N*N);
	dmat *spect=turbpsd(nx, ny, dx, r0, 100, 0, 0.5);
	spect->p[0]=spect->p[1];
	cmat *atm=cnew(nx, ny);
	//cfft2plan(atm, -1);
	dmat *atmr=dnew(nx*ny,1);
	dmat *atmi=dnew(nx*ny,1);
	for(long ii=0; ii<nframe; ii+=2){
	    info("%ld of %ld\n", ii, nframe);
	    for(long i=0; i<atm->nx*atm->ny; i++){
		atm->p[i]=COMPLEX(randn(&rstat), randn(&rstat))*spect->p[i];
	    }
	    cfft2(atm, -1);
	    for(long i=0; i<atm->nx*atm->ny; i++){
		atmr->p[i]=creal(atm->p[i]);
		atmi->p[i]=cimag(atm->p[i]);
	    }
	    dmm(&cxx,1, atmr,atmr,"nt",1);
	    dmm(&cxx,1, atmi,atmi,"nt",1);
	}
	dscale(cxx, 1./nframe);
	writebin(cxx, "cxx_fft");
	dfree(cxx);
	dfree(atmr);
	dfree(atmi);
	cfree(atm);
    }
    loc_t *loc=mksqloc_auto(16,16,1./4,1./4);
    locwrite(loc,"loc");
    dmat *B=stfun_kolmogorov(loc, r0);
    writebin(B, "B_theory");
}
예제 #11
0
/**
   Time domain physical simulation.
   
   noisy: 
   - 0: no noise at all; 
   - 1: poisson and read out noise. 
   - 2: only poisson noise.   
*/
dmat *skysim_sim(dmat **mresout, const dmat *mideal, const dmat *mideal_oa, double ngsol, 
		 ASTER_S *aster, const POWFS_S *powfs, 
		 const PARMS_S *parms, int idtratc, int noisy, int phystart){
    int dtratc=0;
    if(!parms->skyc.multirate){
	dtratc=parms->skyc.dtrats->p[idtratc];
    }
    int hasphy;
    if(phystart>-1 && phystart<aster->nstep){
	hasphy=1;
    }else{
	hasphy=0;
    }
    const int nmod=mideal->nx;
    dmat *res=dnew(6,1);/*Results. 1-2: NGS and TT modes., 
			  3-4:On axis NGS and TT modes,
			  4-6: On axis NGS and TT wihtout considering un-orthogonality.*/
    dmat *mreal=NULL;/*modal correction at this step. */
    dmat *merr=dnew(nmod,1);/*modal error */
    dcell *merrm=dcellnew(1,1);dcell *pmerrm=NULL;
    const int nstep=aster->nstep?aster->nstep:parms->maos.nstep;
    dmat *mres=dnew(nmod,nstep);
    dmat* rnefs=parms->skyc.rnefs;
    dcell *zgradc=dcellnew3(aster->nwfs, 1, aster->ngs, 0);
    dcell *gradout=dcellnew3(aster->nwfs, 1, aster->ngs, 0);
    dmat *gradsave=0;
    if(parms->skyc.dbg){
	gradsave=dnew(aster->tsa*2,nstep);
    }
   
    
    SERVO_T *st2t=0;
    kalman_t *kalman=0;
    dcell *mpsol=0;
    dmat *pgm=0;
    dmat *dtrats=0;
    int multirate=parms->skyc.multirate;
    if(multirate){
	kalman=aster->kalman[0];
	dtrats=aster->dtrats;
    }else{
	if(parms->skyc.servo>0){
	    const double dtngs=parms->maos.dt*dtratc;
	    st2t=servo_new(merrm, NULL, 0, dtngs, aster->gain->p[idtratc]);
	    pgm=aster->pgm->p[idtratc];
	}else{
	    kalman=aster->kalman[idtratc];
	}
    }
    if(kalman){
	kalman_init(kalman);
	mpsol=dcellnew(aster->nwfs, 1); //for psol grad.
    }
    const long nwvl=parms->maos.nwvl;
    dcell **psf=0, **mtche=0, **ints=0;
    ccell *wvf=0,*wvfc=0, *otf=0;
    if(hasphy){
	psf=mycalloc(aster->nwfs,dcell*);
	wvf=ccellnew(aster->nwfs,1);
	wvfc=ccellnew(aster->nwfs,1);
	mtche=mycalloc(aster->nwfs,dcell*);
	ints=mycalloc(aster->nwfs,dcell*);
	otf=ccellnew(aster->nwfs,1);
    
	for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
	    const int ipowfs=aster->wfs[iwfs].ipowfs;
	    const long ncomp=parms->maos.ncomp[ipowfs];
	    const long nsa=parms->maos.nsa[ipowfs];
	    wvf->p[iwfs]=cnew(ncomp,ncomp);
	    wvfc->p[iwfs]=NULL;
	    psf[iwfs]=dcellnew(nsa,nwvl);
	    //cfft2plan(wvf->p[iwfs], -1);
	    if(parms->skyc.multirate){
		mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[(int)aster->idtrats->p[iwfs]];
	    }else{
		mtche[iwfs]=aster->wfs[iwfs].pistat->mtche[idtratc];
	    }
	    otf->p[iwfs]=cnew(ncomp,ncomp);
	    //cfft2plan(otf->p[iwfs],-1);
	    //cfft2plan(otf->p[iwfs],1);
	    ints[iwfs]=dcellnew(nsa,1);
	    int pixpsa=parms->skyc.pixpsa[ipowfs];
	    for(long isa=0; isa<nsa; isa++){
		ints[iwfs]->p[isa]=dnew(pixpsa,pixpsa);
	    }
	}
    }
    for(int irep=0; irep<parms->skyc.navg; irep++){
	if(kalman){
	    kalman_init(kalman);
	}else{
	    servo_reset(st2t);
	}
	dcellzero(zgradc);
	dcellzero(gradout);
	if(ints){
	    for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
		dcellzero(ints[iwfs]);
	    }
	}
	for(int istep=0; istep<nstep; istep++){
	    memcpy(merr->p, PCOL(mideal,istep), nmod*sizeof(double));
	    dadd(&merr, 1, mreal, -1);/*form NGS mode error; */
	    memcpy(PCOL(mres,istep),merr->p,sizeof(double)*nmod);
	    if(mpsol){//collect averaged modes for PSOL.
		for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
		    dadd(&mpsol->p[iwfs], 1, mreal, 1);
		}
	    }
	    pmerrm=0;
	    if(istep>=parms->skyc.evlstart){/*performance evaluation*/
		double res_ngs=dwdot(merr->p,parms->maos.mcc,merr->p);
		if(res_ngs>ngsol*100){
		    dfree(res); res=NULL;
		    break;
		}
		{
		    res->p[0]+=res_ngs;
		    res->p[1]+=dwdot2(merr->p,parms->maos.mcc_tt,merr->p);
		    double dot_oa=dwdot(merr->p, parms->maos.mcc_oa, merr->p);
		    double dot_res_ideal=dwdot(merr->p, parms->maos.mcc_oa, PCOL(mideal,istep));
		    double dot_res_oa=0;
		    for(int imod=0; imod<nmod; imod++){
			dot_res_oa+=merr->p[imod]*IND(mideal_oa,imod,istep);
		    }
		    res->p[2]+=dot_oa-2*dot_res_ideal+2*dot_res_oa;
		    res->p[4]+=dot_oa;
		}
		{
		    double dot_oa_tt=dwdot2(merr->p, parms->maos.mcc_oa_tt, merr->p);
		    /*Notice that mcc_oa_tt2 is 2x5 marix. */
		    double dot_res_ideal_tt=dwdot(merr->p, parms->maos.mcc_oa_tt2, PCOL(mideal,istep));
		    double dot_res_oa_tt=0;
		    for(int imod=0; imod<2; imod++){
			dot_res_oa_tt+=merr->p[imod]*IND(mideal_oa,imod,istep);
		    }
		    res->p[3]+=dot_oa_tt-2*dot_res_ideal_tt+2*dot_res_oa_tt;
		    res->p[5]+=dot_oa_tt;
		}
	    }//if evl

	    if(istep<phystart || phystart<0){
		/*Ztilt, noise free simulation for acquisition. */
		dmm(&zgradc->m, 1, aster->gm, merr, "nn", 1);/*grad due to residual NGS mode. */
		for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
		    const int ipowfs=aster->wfs[iwfs].ipowfs;
		    const long ng=parms->maos.nsa[ipowfs]*2;
		    for(long ig=0; ig<ng; ig++){
			zgradc->p[iwfs]->p[ig]+=aster->wfs[iwfs].ztiltout->p[istep*ng+ig];
		    }
		}
	
		for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
		    int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc);
		    if((istep+1) % dtrati==0){
			dadd(&gradout->p[iwfs], 0, zgradc->p[iwfs], 1./dtrati);
			dzero(zgradc->p[iwfs]);
			if(noisy){
			    int idtrati=(multirate?(int)aster->idtrats->p[iwfs]:idtratc);
			    dmat *nea=aster->wfs[iwfs].pistat->sanea->p[idtrati];
			    for(int i=0; i<nea->nx; i++){
				gradout->p[iwfs]->p[i]+=nea->p[i]*randn(&aster->rand);
			    }
			}
			pmerrm=merrm;//record output.
		    }
		}
	    }else{
		/*Accumulate PSF intensities*/
		for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
		    const double thetax=aster->wfs[iwfs].thetax;
		    const double thetay=aster->wfs[iwfs].thetay;
		    const int ipowfs=aster->wfs[iwfs].ipowfs;
		    const long nsa=parms->maos.nsa[ipowfs];
		    ccell* wvfout=aster->wfs[iwfs].wvfout[istep];
		    for(long iwvl=0; iwvl<nwvl; iwvl++){
			double wvl=parms->maos.wvl[iwvl];
			for(long isa=0; isa<nsa; isa++){
			    ccp(&wvfc->p[iwfs], IND(wvfout,isa,iwvl));
			    /*Apply NGS mode error to PSF. */
			    ngsmod2wvf(wvfc->p[iwfs], wvl, merr, powfs+ipowfs, isa,
				       thetax, thetay, parms);
			    cembedc(wvf->p[iwfs],wvfc->p[iwfs],0,C_FULL);
			    cfft2(wvf->p[iwfs],-1);
			    /*peak in corner. */
			    cabs22d(&psf[iwfs]->p[isa+nsa*iwvl], 1., wvf->p[iwfs], 1.);
			}/*isa */
		    }/*iwvl */
		}/*iwfs */
	
		/*Form detector image from accumulated PSFs*/
		double igrad[2];
		for(long iwfs=0; iwfs<aster->nwfs; iwfs++){
		    int dtrati=dtratc, idtrat=idtratc;
		    if(multirate){//multirate
			idtrat=aster->idtrats->p[iwfs];
			dtrati=dtrats->p[iwfs];
		    }
		    if((istep+1) % dtrati == 0){/*has output */
			dcellzero(ints[iwfs]);
			const int ipowfs=aster->wfs[iwfs].ipowfs;
			const long nsa=parms->maos.nsa[ipowfs];
			for(long isa=0; isa<nsa; isa++){
			    for(long iwvl=0; iwvl<nwvl; iwvl++){
				double siglev=aster->wfs[iwfs].siglev->p[iwvl];
				ccpd(&otf->p[iwfs],psf[iwfs]->p[isa+nsa*iwvl]);
				cfft2i(otf->p[iwfs], 1); /*turn to OTF, peak in corner */
				ccwm(otf->p[iwfs], powfs[ipowfs].dtf[iwvl].nominal);
				cfft2(otf->p[iwfs], -1);
				dspmulcreal(ints[iwfs]->p[isa]->p, powfs[ipowfs].dtf[iwvl].si, 
					   otf->p[iwfs]->p, siglev);
			    }
		
			    /*Add noise and apply matched filter. */
#if _OPENMP >= 200805 
#pragma omp critical 
#endif
			    switch(noisy){
			    case 0:/*no noise at all. */
				break;
			    case 1:/*both poisson and read out noise. */
				{
				    double bkgrnd=aster->wfs[iwfs].bkgrnd*dtrati;
				    addnoise(ints[iwfs]->p[isa], &aster->rand, bkgrnd, bkgrnd, 0,0,IND(rnefs,idtrat,ipowfs));
				}
				break;
			    case 2:/*there is still poisson noise. */
				addnoise(ints[iwfs]->p[isa], &aster->rand, 0, 0, 0,0,0);
				break;
			    default:
				error("Invalid noisy\n");
			    }
		
			    igrad[0]=0;
			    igrad[1]=0;
			    double pixtheta=parms->skyc.pixtheta[ipowfs];
			    if(parms->skyc.mtch){
				dmulvec(igrad, mtche[iwfs]->p[isa], ints[iwfs]->p[isa]->p, 1);
			    }
			    if(!parms->skyc.mtch || fabs(igrad[0])>pixtheta || fabs(igrad[1])>pixtheta){
				if(!parms->skyc.mtch){
				    warning2("fall back to cog\n");
				}else{
				    warning_once("mtch is out of range\n");
				}
				dcog(igrad, ints[iwfs]->p[isa], 0, 0, 0, 3*IND(rnefs,idtrat,ipowfs), 0); 
				igrad[0]*=pixtheta;
				igrad[1]*=pixtheta;
			    }
			    gradout->p[iwfs]->p[isa]=igrad[0];
			    gradout->p[iwfs]->p[isa+nsa]=igrad[1];
			}/*isa */
			pmerrm=merrm;
			dcellzero(psf[iwfs]);/*reset accumulation.*/
		    }/*if iwfs has output*/
		}/*for wfs*/
	    }/*if phystart */
	    //output to mreal after using it to ensure two cycle delay.
	    if(st2t){//Type I or II control.
		if(st2t->mint->p[0]){//has output.
		    dcp(&mreal, st2t->mint->p[0]->p[0]);
		}
	    }else{//LQG control
		kalman_output(kalman, &mreal, 0, 1);
	    }
	    if(kalman){//LQG control
		int indk=0;
		//Form PSOL grads and obtain index to LQG M
		for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
		    int dtrati=(multirate?(int)dtrats->p[iwfs]:dtratc);
		    if((istep+1) % dtrati==0){
			indk|=1<<iwfs;
			dmm(&gradout->p[iwfs], 1, aster->g->p[iwfs], mpsol->p[iwfs], "nn", 1./dtrati);
			dzero(mpsol->p[iwfs]);
		    }
		}
		if(indk){
		    kalman_update(kalman, gradout->m, indk-1);
		}
	    }else if(st2t){
		if(pmerrm){
		    dmm(&merrm->p[0], 0, pgm, gradout->m, "nn", 1);	
		}
		servo_filter(st2t, pmerrm);//do even if merrm is zero. to simulate additional latency
	    }
	    if(parms->skyc.dbg){
		memcpy(PCOL(gradsave, istep), gradout->m->p, sizeof(double)*gradsave->nx);
	    }
	}/*istep; */
    }
    if(parms->skyc.dbg){
	int dtrati=(multirate?(int)dtrats->p[0]:dtratc);
	writebin(gradsave,"%s/skysim_grads_aster%d_dtrat%d",dirsetup, aster->iaster,dtrati);
	writebin(mres,"%s/skysim_sim_mres_aster%d_dtrat%d",dirsetup,aster->iaster,dtrati);
    }
  
    dfree(mreal);
    dcellfree(mpsol);
    dfree(merr);
    dcellfree(merrm);
    dcellfree(zgradc);
    dcellfree(gradout);
    dfree(gradsave);
    if(hasphy){
	dcellfreearr(psf, aster->nwfs);
	dcellfreearr(ints, aster->nwfs);
        ccellfree(wvf);
	ccellfree(wvfc);
	ccellfree(otf);
	free(mtche);
    }
    servo_free(st2t);
    /*dfree(mres); */
    if(mresout) {
	*mresout=mres;
    }else{
	dfree(mres);
    }
    dscale(res, 1./((nstep-parms->skyc.evlstart)*parms->skyc.navg));
    return res;
}