Changeset 577 in Sophya for trunk/Poubelle/archTOI.old/starmatcher.cc

Timestamp:

Nov 16, 1999, 2:20:39 PM (26 years ago)

Author:

ansari

Message:

SST

File:

• trunk/Poubelle/archTOI.old/starmatcher.cc (modified) (17 diffs)

trunk/Poubelle/archTOI.old/starmatcher.cc

@@ -8 +8 @@
 #include "archparam.h"
 #include "gondolageom.h"
+#include "polfitclip.h"
+
+#define STARDUMP
 
 extern "C" {
@@ -14 +17 @@
 #include "nrutil.h"
 
-void lfit(float x[], float y[], float sig[], int ndat, float a[], int ia[],
-        int ma, float **covar, float *chisq, void (*funcs)(float, float [], int));
-
-void polfunc(float x, float afunc[], int ma);
-void sinfunc(float x, float afunc[], int ma);
-}
-
-void polfunc(float x, float afunc[], int ma) {
+void lfit(double x[], double y[], double sig[], int ndat, double a[], int ia[],
+        int ma, double **covar, double *chisq, void (*funcs)(double, double [], int));
+
+void polfunc(double x, double afunc[], int ma);
+void sinfunc(double x, double afunc[], int ma);
+}
+
+void polfunc(double x, double afunc[], int ma) {
   afunc[1] = 1;
   for (int i=2; i<=ma; i++)
@@ -27 +30 @@
 }
 
-void sinfunc(float x, float afunc[], int /*ma*/) {
+void sinfunc(double x, double afunc[], int /*ma*/) {
   afunc[1] = cos(x);
   afunc[2] = sin(x);
@@ -34 +37 @@
 
 
-float polval(float x, float a[], int ma);
-
-float polval(float x, float a[], int ma) {
-  float r = a[ma];
+double polval(double x, double a[], int ma);
+
+double polval(double x, double a[], int ma) {
+  double r = a[ma];
   for (int i=ma-1; i>0; i--) {
     r = r*x+a[i];
@@ -186 +189 @@
 static ofstream pendstream("pendul.dat");
 #endif
+static ofstream logstream("starmatch.log");
 
 void StarMatcher::dataFeed(SSTEtoile const& x) {
@@ -295 +299 @@
   
   // new set of matched stars... Clean, and get parameters...
-  // We don't want more than 20 seconds of data
+  // We don't want more than 30 seconds of data
   
   if (matchStars.empty()) return;
@@ -303 +307 @@
   deque<matchStar>::iterator it;
   for (it = matchStars.begin();  it!=matchStars.end(); it++) {
-     if ((snEnd - (*it).SN)*archParam.acq.perEch < 20) 
+     if ((snEnd - (*it).SN)*archParam.acq.perEch < 30 ||
+         (*it).seq > lastCleanSave) 
        break;
+  }
+  if (it != matchStars.begin()) {
+    it--;
   }
   if (it != matchStars.begin()) {
@@ -314 +322 @@
   int nskip=0;
   for (it = matchStars.begin();  it!=matchStars.end(); it++) {
-     if ((snEnd - (*it).SN)*archParam.acq.perEch < 7) 
+     if ((snEnd - (*it).SN)*archParam.acq.perEch < 5) 
        break;
      nskip++;
@@ -334 +342 @@
     if (burstLen >= 4) {
       for (deque<matchStar>::iterator it2=lastIt; it2 != it1; it2++) {
+        //if ((*it2).ok) 
+        //  logstream << "  kill " << (*it2).seq << " " << setprecision(11) << (*it2).SN << " burst" << '\n';
         (*it2).ok=false;
       }
@@ -343 +353 @@
   // we fit the data to a polynomial, with clipping...
   
-  float* sn   =  ::vector(1, matchStars.size());
-  float* elv0 =  ::vector(1, matchStars.size());
-  float* azi  =  ::vector(1, matchStars.size());
-  float* sig  =  ::vector(1, matchStars.size());
-  float* ae   =  ::vector(1, 3);
-  float* aa   =  ::vector(1, 3);
+  //double* sn   =  ::vector(1, matchStars.size());
+  double* elv0 =  ::vector(1, matchStars.size());
+  double* azi  =  ::vector(1, matchStars.size());
+  double* sig  =  ::vector(1, matchStars.size());
+  //double* ae   =  ::vector(1, 3);
+  double* aa   =  ::vector(1, 3);
   int*  ia    = ivector(1, 3);
-  float** cov =  matrix(1, 3, 1, 3);
+  double** cov =  matrix(1, 3, 1, 3);
   int ndata;
   
-  long sn0 = matchStars.front().SN;
-  long snmin;
-  long snmax;
-  for (int i=0; i<4; i++) {
-    ndata = 0;
-    snmin = 99999999;
-    snmax = -99999999;
-    for (deque<matchStar>::iterator it1 = it ;  it1!=matchStars.end(); it1++) {
-      matchStar s = (*it1);
-      if (!s.ok) continue;
-      double delv, daz;
-      if (i) {
-        delv = polval(s.SN-sn0, ae, 3)-(s.elvGSC - s.nDiode*1.41/45.);
-        daz  = polval(s.SN-sn0, aa, 3)- s.azGSC;
-        if (daz>=180) daz -= 360;
-        if (daz<-180) daz += 360;
+  //long sn0 = matchStars.front().SN;
+  long sn0 = (*it).SN;
+  PolFitClip2 fitElvAz(matchStars.size(), 2); fitElvAz.setClip(0.1,0,2,3);
+  ndata = 0;
+
+  double oldAz = -1;
+  for (deque<matchStar>::iterator it1 = it ;  it1!=matchStars.end(); it1++) {
+    matchStar s1 = (*it1);
+    if (!s1.ok) continue;
+    double az = s1.azGSC;
+    if (ndata > 0 && az - oldAz >  180) az -= 360;
+    if (ndata > 0 && az - oldAz < -180) az += 360;
+    fitElvAz.addData(s1.SN-sn0, s1.elvGSC - s1.nDiode*1.41/45., az);
+    oldAz = az;
+    ndata++;
+  }
+   
+  double celv[3], caz[3];
+  if (fitElvAz.doFit(celv,caz)) return;
+  //if (fitElvAz.doFit()) return;
+  
+  //logstream << "*** Fit sig=" << fitElvAz.getSigmaY() << " " << fitElvAz.getSigmaZ()
+  //          << " n  =" << fitElvAz.getNData() << " " << fitElvAz.getNDataUsed() 
+  //          << " SN :" << fitElvAz.getXMin() + sn0 << " - " << fitElvAz.getXMax() + sn0 << '\n';
+  //logstream << " sn0 = " << sn0 << "; snmin =" << fitElvAz.getXMin() + sn0 << "; snmax =" 
+  //          << fitElvAz.getXMax() + sn0  << '\n';
+  //logstream << " fitelv[x_] := " << celv[2] << " x^2 + " << celv[1] << " x + " << celv[0] << '\n';
+  //logstream << " fitaz[x_]  := " <<  caz[2] << " x^2 + " <<  caz[1] << " x + " <<  caz[0] << '\n';
+  
+  //if (fitElvAz.getSigmaY() > 0.05 || fitElvAz.getSigmaZ() > 0.05) return;
+  if (fitElvAz.getNDataUsed() < 5 ||
+      (double)fitElvAz.getNDataUsed()/fitElvAz.getNData() < .5) return;
+      
+  double dcutElv = fitElvAz.getSigmaY()*3;
+  double dcutAz  = fitElvAz.getSigmaZ()*3;
+  
+  if (dcutElv < 0.05) dcutElv = 0.05;
+  if (dcutAz  < 0.05) dcutAz  = 0.05;
+
+  // don't kill borders of fit....
+  //if (matchStars.end() - it > 6)
+  //  for (deque<matchStar>::iterator it1 = it+3 ;  it1!=matchStars.end()-3; it1++) {
+  for (deque<matchStar>::iterator it1 = it ;  it1!=matchStars.end(); it1++) {
+      matchStar sss = (*it1);
+      if (!sss.ok) continue;
+      if (fabs(fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.)) > dcutElv) {
+        (*it1).ok = false;
+        //logstream << "  kill " << sss.seq << " " << setprecision(11) << sss.SN << " "
+        //          << fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.) << '\n';
+        continue;
       }
-      double dcutelv=0.2;
-      double dcutaz =0.4;
-      if (i>=2) {
-         dcutelv = 0.05;
-         dcutaz  = 0.1;
+      double daz = fitElvAz.valueZ(sss.SN-sn0) - sss.azGSC;
+      if (daz>=180) daz -= 360;
+      if (daz<-180) daz += 360;
+      if (fabs(daz) > dcutAz) (*it1).ok = false;
+      if (!(*it1).ok) {
+        //logstream << "  kill " << sss.seq << " " << setprecision(11) << sss.SN << " "
+        //          << fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.) << " "
+        //          << daz << '\n';
       }
-      if (i>=3) {
-         dcutelv = 0.02;
-         dcutaz  = 0.03;
-      }
-      if (i == 0 || ((fabs(delv)<dcutelv && fabs(daz)<dcutaz) && i<3)) {
-          ndata++;
-          sn  [ndata] = s.SN-sn0;
-          elv0[ndata] = s.elvGSC - s.nDiode*1.41/45.;
-          azi [ndata] = s.azGSC; // $CHECK$  ou ajuster difference avec az galcross ?
-          if (ndata>1 && azi[ndata] - azi[ndata-1] > 180) azi[ndata] -= 360;
-          if (ndata>1 && azi[ndata] - azi[ndata-1] < -180) azi[ndata] += 360;
-          sig [ndata] = 0.01;
-          if (s.SN-sn0 > snmax) snmax = s.SN-sn0;
-          if (s.SN-sn0 < snmin) snmin = s.SN-sn0;
-      }
-      if ((fabs(delv)>=dcutelv || fabs(daz)>=dcutaz) && i==3) {
-          (*it1).ok = false; 
-      }
-    }
-    if (i==3) break;
-    if (ndata<5) return; // on ne peut rien faire
-    ia[1]   =  ia[2] = ia[3] = 1;
-    float chi2;
-    try{
-      lfit(sn, elv0, sig, ndata, ae, ia, 3, cov, &chi2, polfunc);
-      lfit(sn, azi,  sig, ndata, aa, ia, 3, cov, &chi2, polfunc);
-    } catch(string s) {
-      return;
-    }
-  }
-  
-  for (deque<matchStar>::iterator it1 = it ;  it1!=matchStars.end(); it1++) {
+  }
+    
+  bool gotNewStars = false;
+  for (deque<matchStar>::iterator it1 = matchStars.begin() ;  it1!=it; it1++) {
     if ((*it1).ok && (*it1).seq > lastCleanSave) {
+      gotNewStars = true;
       lastCleanSave = (*it1).seq;
 #ifdef STARDUMP
@@ -425 +445 @@
   }
   
+  if (!gotNewStars) return;
+  
   // On a des etoiles nettoyees, on va trouver amplitude et phase du
   // signal en elevation, ce qui va nous donner les deux angles d'Euler
@@ -430 +452 @@
   
   // Il faut avoir une periode entiere ou pas loin, sinon on ne peut
-  // rien dire simplement....
-  
-  it = matchStars.end(); it--;
-  if ((((*it).SN) - (*matchStars.begin()).SN)*archParam.acq.perEch < 17) return;
-  
-  long snmid = (((*it).SN) + (*matchStars.begin()).SN)/2;
+  // rien dire simplement.... -> we want to run on the last 18 seconds of
+  // data before the last fully cleaned star (it).
+  
+  deque<matchStar>::iterator itstart;
+  
+  for (itstart = matchStars.begin();  itstart != it; itstart++) {
+     if (((*it).SN - (*itstart).SN)*archParam.acq.perEch < 19) 
+       break;
+  }
+  
+  if (((*it).SN - (*itstart).SN)*archParam.acq.perEch < 15) return;
+
+  
+ // it = matchStars.end(); it--;
+ // if (((*it).SN - matchStars.front().SN)*archParam.acq.perEch < 17) return;
+  
+  // $CHECK$ utiliser plutot le SN moyen/median de tous les points effectivement utilises.
+  long snmid = ((*it).SN + (*itstart).SN)/2;
     
   ndata=0;  
-  
-  for (deque<matchStar>::iterator it1 = matchStars.begin() ;  it1!=matchStars.end(); it1++) {
-    if (!(*it1).ok) continue;
+  double snmean = 0;
+
+  logstream << "PendFit : " <<  setprecision(11) << (*itstart).SN << '-' << (*it).SN <<  " "
+            << setprecision(4)
+            << ((*it).SN - (*itstart).SN)*archParam.acq.perEch << " " ;
+              
+  for (deque<matchStar>::iterator it1 = itstart ;  it1!=it; it1++) {
+    matchStar st = *it1;
+    if (!st.ok) continue;
     ndata++;
-    azi[ndata]  = (*it1).azGSC * 3.1415926/180;
-    elv0[ndata] = (*it1).elvGSC - (*it1).nDiode*1.41/45.;
+    snmean += st.SN;
+    azi[ndata]  = st.azGSC * 3.1415926/180;
+    elv0[ndata] = st.elvGSC - st.nDiode*1.41/45.;
     sig[ndata]  = 0.01;
   }
-   ia[1]   =  ia[2] = 1;
-   ia[3] = 0;
-   aa[3] = GondolaGeom::elevSST0;// do not fit elv0
+  if (ndata) snmean /= ndata;
+  
+  ia[1]   =  ia[2] = 1;
+  ia[3] = 0;
+  aa[3] = GondolaGeom::elevSST0;// do not fit elv0
 
   if (ndata<5) return;
-  float chi2;
+  double chi2;
   try {
     lfit(azi, elv0,  sig, ndata, aa, ia, 3, cov, &chi2, sinfunc);
-  } catch(string s) {
+  } catch(string st) {
     return;
   }
 
-  double c = aa[1];
-  double s = aa[2];
+  double cc = aa[1];
+  double ss = aa[2];
+  
+  logstream << setprecision(11) << snmean << setprecision(4)
+            << " cs=" << cc << " " << ss << " chi2r=" << chi2/ndata 
+            << " cov " << cov[1][1] << " " << cov[2][2] << '\n';
   
   // Get rid of bad fits. The cuts are rather ad hoc
   
   //if (aa[3] < 39.64 || aa[3] > 39.68) return;
-  if (chi2/ndata > 4) return;
+  if (chi2/ndata > 9) return;
   if (cov[1][1] > 0.0001) return;
   if (cov[2][2] > 0.0001) return;
 
-  double ampl = sqrt(c*c+s*s);
-  double phase = atan2(c,s)/(3.1415926/180);
-  
-#ifdef STARDUMP
-  pendstream << snmid << " " << ampl << " " << phase << " " << ndata << " " << chi2/ndata <<  
-       " "  << cov[1][1] << " " << cov[2][2] << '\n';
-#endif
+  double ampl = sqrt(cc*cc+ss*ss);
+  double phase = atan2(cc,ss)/(3.1415926/180);
+  
   
   pendulInfo info;
-  info.SN          =   snmid;
-  info.azPendul    =   phase;
+  info.SN          =   snmean;
+  info.azPendul    =   180-phase;
+  if (info.azPendul > 360) info.azPendul -= 360;
+  if (info.azPendul <   0) info.azPendul += 360;
   info.angPendul   =   ampl;
   pendulInfos[info.SN] = info;
 
+#ifdef STARDUMP
+  pendstream << setprecision(11) << snmean << " " 
+             << setprecision(4) << ampl << " " << info.azPendul << " " << ndata << " " << chi2/ndata <<  " "
+             << cov[1][1] << " " << cov[2][2] << '\n';
+#endif
   /*
   double snum = (matchStars.front().SN + matchStars.back().SN)/2-sn0;
@@ -497 +547 @@
 //  }
   
-  free_vector(sn, 1, matchStars.size());
+  //free_vector(sn, 1, matchStars.size());
   free_vector(elv0, 1, matchStars.size());
   free_vector(azi, 1, matchStars.size());
   free_vector(sig, 1, matchStars.size());
-  free_vector(ae, 1, 3);
+  //free_vector(ae, 1, 3);
   free_vector(aa, 1, 3);
   free_ivector(ia, 1, matchStars.size());
   free_matrix(cov, 1, 3, 1, 3);
 }
+
+
+// $CHECK$ do a polynomial fit with several points...
+int StarMatcher::getPendulInfo(double sampleNum, pendulInfo& info) {
+
+  PolFitClip2 fitPendul(30,2);
+
+  map<double, pendulInfo>::iterator i = pendulInfos.lower_bound(sampleNum);
+  if (i == pendulInfos.begin() && (*i).second.SN >= sampleNum) return -1;
+  if (i == pendulInfos.end()   && (*i).second.SN <= sampleNum) return -1;
+  
+  if ((*i).second.SN > sampleNum) i--;
+
+  int nn=0;
+  double aziprev=0, azicur=0, azi0=0;
+  for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.begin(); ii--) {
+    nn++;
+    pendulInfo inf1 = (*ii).second;
+    aziprev = azicur;
+    azicur = inf1.azPendul;
+    if (nn==1) azi0 = azicur;
+    if (nn>1 && azicur - aziprev > 180)  azicur -= 360;
+    if (nn>1 && azicur - aziprev < -180) azicur += 360;
+    fitPendul.addData(inf1.SN, inf1.angPendul, azicur);
+    if (nn>=5) break;
+  }
+  
+  azicur = azi0;
+  if (i != pendulInfos.end()) i++;
+  for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.end(); ii++) {
+    nn++;
+    pendulInfo inf1 = (*ii).second;
+    aziprev = azicur;
+    azicur = inf1.azPendul;
+    if (nn>1 && azicur - aziprev > 180)  azicur -= 360;
+    if (nn>1 && azicur - aziprev < -180) azicur += 360;
+    fitPendul.addData(inf1.SN, inf1.angPendul, azicur);
+    if (nn>=10) break;
+  }
+  
+  if (fitPendul.doFit()) return -1;
+  
+  info.SN = sampleNum;
+  info.azPendul   = fitPendul.valueZ(sampleNum);
+  if (info.azPendul > 360) info.azPendul -= 360;
+  if (info.azPendul <   0) info.azPendul += 360;
+  info.angPendul  = fitPendul.valueY(sampleNum);
+  return 0;
+}
+
+#if 0
+
+int StarMatcher::getPendulInfo(double sampleNum, pendulInfo& info) {
+  static double* sn    = ::vector(1, 100);
+  static double* aziP  = ::vector(1, 100);
+  static double* ampP  = ::vector(1, 100);
+  static double* sig   = ::vector(1, 100);
+  static double* aAzi  = ::vector(1, 3);
+  static double* aAmp  = ::vector(1, 3);
+  static int*   ia    = ::ivector(1,3);
+  static double** cov  = ::matrix(1,3,1,3);
+  int ndata = 0;
+  map<double, pendulInfo>::iterator i = pendulInfos.lower_bound(sampleNum);
+  if (i == pendulInfos.begin() && (*i).second.SN >= sampleNum) return -1;
+  if (i == pendulInfos.end()   && (*i).second.SN <= sampleNum) return -1;
+  
+  if ((*i).second.SN > sampleNum) i--;
+
+  int nn=0;
+  for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.begin(); ii--) {
+    nn++;
+    ndata++;
+    pendulInfo inf1 = (*ii).second;
+    sn[ndata]   = inf1.SN;
+    ampP[ndata] = inf1.angPendul;
+    aziP[ndata] = inf1.azPendul;
+    int prev = ndata-1;
+    if (ndata>1 && aziP[ndata] - aziP[prev] > 180)  aziP[ndata] -= 360;
+    if (ndata>1 && aziP[ndata] - aziP[prev] < -180) aziP[ndata] += 360;
+    sig[ndata]  = 1;
+    if (nn>=50) break;
+  }
+  
+  nn=0;
+  if (i != pendulInfos.end()) i++;
+  for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.end(); ii++) {
+    nn++;
+    ndata++;
+    pendulInfo inf1 = (*ii).second;
+    sn[ndata]   = inf1.SN;
+    ampP[ndata] = inf1.angPendul;
+    aziP[ndata] = inf1.azPendul;
+    int prev = ndata-1;
+    if (nn==1) prev=1;
+    if (ndata>1 && aziP[ndata] - aziP[prev] > 180)  aziP[ndata] -= 360;
+    if (ndata>1 && aziP[ndata] - aziP[prev] < -180) aziP[ndata] += 360;
+    sig[ndata]  = 1;
+    if (nn>=50) break;
+  }
+  
+  if (ndata < 3) return -1;
+  
+  ia[1] = ia[2] = ia[3] = 1;
+  double chi2;
+  try {
+    lfit(sn, aziP,  sig, ndata, aAzi, ia, 3, cov, &chi2, polfunc);
+    lfit(sn, ampP,  sig, ndata, aAmp, ia, 3, cov, &chi2, polfunc);
+  } catch(string st) {
+    return -1;
+  }
+
+  info.SN = sampleNum;
+  info.azPendul   = polval(sampleNum, aAzi, 3);
+  if (info.azPendul > 360) info.azPendul -= 360;
+  if (info.azPendul <   0) info.azPendul += 360;
+  info.angPendul  = polval(sampleNum, aAmp, 3);
+  return 0;
+}
+
+#endif
+
+#if 0
 
 int StarMatcher::getPendulInfo(double sampleNum, pendulInfo& info) {
@@ -527 +699 @@
 }
 
+#endif
+
 
 double StarMatcher::getValue(long sampleNum, TOI const& toi) {
@@ -546 +720 @@
   if ((*i).second.SN > sampleNum) i--;
   
+  // $CHECK$ if time spent here, can keep a GondolaGeom object for several
+  // samples...
   GondolaGeom geom;
   geom.setEarthPos((*i).second.lon,(*i).second.lat);
   geom.setTSid((*i).second.ts);
-  
+  geom.setPendulation(pendul.azPendul, pendul.angPendul);
+  
+  int ns=0;
   for (map<double, posInfo>::iterator it=i; it != posInfos.end(); it++) {
     posInfo s = (*it).second;
     double delsn = s.SN - sampleNum;
-    if (delsn * archParam.acq.perEch > 1) break;
+    ns++;
+    //if (delsn * archParam.acq.perEch > 1 && ns > 4) break;
+    if (delsn * archParam.acq.perEch > 5) break;
     geom.addStar(delsn, s.azStar, s.elvStar, s.diodStar);
   }
   
   if (i != posInfos.begin()) i--;
+  ns = 0;
   for (map<double, posInfo>::iterator it=i; it != posInfos.begin(); it--) {
     posInfo s = (*it).second;
     double delsn = s.SN - sampleNum;
-    if (-delsn * archParam.acq.perEch > 1) break;
+    ns++;
+    //if (-delsn * archParam.acq.perEch > 1 && ns > 4) break;
+    if (-delsn * archParam.acq.perEch > 5) break;
     geom.addStar(delsn, s.azStar, s.elvStar, s.diodStar);
   }
 
-  geom.solveStars();
+  if (geom.solveStars()) return -99999;
   
   if (toi.name == azimuthAxis) return geom.getAzimutAxis();
@@ -608 +791 @@
   if (i == pendulInfos.begin()) return true;
   i--;
-  return (sampleNum > (*i).second.SN);
+  return (sampleNum+4000> (*i).second.SN);
 }
 
@@ -627 +810 @@
 
 void StarMatcher::propagateLowBound(TOI const& toi, long sampleNum) {
+ // we want to keep some past information to interpolate...
+ // keep 1000 sampleNums (easier than a number of posinfos...)
+ 
+ sampleNum -= 4000;
+
   map<double, posInfo>::iterator i = posInfos.begin();
   while (i != posInfos.end() && (*i).first < sampleNum) i++;