/**
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;
}
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);
}
}
}
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");
}
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;
}
// 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
}
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);
}
/**
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;
}
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");
}
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();
}
/*
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");
}
/**
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;
}