static void test_cov(){/*not good */
rand_t rstat;
int seed=4;
double r0=0.2;
double dx=1./64;
long N=1+1024;
long nx=N;
long ny=N;
long nframe=1;
seed_rand(&rstat, seed);
map_t *atm=mapnew(nx, ny, dx,dx, NULL);
cmat *atmhat=cnew((N+1)*3,(N+1)*3);
dmat *atmhattot=dnew((N+1)*3,(N+1)*3);
//cfft2plan(atmhat,-1);
//cfft2plan(atmhat, 1);
dset((dmat*)atm,1);
cembedd(atmhat, (dmat*)atm, 0);
cfft2(atmhat, -1);
cabs22d(&atmhattot, 1, atmhat, 1);
ccpd(&atmhat, atmhattot);
cfft2i(atmhat, 1);
cfftshift(atmhat);
dmat *denom=dnew((N+1)*3,(N+1)*3);
dmat *cov=dnew((N+1)*3,(N+1)*3);
creal2d(&denom, 0, atmhat, 1);
writebin(denom, "denom.bin");
dzero(atmhattot);
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);
/*mapwrite(atm, "atm_%ld.bin", i); */
cembedd(atmhat, (dmat*)atm, 0);
cfft2(atmhat, -1);
cabs22d(&atmhattot, 1, atmhat, 1);
if(i==0 || (i+1)%10==0){
dscale(atmhattot, 1./(i+1));
ccpd(&atmhat, atmhattot);
writebin(atmhattot, "atm_psf_%ld.bin",i+1);
cfft2i(atmhat, 1);
cfftshift(atmhat);
creal2d(&cov, 0, atmhat, 1);
for(long k=0; k<cov->nx*cov->ny; k++){
cov->p[k]/=denom->p[k];
}
writebin(cov, "atm_cov_%ld.bin",i+1);
}
}
}
/**
Setup the detector transfer functions. See maos/setup_powfs.c
*/
static void setup_powfs_dtf(POWFS_S *powfs, const PARMS_S* parms){
const int npowfs=parms->maos.npowfs;
for(int ipowfs=0; ipowfs<npowfs; ipowfs++){
const int ncomp=parms->maos.ncomp[ipowfs];
const int ncomp2=ncomp>>1;
const int embfac=parms->maos.embfac[ipowfs];
const int pixpsa=parms->skyc.pixpsa[ipowfs];
const double pixtheta=parms->skyc.pixtheta[ipowfs];
const double blur=parms->skyc.pixblur[ipowfs]*pixtheta;
const double e0=exp(-2*M_PI*M_PI*blur*blur);
const double dxsa=parms->maos.dxsa[ipowfs];
const double pixoffx=parms->skyc.pixoffx[ipowfs];
const double pixoffy=parms->skyc.pixoffy[ipowfs];
const double pxo=-(pixpsa/2-0.5+pixoffx)*pixtheta;
const double pyo=-(pixpsa/2-0.5+pixoffy)*pixtheta;
loc_t *loc_ccd=mksqloc(pixpsa, pixpsa, pixtheta, pixtheta, pxo, pyo);
powfs[ipowfs].dtf=mycalloc(parms->maos.nwvl,DTF_S);
for(int iwvl=0; iwvl<parms->maos.nwvl; iwvl++){
const double wvl=parms->maos.wvl[iwvl];
const double dtheta=wvl/(dxsa*embfac);
const double pdtheta=pixtheta*pixtheta/(dtheta*dtheta);
const double du=1./(dtheta*ncomp);
const double du2=du*du;
const double dupth=du*pixtheta;
cmat *nominal=cnew(ncomp,ncomp);
//cfft2plan(nominal,-1);
//cfft2plan(nominal,1);
cmat* pn=nominal;
const double theta=0;
const double ct=cos(theta);
const double st=sin(theta);
for(int iy=0; iy<ncomp; iy++){
int jy=iy-ncomp2;
for(int ix=0; ix<ncomp; ix++){
int jx=ix-ncomp2;
double ir=ct*jx+st*jy;
double ia=-st*jx+ct*jy;
IND(pn,ix,iy)=sinc(ir*dupth)*sinc(ia*dupth)
*pow(e0,ir*ir*du2)*pow(e0,ia*ia*du2)
*pdtheta;
}
}
if(parms->skyc.fnpsf1[ipowfs]){
warning("powfs %d has additional otf to be multiplied\n", ipowfs);
dcell *psf1c=dcellread("%s", parms->skyc.fnpsf1[ipowfs]);
dmat *psf1=NULL;
if(psf1c->nx == 1){
psf1=dref(psf1c->p[0]);
}else if(psf1c->nx==parms->maos.nwvl){
psf1=dref(psf1c->p[iwvl]);
}else{
error("skyc.fnpsf1 has wrong dimension\n");
}
dcellfree(psf1c);
if(psf1->ny!=2){
error("skyc.fnpsf1 has wrong dimension\n");
}
dmat *psf1x=dnew_ref(psf1->nx, 1, psf1->p);
dmat *psf1y=dnew_ref(psf1->nx, 1, psf1->p+psf1->nx);
dmat *psf2x=dnew(ncomp*ncomp, 1);
for(int iy=0; iy<ncomp; iy++){
int jy=iy-ncomp2;
for(int ix=0; ix<ncomp; ix++){
int jx=ix-ncomp2;
psf2x->p[ix+iy*ncomp]=sqrt(jx*jx+jy*jy)*dtheta;
}
}
info("powfs %d, iwvl=%d, dtheta=%g\n", ipowfs, iwvl, dtheta*206265000);
writebin(psf2x, "powfs%d_psf2x_%d", ipowfs,iwvl);
dmat *psf2=dinterp1(psf1x, psf1y, psf2x, 0);
normalize_sum(psf2->p, psf2->nx*psf2->ny, 1);
psf2->nx=ncomp; psf2->ny=ncomp;
writebin(psf2, "powfs%d_psf2_%d", ipowfs,iwvl);
cmat *otf2=cnew(ncomp, ncomp);
//cfft2plan(otf2, -1);
ccpd(&otf2, psf2);//peak in center
cfftshift(otf2);//peak in corner
cfft2(otf2, -1);
cfftshift(otf2);//peak in center
writebin(otf2, "powfs%d_otf2_%d", ipowfs, iwvl);
writebin(nominal, "powfs%d_dtf%d_nominal_0",ipowfs,iwvl);
for(int i=0; i<ncomp*ncomp; i++){
nominal->p[i]*=otf2->p[i];
}
writebin(nominal, "powfs%d_dtf%d_nominal_1",ipowfs,iwvl);
dfree(psf1x);
dfree(psf1y);
dfree(psf2x);
dfree(psf1);
cfree(otf2);
dfree(psf2);
}
cfftshift(nominal);//peak in corner
cfft2(nominal,-1);
cfftshift(nominal);//peak in center
cfft2i(nominal,1);
warning_once("double check nominal for off centered skyc.fnpsf1\n");
/*This nominal will multiply to OTF with peak in corner. But after
inverse fft, peak will be in center*/
ccp(&powfs[ipowfs].dtf[iwvl].nominal, nominal);
cfree(nominal);
loc_t *loc_psf=mksqloc(ncomp, ncomp, dtheta, dtheta, -ncomp2*dtheta, -ncomp2*dtheta);
powfs[ipowfs].dtf[iwvl].si=mkh(loc_psf,loc_ccd,0,0,1);
locfree(loc_psf);
if(parms->skyc.dbg){
writebin(powfs[ipowfs].dtf[iwvl].nominal,
"%s/powfs%d_dtf%d_nominal",dirsetup,ipowfs,iwvl);
writebin(powfs[ipowfs].dtf[iwvl].si,
"%s/powfs%d_dtf%d_si",dirsetup,ipowfs,iwvl);
}
powfs[ipowfs].dtf[iwvl].U=cnew(ncomp,1);
dcomplex *U=powfs[ipowfs].dtf[iwvl].U->p;
for(int ix=0; ix<ncomp; ix++){
int jx=ix<ncomp2?ix:(ix-ncomp);
U[ix]=COMPLEX(0, -2.*M_PI*jx*du);
}
}/*iwvl */
locfree(loc_ccd);
}/*ipowfs */
}
/**
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;
}
