static void setup_aster_kalman(SIM_S *simu, ASTER_S *aster, STAR_S *star, const PARMS_S *parms){
int ndtrat=parms->skyc.ndtrat;
if(parms->skyc.multirate){
aster->res_ngs=dnew(1,1);
//assemble neam
if(aster->neam) error("neam is already set?\n");
aster->neam=calloc(1, sizeof(dcell*));
aster->neam[0]=cellnew(aster->nwfs, aster->nwfs);
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
int ng=aster->g->p[iwfs]->nx;
aster->neam[0]->p[iwfs+aster->nwfs*iwfs]=dnew(ng, ng);
}
aster->dtrats=dnew(aster->nwfs, 1);
aster->idtrats=dnew(aster->nwfs, 1);
int wfs0_min=0, wfs0_max=0;
PISTAT_S *pistat0=&star[aster->wfs[0].istar].pistat[aster->wfs[0].ipowfs];
for(int idtrat=0; idtrat<parms->skyc.ndtrat; idtrat++){
if(wfs0_min==0 && pistat0->snr->p[idtrat]>3){
wfs0_min=idtrat;
}
if(pistat0->snr->p[idtrat]>=parms->skyc.snrmin->p[idtrat]){
wfs0_max=idtrat;
}
}
aster->kalman=calloc(1, sizeof(kalman_t*));
double resmin=INFINITY;
kalman_t *kalman_min=0;
int idtrat_min=0;
//Try progressively lower sampling frequencies until performance starts to degrades
for(int idtrat_limit=wfs0_max; idtrat_limit>wfs0_min; idtrat_limit--){
setup_aster_kalman_dtrat(aster, star, parms, idtrat_limit);
aster->kalman[0]=sde_kalman(simu->sdecoeff, parms->maos.dt, aster->dtrats, aster->g, aster->neam[0], 0);
dmat *rests=0;
#if 1 //more accurate
dmat *res=skysim_sim(parms->skyc.dbg?&rests:0, simu->mideal, simu->mideal_oa, simu->rmsol,
aster, 0, parms, -1, 1, -1);
double res0=res?res->p[0]:simu->rmsol;
dfree(res);
#else
rests=kalman_test(aster->kalman[0], simu->mideal);
double res0=calc_rms(rests, parms->maos.mcc, parms->skyc.evlstart);
#endif
if(parms->skyc.dbg){
writebin(rests, "isky%d_iaster%d_dtrat%d_rest", simu->isky, aster->iaster, idtrat_limit);
}
if(parms->skyc.verbose) info2("res0=%g, resmin=%g\n", sqrt(res0)*1e9, sqrt(resmin)*1e9);
dfree(rests);
if(res0<resmin-100e-18){//better by 10 nm
resmin=res0;
kalman_free(kalman_min);
kalman_min=aster->kalman[0];
aster->kalman[0]=0;
idtrat_min=idtrat_limit;
}else{
kalman_free(aster->kalman[0]);aster->kalman[0]=0;
if(isfinite(resmin) && res0>resmin*2){//stop trying.
break;
}
}
}
setup_aster_kalman_dtrat(aster, star, parms, idtrat_min);
if(parms->skyc.verbose) info2("selected\n");
aster->res_ngs->p[0]=resmin;
aster->kalman[0]=kalman_min;
}else{
if(aster->neam) dcellfreearr(aster->neam, ndtrat);
aster->neam=calloc(ndtrat, sizeof(dcell*));
aster->res_ngs=dnew(ndtrat,3);
PDMAT(aster->res_ngs, pres_ngs);
aster->kalman=calloc(ndtrat, sizeof(kalman_t*));
dmat *dtrats=dnew(aster->nwfs,1);
for(int idtrat=0; idtrat<ndtrat; idtrat++){
//assemble neam
//TIC;tic;
aster->neam[idtrat]=cellnew(aster->nwfs, aster->nwfs);
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
dmat *tmp=ddup(aster->wfs[iwfs].pistat->sanea->p[idtrat]);/*in rad */
dcwpow(tmp, 2);
dsp *tmp2=dspnewdiag(tmp->nx, tmp->p, 1);
dspfull(&aster->neam[idtrat]->p[iwfs+aster->nwfs*iwfs], tmp2,'n',1);
dfree(tmp); dspfree(tmp2);
}
int dtrat=parms->skyc.dtrats->p[idtrat];
for(int iwfs=0; iwfs<aster->nwfs; iwfs++){
dtrats->p[iwfs]=dtrat;
}
aster->kalman[idtrat]=sde_kalman(simu->sdecoeff, parms->maos.dt, dtrats, aster->g, aster->neam[idtrat], 0);
//toc("kalman");
#if 1
dmat *res=skysim_sim(0, simu->mideal, simu->mideal_oa, simu->rmsol, aster, 0, parms, idtrat, 1, -1);
double rms=res?res->p[0]:simu->rmsol;
dfree(res);
#else
dmat *res=kalman_test(aster->kalman[idtrat], simu->mideal);
double rms=calc_rms(res, parms->maos.mcc, parms->skyc.evlstart);
dfree(res);
#endif
//toc("estimate");
pres_ngs[0][idtrat]=rms;
}
dfree(dtrats);
}
}
/**
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 */
}
}
}
