/**
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;
}
INT NS_DIM_PREFIX deadd (MULTIGRID *mg, INT fl, INT tl, INT mode, EVECDATA_DESC *x, const EVECDATA_DESC *y)
{
INT i,ret,level;
ret=dadd(mg,fl,tl,mode,x->vd,y->vd); if (ret!=NUM_OK) return ret;
for (level=fl; level<=tl; level++)
for (i=0; i<x->n; i++) EVDD_E(x,level,i)+=EVDD_E(y,level,i);
return NUM_OK;
}
static void /* column adjust for range -1 to 1 to a to b */
colshft(double a, double b, projUV **d, int n, int m) {
int k, j;
double fac, cnst;
cnst = 2. / (b - a);
fac = cnst;
for (j = 1; j < n; ++j) {
dmult(d[j], fac, m);
fac *= cnst;
}
cnst = .5 * (a + b);
for (j = 0; j <= n-2; ++j)
for (k = n - 2; k >= j; --k)
dadd(d[k], d[k+1], cnst, m);
}
/**
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;
}
/**
Update DM command for next cycle using info from last cycle (two cycle delay)
in closed loop mode */
static void filter_cl(SIM_T *simu){
/*
2009-11-02: Moved to the end of isim loop to update
for next step. only need to cache a single dmerrlast
now.
2009-12-23: Updated low fs to do lead filter/type II
2010-01-07: Create an option to merge the last
integrator in the hi/lo loop to simulation the actual
block diagram. removed dmreal_hi, Mreal_lo;
2010-01-08: Changed the filtering scheme by computing
dm command for next cycle instead of maintianing last
step error information.
2010-01-13: Implemented apdm.
a(n)=a(n-1)+ep*e(n-2) or
a(n)=0.5*(a(n-1)+a(n-2))+ep*e(n-2);
*/
const PARMS_T *parms=simu->parms;
RECON_T *recon=simu->recon;
assert(parms->sim.closeloop);
/*copy dm computed in last cycle. This is used in next cycle (already after perfevl) */
const SIM_CFG_T *simcfg=&(parms->sim);
const int isim=simu->isim;
{/*Auto adjusting epdm for testing different epdm*/
static int epdm_is_auto=0;
if(simcfg->epdm->p[0]<0){
epdm_is_auto=1;
simcfg->epdm->p[0]=0.5;
}
if(epdm_is_auto){
if((isim*10)<parms->sim.end){//initial steps
simcfg->epdm->p[0]=0.5;
}else if((isim*10)%parms->sim.end==0){
simcfg->epdm->p[0]=(double)isim/(double)parms->sim.end;
info("epdm is set to %.1f at step %d\n", simcfg->epdm->p[0], isim);
}
}
}
/*Do the servo filtering. First simulate a drop frame*/
int drop=0;
if(simu->dmerr && parms->sim.dtrat_skip){
if(parms->sim.dtrat_skip>0){
if((isim+1)%parms->sim.dtrat_skip==0){//evenly
drop=1;
}
}else if(parms->sim.dtrat_skip<0){//use random draws
double tmp=randu(simu->misc_rand);
if(tmp*(-parms->sim.dtrat_skip)<1.){
drop=1;
}
}
}
dcell *dmerr=0;
if(drop){
warning("Drop a frame at step %d\n", isim);
}else if(simu->dmerr){
dmerr=simu->dmerr;
}
//always run servo_filter even if dmerr is NULL.
int hiout=servo_filter(simu->dmint, dmerr);
if(parms->recon.split){
/*Low order in split tomography only. fused integrator*/
if(servo_filter(simu->Mint_lo, simu->Merr_lo) && parms->sim.fuseint){
/*accumulate to the main integrator.*/
addlow2dm(&simu->dmint->mint->p[0], simu, simu->Mint_lo->mpreint, 1);
}
}
/*The following are moved from the beginning to the end because the
gradients are now from last step.*/
dcellcp(&simu->dmcmd0,simu->dmint->mint->p[0]);
if(!parms->sim.fuseint){
addlow2dm(&simu->dmcmd0,simu,simu->Mint_lo->mint->p[0], 1);
}
for(int ipowfs=0; ipowfs<parms->npowfs; ipowfs++){
//Record dmpsol for this time step for each powfs before updating it (z^-1).
//Do not reference the data, even for dtrat==1
if(!parms->powfs[ipowfs].psol || !parms->powfs[ipowfs].dtrat) continue;
double alpha=(isim % parms->powfs[ipowfs].dtrat == 0)?0:1;
dcelladd(&simu->wfspsol->p[ipowfs], alpha, simu->dmpsol, 1./parms->powfs[ipowfs].dtrat);
}
dcellcp(&simu->dmpsol, simu->dmcmd0);
if(parms->recon.modal){
dcellzero(simu->dmcmd);
dcellmm(&simu->dmcmd, simu->recon->amod, simu->dmcmd0, "nn", 1);
//convert DM command from modal to zonal spae
}else if(simu->recon->actinterp && !parms->recon.psol){
//Extrapolate to edge actuators
dcellzero(simu->dmcmd);
dcellmm(&simu->dmcmd, simu->recon->actinterp, simu->dmcmd0, "nn", 1);
}else{
dcellcp(&simu->dmcmd, simu->dmcmd0);
}
//The DM commands are always on zonal modes from this moment
if(simu->ttmreal){
ttsplit_do(recon, simu->dmcmd, simu->ttmreal, parms->sim.lpttm);
}
if(parms->sim.focus2tel && hiout){
dcellcp(&simu->telfocusreal, simu->telfocusint);
dcellmm(&simu->telfocusint, recon->RFdm, simu->dmcmd, "nn", parms->sim.epfocus2tel);
}
if(recon->dither_m){
//Change phase in calc_dither_amp if phase of dithering is changed
//this is for step isim+1
double anglei=((isim+1)/recon->dither_dtrat)*(2*M_PI/recon->dither_npoint);
dcelladd(&simu->dmcmd, 1, recon->dither_m, sin(anglei));
}
if(!parms->dbg.ncpa_preload && recon->dm_ncpa){
info_once("Add NCPA after integrator\n");
dcelladd(&simu->dmcmd, 1, recon->dm_ncpa, 1);
}
if(parms->sim.dmclip || parms->sim.dmclipia || recon->actstuck){
dcell *tmp=dcelldup(simu->dmcmd);
if(recon->actstuck){//zero stuck actuators
act_stuck_cmd(recon->aloc, simu->dmerr, recon->actstuck);
}
clipdm(simu, simu->dmcmd);
dcelladd(&tmp, 1, simu->dmcmd, -1); //find what is clipped
dcelladd(&simu->dmint->mint->p[0], 1, tmp, -1);//remove from integrator (anti wind up)
dcelladd(&simu->dmpsol, 1, tmp, -1);//also feed to PSOL (is this really necessary?)
dcellfree(tmp);
}
/*This is after the integrator output and clipping*/
if(simu->dmhist){
for(int idm=0; idm<parms->ndm; idm++){
if(simu->dmhist->p[idm]){
dhistfill(&simu->dmhist->p[idm], simu->dmcmd->p[idm],0,
parms->dm[idm].histbin, parms->dm[idm].histn);
}
}
}
/*hysteresis. */
if(simu->hyst){
hyst_dcell(simu->hyst, simu->dmreal, simu->dmcmd);
}
if(recon->moao && !parms->gpu.moao){
warning_once("moao filter implemented with LPF\n");
if(simu->dm_wfs){
const int nwfs=parms->nwfs;
for(int iwfs=0; iwfs<nwfs; iwfs++){
int ipowfs=parms->wfs[iwfs].powfs;
int imoao=parms->powfs[ipowfs].moao;
if(imoao<0) continue;
double g=parms->moao[imoao].gdm;
dadd(&simu->dm_wfs->p[iwfs], 1-g, simu->dm_wfs->p[iwfs+nwfs], g);
}
}
if(simu->dm_evl){
const int nevl=parms->evl.nevl;
int imoao=parms->evl.moao;
double g=parms->moao[imoao].gdm;
for(int ievl=0; ievl<nevl; ievl++){
dadd(&simu->dm_evl->p[ievl], 1-g, simu->dm_evl->p[ievl+nevl], g);
}
}
}
if(simu->fsmint){
/*fsmerr is from gradients from this time step. so copy before update for correct delay*/
dcellcp(&simu->fsmreal, simu->fsmint->mint->p[0]);
servo_filter(simu->fsmint, simu->fsmerr);
/*Inject dithering command, for step isim+1*/
for(int iwfs=0; iwfs<parms->nwfs; iwfs++){
const int ipowfs=parms->wfs[iwfs].powfs;
if(parms->powfs[ipowfs].dither==1){//T/T dithering.
//adjust delay due to propagation, and computation delay.
const int adjust=parms->sim.alfsm+1-parms->powfs[ipowfs].dtrat;
//Use isim+1 because the command is for next time step.
//minus adjust for delay
double anglei=(2*M_PI/parms->powfs[ipowfs].dither_npoint);
double angle=((isim+1-adjust)/parms->powfs[ipowfs].dtrat)*anglei;
simu->fsmreal->p[iwfs]->p[0]-=parms->powfs[ipowfs].dither_amp*cos(angle);
simu->fsmreal->p[iwfs]->p[1]-=parms->powfs[ipowfs].dither_amp*sin(angle);
}
}
}
}
long setup_star_read_ztilt(STAR_S *star, int nstar, const PARMS_S *parms, int seed){
const double ngsgrid=parms->maos.ngsgrid;
long nstep=0;
TIC;tic;
for(int istar=0; istar<nstar; istar++){
STAR_S *stari=&star[istar];
int npowfs=parms->maos.npowfs;
stari->ztiltout=dcellnew(npowfs, 1);
const double thetax=stari->thetax*206265;/*in as */
const double thetay=stari->thetay*206265;
double thxnorm=thetax/ngsgrid;
double thynorm=thetay/ngsgrid;
long thxl=(long)floor(thxnorm);/*Used to be double, but -0 appears. */
long thyl=(long)floor(thynorm);
double wtx=thxnorm-thxl;
double wty=thynorm-thyl;
for(int ipowfs=0; ipowfs<npowfs; ipowfs++){
const int msa=parms->maos.msa[ipowfs];
const int nsa=parms->maos.nsa[ipowfs];
const int ng=nsa*2;
char *fnztilt[2][2]={{NULL,NULL},{NULL,NULL}};
char *fngoff[2][2]={{NULL, NULL}, {NULL, NULL}};
double wtsum=0;
for(int ix=0; ix<2; ix++){
double thx=(thxl+ix)*ngsgrid;
for(int iy=0; iy<2; iy++){
double thy=(thyl+iy)*ngsgrid;
double wtxi=fabs(((1-ix)-wtx)*((1-iy)-wty));
if(wtxi<0.01){
/*info("skipping ix=%d,iy=%d because wt=%g\n",ix,iy,wtxi); */
continue;
}
fnztilt[iy][ix]=myalloca(PATH_MAX, char);
if(parms->skyc.usephygrad){
warning_once("Using phygrad\n");
snprintf(fnztilt[iy][ix],PATH_MAX,"%s/phygrad/phygrad_seed%d_sa%d_x%g_y%g",
dirstart,seed,msa,thx,thy);
}else{
snprintf(fnztilt[iy][ix],PATH_MAX,"%s/ztiltout/ztiltout_seed%d_sa%d_x%g_y%g",
dirstart,seed,msa,thx,thy);
}
fngoff[iy][ix]=myalloca(PATH_MAX, char);
snprintf(fngoff[iy][ix],PATH_MAX,"%s/gradoff/gradoff_sa%d_x%g_y%g",
dirstart,msa,thx,thy);
if(!zfexist(fnztilt[iy][ix])){
//warning("%s doesnot exist\n",fnwvf[iy][ix]);
fnztilt[iy][ix]=fngoff[iy][ix]=NULL;
}else{
wtsum+=wtxi;
}
}
}
if(wtsum<0.01){
error("PSF is not available for (%g,%g). wtsum=%g\n",thetax,thetay, wtsum);
}
/*Now do the actual reading */
for(int ix=0; ix<2; ix++){
for(int iy=0; iy<2; iy++){
double wtxi=fabs(((1-ix)-wtx)*((1-iy)-wty))/wtsum;
if(fnztilt[iy][ix]){
file_t *fp_ztilt=zfopen(fnztilt[iy][ix],"rb");
header_t header={0,0,0,0};
read_header(&header, fp_ztilt);
if(iscell(&header.magic)){
// error("expected data type: %u, got %u\n",(uint32_t)MCC_ANY, header.magic);
nstep=header.nx;
free(header.str);
if(stari->nstep==0){
stari->nstep=nstep;
}else{
if(stari->nstep!=nstep){
error("Different type has different steps\n");
}
}
if(!stari->ztiltout->p[ipowfs]){
stari->ztiltout->p[ipowfs]=dnew(ng, nstep);
}
dmat *ztiltout=stari->ztiltout->p[ipowfs];
for(long istep=0; istep<nstep; istep++){
dmat *ztilti=dreaddata(fp_ztilt, 0);
for(int ig=0; ig<ng; ig++){
ztiltout->p[ig+istep*ng]+=ztilti->p[ig]*wtxi;
}
dfree(ztilti);
}
}else{
dmat *tmp=dreaddata(fp_ztilt, &header);
dadd(&stari->ztiltout->p[ipowfs], 1, tmp, wtxi );
dfree(tmp);
}
zfclose(fp_ztilt);
}/* if(fnwvf) */
if(fngoff[iy][ix] && zfexist(fngoff[iy][ix])){
if(!stari->goff){
stari->goff=dcellnew(npowfs, 1);
}
dmat *tmp=dread("%s", fngoff[iy][ix]);
dadd(&stari->goff->p[ipowfs], 1, tmp, wtxi);
dfree(tmp);
}
}/*iy */
}/*ix */
}/*ipowfs */
}/*istar */
if(parms->skyc.verbose){
toc2("Reading PSF");
}
//close(fd);
return nstep;
}
/**
Setup matched filter for stars.
*/
static void setup_star_mtch(const PARMS_S *parms, POWFS_S *powfs, STAR_S *star, int nstar, dcell**nonlin){
const long nwvl=parms->maos.nwvl;
const long npowfs=parms->maos.npowfs;
dmat* rnefs=parms->skyc.rnefs;
for(int istar=0; istar<nstar; istar++){
if(!star[istar].idtrat){
star[istar].idtrat=dnew(npowfs, 1);
}
double radius=sqrt(pow(star[istar].thetax,2)+pow(star[istar].thetay,2));
int igg=round(radius*206265/parms->maos.ngsgrid);
//info("radius=%g as, igg=%d\n", radius*206265, igg);
for(int ipowfs=0; ipowfs<npowfs; ipowfs++){
const long nsa=parms->maos.nsa[ipowfs];
const long pixpsa=parms->skyc.pixpsa[ipowfs];
//size of PSF
const double sigma_theta=parms->skyc.wvlmean/parms->maos.dxsa[ipowfs];
PISTAT_S *pistat=&star[istar].pistat[ipowfs];
pistat->i0=dcellnew(nsa,nwvl);
pistat->gx=dcellnew(nsa,nwvl);
pistat->gy=dcellnew(nsa,nwvl);
pistat->i0s=dcellnew(nsa,1);
pistat->gxs=dcellnew(nsa,1);
pistat->gys=dcellnew(nsa,1);
dcell* psf=pistat->psf;
dcell* i0=pistat->i0;
dcell* gx=pistat->gx;
dcell* gy=pistat->gy;
for(long iwvl=0; iwvl<nwvl; iwvl++){
for(long isa=0; isa<nsa; isa++){
double siglev=star[istar].siglev->p[ipowfs]->p[iwvl];
IND(i0,isa,iwvl)=dnew(pixpsa,pixpsa);
IND(gx,isa,iwvl)=dnew(pixpsa,pixpsa);
IND(gy,isa,iwvl)=dnew(pixpsa,pixpsa);
psf2i0gxgy(IND(i0,isa,iwvl),IND(gx,isa,iwvl),IND(gy,isa,iwvl),
IND(psf,isa,iwvl),powfs[ipowfs].dtf+iwvl);
dadd(&pistat->i0s->p[isa], 1, IND(i0,isa,iwvl), siglev);
dadd(&pistat->gxs->p[isa], 1, IND(gx,isa,iwvl), siglev);
dadd(&pistat->gys->p[isa], 1, IND(gy,isa,iwvl), siglev);
}
}
if(parms->skyc.dbg){
writebin(pistat->i0s, "%s/star%d_ipowfs%d_i0s", dirsetup,istar,ipowfs);
}
const double pixtheta=parms->skyc.pixtheta[ipowfs];
int ndtrat=parms->skyc.ndtrat;
pistat->mtche=mycalloc(ndtrat,dcell*);
pistat->sanea=dcellnew(ndtrat,1);
pistat->sanea0=dcellnew(ndtrat,1);
pistat->snr=dnew(ndtrat,1);
dcell *i0s=NULL; dcell *gxs=NULL; dcell *gys=NULL;
for(int idtrat=0; idtrat<ndtrat; idtrat++){
int dtrat=parms->skyc.dtrats->p[idtrat];
dcelladd(&i0s, 0, pistat->i0s, dtrat);
dcelladd(&gxs, 0, pistat->gxs, dtrat);
dcelladd(&gys, 0, pistat->gys, dtrat);
genmtch(&pistat->mtche[idtrat], &pistat->sanea->p[idtrat],
i0s, gxs, gys, pixtheta, IND(rnefs,idtrat,ipowfs),
star[istar].bkgrnd->p[ipowfs]*dtrat, parms->skyc.mtchcr);
/*Add nolinearity*/
if(nonlin){
//add linearly not quadratically since the errors are related.
dmat *nea_nonlin=dinterp1(nonlin[ipowfs]->p[igg], NULL, pistat->sanea->p[idtrat], 0);
for(int i=0; i<nsa*2; i++){
//info2("%g mas", pistat->sanea->p[idtrat]->p[i]*206265000);
pistat->sanea->p[idtrat]->p[i]=sqrt(pow(pistat->sanea->p[idtrat]->p[i],2)
+pow(nea_nonlin->p[i],2));
//info2("-->%g mas\n", pistat->sanea->p[idtrat]->p[i]*206265000);
}
dfree(nea_nonlin);
}
dcp(&pistat->sanea0->p[idtrat], pistat->sanea->p[idtrat]);
if(parms->skyc.neaaniso){
for(int i=0; i<nsa*2; i++){
pistat->sanea->p[idtrat]->p[i]=sqrt(pow(pistat->sanea->p[idtrat]->p[i],2)
+pow(star[istar].pistat[ipowfs].neaspec->p[i]->p[idtrat], 2));
}
}
if(parms->skyc.dbg){
writebin(pistat->mtche[idtrat], "%s/star%d_ipowfs%d_mtche_dtrat%d",
dirsetup,istar,ipowfs,dtrat);
}
#if 1
double nea=sqrt(dsumsq(pistat->sanea->p[idtrat])/(nsa*2));
#else
double nea=dmax(pistat->sanea->p[idtrat]);
#endif
double snr=sigma_theta/nea;
pistat->snr->p[idtrat]=snr;
if(parms->skyc.verbose) info2("dtrat=%3d, nea=%4.1f, snr=%4.1f\n",
(int)parms->skyc.dtrats->p[idtrat], nea*206265000, snr);
if(snr>parms->skyc.snrmin->p[parms->skyc.snrmin->nx==1?0:idtrat]){
star[istar].idtrat->p[ipowfs]=idtrat;
}
}//for idtrat
if(parms->skyc.dbg){
info2("star %2d optim: powfs %1d: dtrat=%3d\n", istar, ipowfs,
(int)parms->skyc.dtrats->p[(int)star[istar].idtrat->p[ipowfs]]);
writebin(pistat->sanea, "%s/star%d_ipowfs%d_sanea",
dirsetup,istar,ipowfs);
}/*idtrat */
dcellfree(i0s);
dcellfree(gxs);
dcellfree(gys);
}/*for istar */
}/*for ipowfs */
}
/**
Read in pistat information, used to compute matched filter, and SANEA.
*/
static void setup_star_read_pistat(SIM_S *simu, STAR_S *star, int nstar, int seed){
const PARMS_S *parms=simu->parms;
const int npowfs=parms->maos.npowfs;
const int nwvl=parms->maos.nwvl;
const double ngsgrid=parms->maos.ngsgrid;
for(int istar=0; istar<nstar; istar++){
STAR_S *stari=&star[istar];
stari->pistat=mycalloc(npowfs,PISTAT_S);
const double thetax=stari->thetax*206265;/*in as */
const double thetay=stari->thetay*206265;
double thxnorm=thetax/ngsgrid;
double thynorm=thetay/ngsgrid;
long thxl=(long)floor(thxnorm);
long thyl=(long)floor(thynorm);
double wtx=thxnorm-thxl;
double wty=thynorm-thyl;
for(int ipowfs=0; ipowfs<npowfs; ipowfs++){
const int msa=parms->maos.msa[ipowfs];
const int nsa=parms->maos.nsa[ipowfs];
dcell *avgpsf=NULL;
dcell *neaspec=NULL;
double wtsum=0;
for(int ix=0; ix<2; ix++){
double thx=(thxl+ix)*ngsgrid;
for(int iy=0; iy<2; iy++){
double thy=(thyl+iy)*ngsgrid;
double wtxi=fabs(((1-ix)-wtx)*((1-iy)-wty));
if(wtxi<0.01){
/*info("skipping ix=%d,iy=%d because wt=%g\n",ix,iy,wtxi); */
continue;
}
char fn[PATH_MAX];
snprintf(fn,PATH_MAX,"%s/pistat/pistat_seed%d_sa%d_x%g_y%g",
dirstart, seed,msa,thx,thy);
if(!zfexist(fn)){
/*warning("%s doesn't exist\n",fn); */
}else{
dcell *avgpsfi=dcellread("%s",fn);
dcelladd(&avgpsf, 1, avgpsfi, wtxi);
dcellfree(avgpsfi);
wtsum+=wtxi;
snprintf(fn,PATH_MAX,"%s/neaspec/neaspec_seed%d_sa%d_x%g_y%g",
dirstart, seed, msa, thx, thy);
dcell *neaspeci=dcellread("%s",fn);
dcelladd(&neaspec, 1, neaspeci, wtxi);
dcellfree(neaspeci);
}
}
}
if(wtsum<0.01){
warning("PISTAT is not available for (%g,%g) msa=%d\n",thetax,thetay,msa);
}
dcellscale(neaspec, 1./wtsum);
dcellscale(avgpsf, 1./wtsum);
dmat *scale=NULL;
if(parms->skyc.bspstrehl){
scale=dnew(nsa,nwvl);
dmat *gx=dnew(1,1); gx->p[0]=thxnorm;
dmat *gy=dnew(1,1); gy->p[0]=thynorm;
if(nsa!=avgpsf->nx || nwvl!=avgpsf->ny){
error("Mismatch: nsa=%d, nwvl=%d, avgpsf->nx=%ld, avgpsf->ny=%ld\n",
nsa, nwvl, avgpsf->nx, avgpsf->ny);
}
for(int ic=0; ic<nsa*nwvl; ic++){
dmat *val=dbspline_eval(simu->bspstrehl[ipowfs][ic],
simu->bspstrehlxy,simu->bspstrehlxy,
gx, gy);
double ratio=val->p[0]/avgpsf->p[ic]->p[0];
/*info("strehl: bilinear: %g, cubic: %g\n", avgpsf->p[ic]->p[0],val->p[0]); */
if(ratio<0){
warning("Ratio=%g is less than zero.\n", ratio);
scale->p[ic]=1;
}else{
dscale(avgpsf->p[ic], ratio);
scale->p[ic]=ratio;
}
dfree(val);
}
dfree(gx);
dfree(gy);
}
stari->pistat[ipowfs].psf=avgpsf;/*PSF is in corner. */
stari->pistat[ipowfs].neaspec=dcellnew(nsa*2, 1);
for(int ig=0; ig<nsa*2; ig++){
dmat *tmp=0;
for(int iwvl=0; iwvl<nwvl; iwvl++){
dadd(&tmp, 0, neaspec->p[ig+nsa*2*iwvl], parms->skyc.wvlwt->p[iwvl]);
}
stari->pistat[ipowfs].neaspec->p[ig]=dinterp1(simu->neaspec_dtrats, tmp, parms->skyc.dtrats, 0);
dfree(tmp);
}
dcellfree(neaspec);
stari->pistat[ipowfs].scale=scale;
{/* skip stars with large PSF.*/
int size=INT_MAX;
for(int ic=0; ic<avgpsf->nx*avgpsf->ny; ic++){
int size0=dfwhm(avgpsf->p[ic]);
if(size0<size) size=size0;
}
if(size>6){
stari->use[ipowfs]=-1;
}
}
if(parms->skyc.dbg){
writebin(avgpsf, "%s/avgpsf_star%d_ipowfs%d_psf",dirsetup,istar,ipowfs);
writebin(stari->pistat[ipowfs].neaspec, "%s/pistat_star%d_ipowfs%d_neaspec",dirsetup,istar,ipowfs);
}
}
}
}
