/**
filter DM commands in open loop mode by simply copy the output
*/
static void filter_ol(SIM_T *simu){
assert(!simu->parms->sim.closeloop);
if(simu->dmerr){
dcellcp(&simu->dmcmd, simu->dmerr);
}else{
dcellzero(simu->dmcmd);
}
if(simu->Merr_lo){
addlow2dm(&simu->dmcmd, simu, simu->Merr_lo,1);
}
extern int DM_NCPA;
if(DM_NCPA && simu->recon->dm_ncpa){
warning_once("Add NCPA after integrator\n");
dcelladd(&simu->dmcmd, 1, simu->recon->dm_ncpa, 1);
}
//Extrapolate to edge actuators
if(simu->recon->actinterp && !simu->parms->recon.modal){
dcellcp(&simu->dmcmd0, simu->dmcmd);
dcellzero(simu->dmcmd);
dcellmm(&simu->dmcmd, simu->recon->actinterp, simu->dmcmd0, "nn", 1);
}
if(simu->ttmreal){
ttsplit_do(simu->recon, simu->dmcmd, simu->ttmreal, simu->parms->sim.lpttm);
}
/*hysterisis. */
if(simu->hyst){
hyst_dcell(simu->hyst, simu->dmreal, simu->dmcmd);
}
/*moao DM is already taken care of automatically.*/
}
/**
Applies type I or type II filter based on number of entries in gain.
*/
int servo_filter(SERVO_T *st, const dcell *_merr){
if(!st->initialized && _merr){
servo_init(st, _merr);
}
const dcell *merr=servo_shift_al(st, _merr);
if(!merr) return 0;
servo_shift_ap(st);
if(!st->mint){
error("SERVO_T must be created using servo_new()\n");
}
switch(st->ep->nx){
case 1://type I
if(st->ep->ny==1){
dcelladd(&st->mpreint, 0, merr, st->ep->p[0]);//just record what is added.
}else{
if(!st->mpreint){
st->mpreint=dcellnew2(merr);
}
for(int ic=0; ic<merr->nx; ic++){
if(!merr->p[ic]) continue;
assert(merr->p[ic]->nx==st->ep->ny);
for(long i=0; i<merr->p[ic]->nx; i++){
st->mpreint->p[ic]->p[i]=st->ep->p[i]*merr->p[ic]->p[i];
}
}
}
break;
case 2:{//PID controller
if(st->ep->ny!=1) error("not supported\n");
double g1=st->ep->p[0]+st->ep->p[1];
double g2=-st->ep->p[1];
dcelladd(&st->mpreint, 0, merr, g1);
dcelladd(&st->mpreint, 1, st->merrlast, g2);
dcellcp(&st->merrlast, merr);
}
break;
case 3://type II
servo_typeII_filter(st, merr);
break;
default:
error("Invalid: st->ep->nx=%ld", st->ep->nx);
}
dcelladd(st->mint->p, 1, st->mpreint, 1);
return 1;
}
/**
Apply type II servo filter on measurement error and output integrator. gain
must be 3x1 or 3x5. */
static inline void
servo_typeII_filter(SERVO_T *st, const dcell *merrc){
if(!merrc) return;
const dmat *gain=st->ep;
PDMAT(gain,pgain);
int indmul=0;
if(gain->nx!=3){
error("Wrong format in gain\n");
}
double gg,e1a, e1;
for(int ic=0; ic<merrc->nx*merrc->ny; ic++){
dmat *merr=merrc->p[ic];
if(!merr) continue;
dmat *mlead=st->mlead->p[ic];
dmat *merrlast=st->merrlast->p[ic];
int nmod=0;/*error. */
if(merr->ny==1){
nmod=merr->nx;
}else{
if(merr->nx!=1){
error("Don't handle this case\n");
}
nmod=merr->ny;
}
if(gain->ny==1){
indmul=0;
}else if(gain->ny==nmod){
indmul=1;
}else{
error("Wrong format in gain\n");
}
/**2013-12-03: fix lead filter implementation. gg is relocated
2013-12-05: Implemented a more accurate and robust lead filter
*/
for(int imod=0; imod<nmod; imod++){
int indm=imod * indmul;
gg=pgain[indm][0];
e1a=pgain[indm][1];
e1=pgain[indm][2];
mlead->p[imod] = e1a*mlead->p[imod]+gg*(1-e1a)/(1-e1)*(merr->p[imod]-e1*merrlast->p[imod]);
}
}
dcellcp(&st->merrlast, merrc);
dcelladd(&st->mpreint,1, st->mlead, 1);
}
/*A FIFO queue to add delay*/
static const dcell*servo_shift_al(SERVO_T *st, const dcell *merr){
if(!st->al){
return merr;
}else{
long nhist=st->merrhist->nx;
dcell *cycle=st->merrhist->p[0];
for(int i=0; i<nhist-1; i++){
st->merrhist->p[i]=st->merrhist->p[i+1];
}
st->merrhist->p[nhist-1]=cycle;
if(!merr){
dcellfree(st->merrhist->p[nhist-1]);
}else{
dcelladd(&st->merrhist->p[nhist-1], 0, merr, 1);
}
return st->merrhist->p[0];
}
}
/**
prepare the integrator by shifting commands. similar to laos.
inte->p[0]=inte->p[0]*ap[0]+inte->p[1]*ap[1]+...
*/
static void servo_shift_ap(SERVO_T *st){
const dmat *ap=st->ap;
if(!ap) return; //no need to shift.
if(st->mint->nx<ap->nx){
cellresize(st->mint, ap->nx, 1);
}
if(!st->initialized) return;
dcell **inte=st->mint->p;
dcell *cyclic=inte[ap->nx-1];
dcellscale(cyclic, ap->p[ap->nx-1]);
for(int iap=ap->nx-2; iap>=0; iap--){
dcelladd(&cyclic,1,inte[iap],ap->p[iap]);
}
for(int iap=ap->nx-1; iap>0; iap--){
inte[iap]=inte[iap-1];/*shifting */
}
inte[0]=cyclic;/*new command. */
}
/**
Setup the least square reconstruct by directly inverting GA matrix.
The reconstructor is simply the pseudo inverse of GA matrix:
\f[\hat{x}=(G_a^TC_g^{-1}G_a)^{-1}G_a^TC_g^{-1}\f]
This is very close to RR except replacing GX with GA.
We use the tomograhy parameters for lsr, since lsr is simply "tomography" onto DM directly.
*/
void setup_recon_lsr(RECON_T *recon, const PARMS_T *parms){
const int ndm=parms->ndm;
const int nwfs=parms->nwfsr;
cell *GAlsr;
cell *GAM=parms->recon.modal?(cell*)recon->GM:(cell*)recon->GA;
if(parms->recon.split){ //high order wfs only in split mode.
GAlsr=parms->recon.modal?(cell*)recon->GMhi:(cell*)recon->GAhi;
}else{ //all wfs in integrated mode.
GAlsr=GAM;
}
int free_GAlsr=0;
if(GAlsr->p[0]->id!=M_DBL){
dsp *tmp=dsp_cast(GAlsr->p[0]);
if(tmp->nzmax>tmp->nx*tmp->ny*0.2){//not very sparse
dcell *tmp2=0;
free_GAlsr=1;
dcelladd(&tmp2, 1, (dspcell*)GAlsr, 1);
GAlsr=(cell*)tmp2;
}
}
info2("Building recon->LR\n");
recon->LR.M=dcellmm2(GAlsr, recon->saneai, "tn");
// Tip/tilt and diff focus removal low rand terms for LGS WFS.
if(recon->TTF){
dcellmm(&recon->LR.U, recon->LR.M, recon->TTF, "nn", 1);
recon->LR.V=dcelltrans(recon->PTTF);
}
info2("Building recon->LL\n");
recon->LL.M=dcellmm2(recon->LR.M, GAlsr, "nn");
if(free_GAlsr){
cellfree(GAlsr);
}
double maxeig=pow(recon->neamhi * recon->aloc->p[0]->dx, -2);
if(parms->recon.modal){
double strength=1;
for(int idm=0; idm<ndm; idm++){
strength*=dnorm(recon->amod->p[idm]);
}
strength=pow(strength, 2./ndm);
maxeig*=strength;
}
if(fabs(parms->lsr.tikcr)>EPS){
info2("Adding tikhonov constraint of %g to LLM\n", parms->lsr.tikcr);
info2("The maximum eigen value is estimated to be around %g\n", maxeig);
dcelladdI(recon->LL.M, parms->lsr.tikcr*maxeig);
}
dcell *NW=NULL;
if(!parms->recon.modal){
if(parms->lsr.alg!=2){
/* Not SVD, need low rank terms for piston/waffle mode constraint. */
NW=dcellnew(ndm,1);
int nmod=2;/*two modes. */
for(int idm=0; idm<ndm; idm++){
loc_create_map(recon->aloc->p[idm]);
const long nloc=recon->aloc->p[idm]->nloc;
NW->p[idm]=dnew(nloc, ndm*nmod);
double *p=NW->p[idm]->p+nmod*idm*nloc;
const double *cpl=recon->actcpl->p[idm]->p;
for(long iloc=0; iloc<nloc; iloc++){
if(cpl[iloc]>0.1){
p[iloc]=1;/*piston mode */
}
}
/*notice offset of 1 because map start count at 1 */
p=NW->p[idm]->p+(1+nmod*idm)*nloc-1;
map_t *map=recon->aloc->p[idm]->map;
for(long iy=0; iy<map->ny; iy++){
for(long ix=0; ix<map->nx; ix++){
if(IND(map,ix,iy)){
p[(long)IND(map,ix,iy)]=(double)2*((iy+ix)&1)-1;
}
}
}
}
/*scale it to match the magnitude of LL.M */
dcellscale(NW, sqrt(maxeig));
if(parms->save.setup){
writebin(NW, "lsrNW");
}
}
if(parms->lsr.actslave){
/*actuator slaving. important. change from 0.5 to 0.1 on 2011-07-14. */
dspcell *actslave=slaving(recon->aloc, recon->actcpl, NW,
recon->actstuck, recon->actfloat, parms->lsr.actthres, maxeig);
if(parms->save.setup){
if(NW){
writebin(NW, "lsrNW2");
}
writebin(actslave,"actslave");
}
dcelladd(&recon->LL.M, 1, actslave, 1);
cellfree(actslave);
}
}
/*Low rank terms for low order wfs. Only in Integrated tomography. */
dcell *ULo=dcellnew(ndm,nwfs);
dcell *VLo=dcellnew(ndm,nwfs);
dcell* pULo=ULo/*PDELL*/;
dcell* pVLo=VLo/*PDELL*/;
for(int iwfs=0; iwfs<nwfs; iwfs++){
int ipowfs=parms->wfsr[iwfs].powfs;
if(parms->powfs[ipowfs].skip || !parms->powfs[ipowfs].lo){
continue;
}
for(int idm=0; idm<ndm; idm++){
dspfull(PIND(pULo,idm,iwfs), (dsp*)IND(recon->LR.M, idm, iwfs),'n',-1);
dspfull(PIND(pVLo,idm,iwfs), (dsp*)IND(GAM, iwfs, idm),'t',1);
}
}
recon->LL.U=dcellcat(recon->LR.U, ULo, 2);
dcell *GPTTDF=NULL;
dcellmm(&GPTTDF, GAM, recon->LR.V, "tn", 1);
recon->LL.V=dcellcat(GPTTDF, VLo, 2);
dcellfree(GPTTDF);
dcellfree(ULo);
dcellfree(VLo);
if(!parms->recon.modal && NW){
info2("Create piston and check board modes that are in NULL space of GA.\n");
/*add to low rank terms. */
dcell *tmp=recon->LL.U;
recon->LL.U=dcellcat(tmp, NW, 2);
dcellfree(tmp);
dcellscale(NW, -1);
tmp=recon->LL.V;
recon->LL.V=dcellcat(tmp, NW, 2);
dcellfree(tmp);
dcellfree(NW);
}
if(parms->lsr.fnreg){
warning("Loading LSR regularization from file %s.\n", parms->lsr.fnreg);
dspcell *tmp=dspcellread("%s", parms->lsr.fnreg);
dcelladd(&recon->LL.M, 1, tmp, 1);
dspcellfree(tmp);
}
recon->LL.alg = parms->lsr.alg;
recon->LL.bgs = parms->lsr.bgs;
recon->LL.warm = parms->recon.warm_restart;
recon->LL.maxit = parms->lsr.maxit;
/*Remove empty cells. */
dcelldropempty(&recon->LR.U,2);
dcelldropempty(&recon->LR.V,2);
dcelldropempty(&recon->LL.U,2);
dcelldropempty(&recon->LL.V,2);
if(parms->save.recon){
writebin(recon->LR.M,"LRM");
writebin(recon->LR.U,"LRU");
writebin(recon->LR.V,"LRV");
writebin(recon->LL.M,"LLM.bin");/*disable compression */
writebin(recon->LL.U,"LLU");
writebin(recon->LL.V,"LLV");
}
if(parms->lsr.alg==0 || parms->lsr.alg==2){
if(!parms->lsr.bgs){
muv_direct_prep(&recon->LL, (parms->lsr.alg==2)*parms->lsr.svdthres);
if(parms->save.recon){
if(recon->LL.C)
chol_save(recon->LL.C, "LLC.bin");
else
writebin(recon->LL.MI, "LLMI.bin");
}
cellfree(recon->LL.M);
dcellfree(recon->LL.U);
dcellfree(recon->LL.V);
}else{
muv_direct_diag_prep(&(recon->LL), (parms->lsr.alg==2)*parms->lsr.svdthres);
if(parms->save.recon){
for(int ib=0; ib<recon->LL.nb; ib++){
if(recon->LL.CB)
chol_save(recon->LL.CB[ib],"LLCB_%d.bin", ib);
else
writebin(recon->LL.MI,"LLMIB_%d.bin", ib);
}
}
/*Don't free M, U, V */
}
}
}
/**
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);
}
}
}
}
/**
Assemble the DM fitting matrix
The fitting is done by minimizing \f$||H_X x - H_A a||^2_W\f$ where \f$H_X,
H_A\f$ are ray tracing operator from tomography grid xloc, and deformable
mirror grid aloc to pupil grid ploc. The norm is weighted using bilinear
influence functions within the telescope aperture. We have
\f$a=\left[H_A^T(W_0-W_1 W_1^T)H_A\right]^{-1} H_A^T (W_0-W_1) H_X x\f$
For details see www.opticsinfobase.org/abstract.cfm?URI=josaa-19-9-1803
*/
static void
setup_recon_fit_matrix(RECON_T *recon, const PARMS_T *parms){
const int nfit=parms->fit.nfit;
const int ndm=parms->ndm;
if(ndm==0) return;
dspcell *HATc=dspcelltrans(recon->HA);
PDSPCELL(HATc, HAT);
PDSPCELL(recon->HA,HA);
info2("Before assembling fit matrix:\t%.2f MiB\n",get_job_mem()/1024.);
/*Assemble Fit matrix. */
int npsr=recon->npsr;
if(parms->load.fit){
if(!(zfexist("FRM") && zfexist("FRU") && zfexist("FRV"))){
error("FRM, FRU, FRV (.bin) not all exist\n");
}
warning("Loading saved recon->FR\n");
recon->FR.M=readbin("FRM");
recon->FR.U=dcellread("FRU");
recon->FR.V=dcellread("FRV");
}else{
if(recon->HXF){
info2("Building recon->FR\n");
recon->FR.M=cellnew(ndm, npsr);
PDSPCELL(recon->FR.M, FRM);
PDSPCELL(recon->HXF, HXF);
for(int ips=0; ips<npsr; ips++){
for(int ifit=0; ifit<nfit; ifit++){
if(fabs(recon->fitwt->p[ifit])<1.e-12) continue;
dsp *tmp=dspmulsp(recon->W0, HXF[ips][ifit],"nn");
for(int idm=0; idm<ndm; idm++){
dspmulsp2(&FRM[ips][idm],HAT[ifit][idm], tmp, "nn",
recon->fitwt->p[ifit]);
}
dspfree(tmp);
}
}
recon->FR.V=cellnew(npsr, 1);
dmat **FRV=recon->FR.V->p;
for(int ips=0; ips<npsr; ips++){
int nloc=recon->xloc->p[ips]->nloc;
FRV[ips]=dnew(nloc,nfit);
for(int ifit=0; ifit<nfit; ifit++){
/*notice the sqrt. */
if(fabs(recon->fitwt->p[ifit])<1.e-12) continue;
dspmulvec(FRV[ips]->p+ifit*nloc,
HXF[ips][ifit], recon->W1->p, 't',
sqrt(recon->fitwt->p[ifit]));
}
}
if(parms->save.recon){
writebin(recon->FR.M,"FRM");
writebin(recon->FR.V,"FRV");
}
}else{
info("Avoid building recon->FR.M\n");
recon->FR.M=NULL;
recon->FR.V=NULL;
}
/*Always need FR.U as it is used to do FL.U, FL.V */
recon->FR.U=cellnew(ndm, 1);
dmat **FRU=recon->FR.U->p;
for(int idm=0; idm<ndm; idm++){
int nloc=recon->aloc->p[idm]->nloc;
FRU[idm]=dnew(nloc, nfit);
for(int ifit=0; ifit<nfit; ifit++){
/*notice the sart. */
if(fabs(recon->fitwt->p[ifit])<1.e-12) continue;
dspmulvec(FRU[idm]->p+ifit*nloc,
HA[idm][ifit], recon->W1->p,'t',
sqrt(recon->fitwt->p[ifit]));
}
}
if(parms->save.recon){
writebin(recon->FR.U,"FRU");
}
}
if(parms->load.fit){
if(!(zfexist("FLM") && zfexist("FLU") && zfexist("FLV"))){
error("FLM, FLU, FLV (.bin) not all exist\n");
}
warning("Loading saved recon->FL\n");
recon->FL.M=readbin("FLM");
recon->FL.U=dcellread("FLU");
recon->FL.V=dcellread("FLV");
}else{
info2("Building recon->FL\n");
recon->FL.M=cellnew(ndm, ndm);
dsp *(*FLM)[ndm]=(dsp*(*)[ndm])recon->FL.M->p;
for(int idm=0; idm<ndm; idm++){
for(int ifit=0; ifit<nfit; ifit++){
if(fabs(recon->fitwt->p[ifit])<1.e-12) continue;
dsp *tmp=dspmulsp(recon->W0, HA[idm][ifit],"nn");
for(int jdm=0; jdm<ndm; jdm++){
dspmulsp2(&FLM[idm][jdm],HAT[ifit][jdm], tmp,"nn",
recon->fitwt->p[ifit]);
}
dspfree(tmp);
}
}
dspcellfree(HATc);
if(fabs(parms->fit.tikcr)>1.e-15){
double tikcr=parms->fit.tikcr;
/*Estimated from the formula. 1/nloc is due to W0, the other
scaling is due to ray tracing between different sampling freq.*/
int nact=0;
for(int idm=0; idm<parms->ndm; idm++){
nact+=recon->aloc->p[idm]->nloc;
}
double maxeig=4./nact;
info2("Adding tikhonov constraint of %g to FLM\n", tikcr);
info2("The maximum eigen value is estimated to be around %e\n", maxeig);
dcelladdI(recon->FL.M,tikcr*maxeig);
}
{/*Low rank terms. */
recon->FL.U=dcellcat_each(recon->FR.U, recon->fitNW, 2);
dcell *tmp=NULL;/*negative NW. */
dcelladd(&tmp, 1, recon->fitNW, -1);
recon->FL.V=dcellcat_each(recon->FR.U, tmp, 2);
dcellfree(tmp);
}
if(recon->actslave){
dcelladd(&recon->FL.M, 1, recon->actslave, 1);
}
/*dspcellsym(recon->FL.M); */
info2("DM Fit number of Low rank terms: %ld in RHS, %ld in LHS\n",
recon->FR.U->p[0]->ny, recon->FL.U->p[0]->ny);
if(parms->save.recon){
writebin(recon->FL.M,"FLM.bin");
writebin(recon->FL.U,"FLU");
writebin(recon->FL.V,"FLV");
}
}
if((parms->fit.alg==0 || parms->fit.alg==2) && parms->fit.bgs){
muv_direct_diag_prep(&(recon->FL),(parms->fit.alg==2)*parms->fit.svdthres);
}
if((parms->fit.alg==0 || parms->fit.alg==2) && !parms->fit.bgs){
if(fabs(parms->fit.tikcr)<1.e-14){
warning("tickcr=%g is too small, chol may fail.\n", parms->fit.tikcr);
}
muv_direct_prep(&(recon->FL),(parms->fit.alg==2)*parms->fit.svdthres);
info2("After cholesky/svd on matrix:\t%.2f MiB\n",get_job_mem()/1024.);
}
if(parms->save.recon){
if(recon->FL.C){
chol_convert(recon->FL.C, 1);
chol_save(recon->FL.C,"FLC.bin");
}
if(recon->FL.MI)
writebin(recon->FL.MI,"FLMI");
if(recon->FL.Up)
writebin(recon->FL.Up, "FLUp");
if(recon->FL.Vp)
writebin(recon->FL.Vp, "FLVp");
if(recon->FL.CB){
for(int ib=0; ib<recon->FL.nb; ib++){
chol_save(recon->FL.CB[ib],"FLCB_%d.bin", ib);
}
}
if(recon->FL.MIB){
writebin(recon->FL.MIB,"FLMIB");
}
}
info2("After assemble fit matrix:\t%.2f MiB\n",get_job_mem()/1024.);
}
/**
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);
}
}
}
}
