/**
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);
/*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]+=IND(psp,ix,iy)*IND(mcc,ix,iy);
}
}
}
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 setup_recon_fit(RECON_T *recon, const PARMS_T *parms){
TIC;tic;
if(!parms->sim.idealfit){
/*In idealfit, xloc has high sampling. We avoid HXF. */
setup_recon_HXF(recon,parms);
}
/*copy over fitwt since we need a dmat */
int nfit=parms->fit.nfit;
recon->fitwt=dnew(nfit,1);
dcp(&recon->fitwt,parms->fit.wt);
/*always assemble fit matrix, faster if many directions */
if(parms->fit.assemble){
setup_recon_fit_matrix(recon,parms);
}
toc2("Generating fit matrix ");
/*Fall back function method if FR.M is NULL (!HXF<-idealfit) */
if(!recon->FR.M){
recon->FR.Mfun = FitR;
recon->FR.Mdata = recon;
}
/*Fall back function method if FL.M is NULL */
if(!recon->FL.M){
recon->FL.Mfun = FitL;
recon->FL.Mdata = recon;
}
recon->FL.alg = parms->fit.alg;
recon->FL.bgs = parms->fit.bgs;
recon->FL.warm = parms->recon.warm_restart;
recon->FL.maxit = parms->fit.maxit;
}
static void test_ints(){
rand_t init;
seed_rand(&init,1);
const int nwfs=6;
lrand(&init);/*atm */
rand_t wfs_rand[nwfs];
for(int iwfs=0; iwfs<nwfs; iwfs++){
seed_rand(wfs_rand+iwfs,lrand(&init));
}
dcell *mtche=dcellread("powfs0_mtche.bin");
int nsim=500;
int nsa=2582;
dmat *nea=dnew(nsim,nsa*2);
double(*pnea)[nsim]=(void*)nea->p;
double rne=3;
double bkgrnd=0;
double siglev=1000;
for(int iwfs=0; iwfs<nwfs; iwfs++){
for(int isim=0; isim<nsim; isim++){
dbg("iwfs %d isim=%d\n",iwfs,isim);
/*dcell *ints=dcellread("ints_%d_wfs%d.bin",isim,iwfs); */
dcell *ints=dcellread("ints_%d_wfs%d.bin",isim,iwfs);
/*dcell *i0=dcellread("powfs0_i0.bin"); */
dmat *im=NULL, *imy=NULL;
double gnf[2], gny[2];
for(int isa=0; isa<nsa; isa++){
dcp(&im,ints->p[isa]);
dscale(im,siglev);
gnf[0]=0; gnf[1]=0;
dmulvec(gnf, mtche->p[isa], im->p,1.);
gny[0]=0; gny[1]=0;
dcp(&imy, im);
addnoise(imy, &wfs_rand[iwfs], bkgrnd, bkgrnd, 0,0,rne);
dmulvec(gny, mtche->p[isa], imy->p,1.);
P(pnea,isim,isa)=gny[0]-gnf[0];
P(pnea,isim,isa+nsa)=gny[1]-gnf[1];
}
}
writebin(nea,"test_sanea_wfs%d.bin",iwfs);
}
}
void wxSkinWindow::OnPaint(wxPaintEvent& e)
{
wxBufferedPaintDC dcp(this);
wxSize size = GetSize();
if(m_inside && m_imageOver.IsOk())
{
dcp.DrawBitmap(m_scaleFill ? wxBitmap(m_imageOver.Scale(size.x, size.y)) : m_imageOver, 0, 0, true);
}
else if(IsEnabled() && (m_imageNormal.IsOk() || m_imageFocus.IsOk()))
{
if(m_imageFocus.IsOk() && HasFocus())
dcp.DrawBitmap(m_scaleFill ? wxBitmap(m_imageFocus.Scale(size.x, size.y)) : m_imageFocus, 0, 0, true);
else
dcp.DrawBitmap(m_scaleFill ? wxBitmap(m_imageNormal.Scale(size.x, size.y)) : m_imageNormal, 0, 0, true);
}
else if(m_imageDisabled.IsOk())
{
dcp.DrawBitmap(m_scaleFill ? wxBitmap(m_imageDisabled.Scale(size.x, size.y)) : m_imageDisabled, 0, 0, true);
}
DrawCustom(dcp);
}
int main (int argc, char **argv)
{
Q330 q330;
Q330_ADDR addr;
init(argc, argv, &q330);
switch (q330.cmd.code) {
case Q330_CMD_REBOOT:
boot(&q330);
break;
case Q330_CMD_SAVE:
save(&q330);
break;
case Q330_CMD_SAVEBOOT:
save(&q330);
sleep(5);
boot(&q330);
break;
case Q330_CMD_RESYNC:
resync(&q330);
break;
case Q330_CMD_GPS_ON:
gpsON(&q330);
break;
case Q330_CMD_GPS_OFF:
gpsOFF(&q330);
break;
case Q330_CMD_GPS_COLD:
gpsColdStart(&q330);
break;
case Q330_CMD_GPS_CNF:
gpsCnf(&q330);
break;
case Q330_CMD_GPS_ID:
gpsId(&q330);
break;
case Q330_CMD_CAL_START:
calStart(&q330);
break;
case Q330_CMD_CAL_STOP:
calStop(&q330);
break;
case Q330_CMD_IFCONFIG:
ifconfig(&q330);
break;
case Q330_CMD_STATUS:
status(&q330);
break;
case Q330_CMD_FIX:
fix(&q330);
break;
case Q330_CMD_GLOB:
glob(&q330);
break;
case Q330_CMD_SC:
sc(&q330);
break;
case Q330_CMD_PULSE:
pulse(&q330);
break;
case Q330_CMD_AMASS:
amass(&q330);
break;
case Q330_CMD_DCP:
dcp(&q330);
break;
case Q330_CMD_SPP:
spp(&q330);
break;
case Q330_CMD_MAN:
man(&q330);
break;
case Q330_CMD_CO:
checkout(&q330);
break;
default:
fprintf(stderr, "ERROR: command '%s' is unsupported\n", q330.cmd.name);
break;
}
if (q330.qdp != NULL) qdpDeregister(q330.qdp, TRUE);
}
/**
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;
}
static inline void clipdm(SIM_T *simu, dcell *dmcmd){
const PARMS_T *parms=simu->parms;
if(!dmcmd) return;
/*
clip integrator. This both limits the output and
feeds back the clip since we are acting on the integrator directly.
*/
if(parms->sim.dmclip){
for(int idm=0; idm<parms->ndm; idm++){
const int nact=dmcmd->p[idm]->nx;
if(parms->dm[idm].stroke->nx==1){
if(parms->dm[idm].stroke->ny!=1){
error("dm.stroke is in wrong format\n");
}
int nclip=dclip(dmcmd->p[idm],
-parms->dm[idm].stroke->p[0],
parms->dm[idm].stroke->p[0]);
if(nclip>0){
info2("step %d DM %d: %d actuators clipped\n", simu->isim, idm, nclip);
}
}else if(parms->dm[idm].stroke->nx==nact){
if(parms->dm[idm].stroke->ny!=2){
error("dm.stroke is in wrong format\n");
}
double *pcmd=dmcmd->p[idm]->p;
double *plow=parms->dm[idm].stroke->p;
double *phigh=parms->dm[idm].stroke->p+nact;
for(int iact=0; iact<nact; iact++){
if(pcmd[iact]<plow[iact]){
pcmd[iact]=plow[iact];
}else if(pcmd[iact]>phigh[iact]){
pcmd[iact]=phigh[iact];
}
}
}else{
error("Invalid format\n");
}
}
}
if(parms->sim.dmclipia){
/*Clip interactuator stroke*/
for(int idm=0; idm<parms->ndm; idm++){
/* Embed DM commands to a square array (borrow dmrealsq) */
double iastroke;
int nx=simu->recon->anx->p[idm];
double (*dmr)[nx];
dmat *dm;
if(parms->dm[idm].iastrokescale){ //convert dm to voltage
dm=dinterp1(parms->dm[idm].iastrokescale->p[0], 0, dmcmd->p[idm], NAN);
iastroke=parms->dm[idm].iastroke;//voltage.
}else{
dm=dmcmd->p[idm];
iastroke=parms->dm[idm].iastroke*2;//surface to opd
}
if(!parms->fit.square){
loc_embed(simu->dmrealsq->p[idm], simu->recon->aloc->p[idm], dm->p);
dmr=(double(*)[nx])simu->dmrealsq->p[idm]->p;
}else{
dmr=(double(*)[nx])dm->p;
}
lcell *actstuck=simu->recon->actstuck;
long *stuck=actstuck?(actstuck->p[idm]?actstuck->p[idm]->p:0):0;
int count=0,trials=0;
do{
count=0;
PDMAT(simu->recon->amap->p[idm],map);
for(int iy=0; iy<simu->recon->any->p[idm]-1; iy++){
for(int ix=0; ix<nx; ix++){
int iact1=map[iy][ix];
int iact2=map[iy+1][ix];
if(iact1>0 && iact2>0){
count+=limit_diff(&dmr[iy][ix], &dmr[iy+1][ix], iastroke,
stuck?stuck[iact1-1]:0, stuck?stuck[iact2-1]:0);
}
}
}
for(int iy=0; iy<simu->recon->any->p[idm]; iy++){
for(int ix=0; ix<nx-1; ix++){
int iact1=map[iy][ix];
int iact2=map[iy][ix+1];
if(iact1>0 && iact2>0){
count+=limit_diff(&dmr[iy][ix], &dmr[iy][ix+1], iastroke,
stuck?stuck[iact1-1]:0, stuck?stuck[iact2-1]:0);
}
}
}
trials++;
if(trials==1 && count>0) {
info2("Step %d, DM %d: %d actuators over ia limit. ", simu->isim, idm, count);
}
}while(count>0 && trials<100);
if(trials>1){
info2("trials=%d: %s\n", trials, count?"failed.":"success.");
}
if(!parms->fit.square){//copy data back
loc_extract(simu->dmreal->p[idm], simu->recon->aloc->p[idm], simu->dmrealsq->p[idm]);
}
if(parms->dm[idm].iastrokescale){//convert back to opd
dmat *dm2=dinterp1(parms->dm[idm].iastrokescale->p[1], 0, dm, NAN);
dcp(&dmcmd->p[idm], dm2);
dfree(dm); dfree(dm2);
}
}
}
}
Int Interface::innerNormalDirection (Real & infeasible_gradient) {
Real oldnormc = c->norm();
Vector d(*env), dcp(*env), dn(*env);
Real ndn;
Real alpha, Ared, Pred;
Real one[2] = {1,0};
Real zero[2] = {0,0};
Int iout, naflag;
Bool dnavail;
Vector gtmp (*env);
Vector xtmp (*xc), ctmp (*c), stmp (*env);
Real normgtmp = 0;
Aavail = dciFalse;
Real scalingMatrix[nvar + nconI];
gtmp.sdmult (*J, 1, one, zero, ctmp); // g = J'*c
pReal gtmpx = gtmp.get_doublex();
for (Int i = 0; i < nvar+nconI; i++) {
Real gi = gtmpx[i], zi = xcx[i], ui = u_bndx[i], li = l_bndx[i];
if ( (gi < 0) && (ui < dciInf) ) {
scalingMatrix[i] = 1.0/sqrt(ui - zi);
} else if ( (gi > 0) && (li > -dciInf) ) {
scalingMatrix[i] = 1.0/sqrt(zi - li);
} else {
scalingMatrix[i] = 1;
}
}
normgtmp = gtmp.norm ();
Vector gtmp_proj(*env);
gtmp_proj.scale(gtmp, -1.0);
projectBounds_xc(gtmp_proj);
infeasible_gradient = gtmp_proj.norm();
// DeltaV = normgtmp;
if (normgtmp < dciTiny) {
normc = oldnormc;
iout = 6;
// std::cout << "iout = 6" << std::endl;
return iout;
}
//Now with the infinity norm
Real lower[nvar + nconI], upper[nvar + nconI];
for (Int i = 0; i < nvar+nconI; i++) {
Real zi = xcx[i], li = l_bndx[i], ui = u_bndx[i];
lower[i] = Max( -DeltaV,
(li > -dciInf ? (li - zi) * (1 - epsmu) : -dciInf) );
upper[i] = Min( DeltaV,
(ui < dciInf ? (ui - zi) * (1 - epsmu) : dciInf) );
}
Vector aux(*env);
d = gtmp;
pReal dx = 0;
gtmpx = gtmp.get_doublex();
dx = d.get_doublex();
for (Int i = 0; i < nvar + nconI; i++) {
gtmpx[i] /= scalingMatrix[i];
dx[i] = -dx[i]/pow(scalingMatrix[i], 2);
}
aux.sdmult(*J, 0, one, zero, d);
alpha = Min(gtmp.dot(gtmp)/aux.dot(aux), DeltaV/gtmp.norm());
dcp.scale (d, alpha);
pReal dcpx = dcp.get_doublex();
// alpha = -d.dot(gtmp)/aux.dot(aux);
// alpha = Min(alpha, DeltaV/d.norm());
alpha = 1.0;
for (int i = 0; i < nvar + nconI; i++) {
Real di = dcpx[i], ui = upper[i], li = lower[i];
if (di > dciEps) {
alpha = Min(alpha, ui/(di));
} else if (di < -dciEps) {
alpha = Min(alpha, li/(di));
} else
dcpx[i] = 0;
}
Real theta = 0.99995;
if (alpha < 1) {
alpha = Max(theta, 1 - dcp.norm())*alpha;
dcp.scale(alpha);
}
/* for (Int i = 0; i < nvar + nconI; i++)
* dcpx[i] *= scalingMatrix[i];
*/
dnavail = dciFalse;
ndn = 0;
Ared = 0;
Pred = 1;
// DeltaV = DeltaV/kappa2;
iout = 0;
// For the inequalities
pReal dnx = 0;
dnavail = dciFalse;
if (!dnavail) {
naflag = naStep (*c, dn);
dnavail = dciTrue;
dnx = dn.get_doublex();
//Project this step
alpha = 1.0;
for (Int i = 0; i < nvar + nconI; i++) {
Real di = dnx[i], ui = upper[i], li = lower[i];
if (di > dciEps) {
alpha = Min(alpha, ui/di);
} else if (di < -dciEps) {
alpha = Min(alpha, li/di);
} else
di = 0;
}
if (alpha < 1) {
alpha = Max(theta, 1 - dn.norm())*alpha;
dn.scale(alpha);
}
if (naflag > 1)
ndn = 0;
else
ndn = dn.norm (0);
assert(ndn <= DeltaV || "ndn > DeltaV");
}
/* ||a + b||^2 = <a+b,a+b> = <a,a> + 2*<a,b> + <b,b> */
/* m(d) = 0.5*||J*d + h||^2
* dtr = t*dn + (1 - t)*dcp
* m(dtr) = 0.5*||J*(t*dn + (1-t)*dcp) + h||^2
* = 0.5*||J*dcp + h + t*J*(dn - dcp)||^2
* = 0.5*||J*dcp + h||^2 + t*(J*dcp + h)'*J*(dn - dcp) + 0.5*t^2*||J*(dn - dcp)||^2 */
Vector Adcph(*c), difdcdn(dn), Adif(*env);
Adcph.sdmult(*J, 0, one, one, dcp);
difdcdn.saxpy(dcp, -1);
Adif.sdmult(*J, 0, one, zero, difdcdn);
Real objValAdcph = 0.5*Adcph.dot(Adcph);
Real dotAdcphAdif = Adcph.dot(Adif);
Real halfSqrNormAdif = 0.5*Adif.dot(Adif);
Real cauchyReduction = 0.5*oldnormc*oldnormc - objValAdcph;
Real newtonReduction = cauchyReduction - dotAdcphAdif - halfSqrNormAdif;
Real factor = 1.0;
while (newtonReduction/cauchyReduction < beta1) {
// Line search among from newton to cauchy
factor *= 0.9;
newtonReduction = cauchyReduction - factor*dotAdcphAdif - pow(factor,2)*halfSqrNormAdif;
if (factor < 1e-8) {
factor = 0;
break;
}
}
Vector xtemp(*xc);
for (Int i = 0; i < nvar+nconI; i++) {
if (l_bndx[i] - u_bndx[i] > -dciEps)
continue;
xcx[i] += (factor*dnx[i] + (1 - factor)*dcpx[i]);
if (xcx[i] >= u_bndx[i])
xcx[i] = u_bndx[i] - dciEps;
else if (xcx[i] <= l_bndx[i])
xcx[i] = l_bndx[i] + dciEps;
}
#ifndef NDEBUG
checkInfactibility();
#endif
call_ccfsg_xc(dciFalse);
normc = c->norm ();
Ared = 0.5*(oldnormc*oldnormc - normc*normc);
Pred = newtonReduction;
if (Ared/Pred < beta2) {
DeltaV /= 4;
*xc = xtmp;
call_ccfsg_xc(dciFalse);
normc = c->norm();
} else if (Ared/Pred > 0.75) {
DeltaV *= 2;
}
if (normc < rho)
return 0;
current_time = getTime() - start_time;
return 0;
}
/**
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 */
}
