source: Sophya/trunk/Poubelle/archTOI.old/starmatcher.cc@ 577

Last change on this file since 577 was 577, checked in by ansari, 26 years ago

SST
File size: 27.4 KB
Line 
1// starmatcher.cc
2// Eric Aubourg CEA/DAPNIA/SPP novembre 1999
3
4#include "starmatcher.h"
5#include "sststarfinder.h"
6#include "toimanager.h"
7#include "archexc.h"
8#include "archparam.h"
9#include "gondolageom.h"
10#include "polfitclip.h"
11
12#define STARDUMP
13
14extern "C" {
15#include "aa_hadec.h"
16#define NRANSI
17#include "nrutil.h"
18
19void lfit(double x[], double y[], double sig[], int ndat, double a[], int ia[],
20 int ma, double **covar, double *chisq, void (*funcs)(double, double [], int));
21
22void polfunc(double x, double afunc[], int ma);
23void sinfunc(double x, double afunc[], int ma);
24}
25
26void polfunc(double x, double afunc[], int ma) {
27 afunc[1] = 1;
28 for (int i=2; i<=ma; i++)
29 afunc[i] = afunc[i-1]*x;
30}
31
32void sinfunc(double x, double afunc[], int /*ma*/) {
33 afunc[1] = cos(x);
34 afunc[2] = sin(x);
35 afunc[3] = 1;
36}
37
38
39double polval(double x, double a[], int ma);
40
41double polval(double x, double a[], int ma) {
42 double r = a[ma];
43 for (int i=ma-1; i>0; i--) {
44 r = r*x+a[i];
45 }
46 return r;
47}
48
49#include <stdio.h>
50
51#ifdef __DECCXX
52#define SWAP
53#endif
54#if defined(Linux) || defined(linux)
55#define SWAP
56#endif
57
58typedef unsigned int4 uint_4;
59typedef unsigned short uint_2;
60
61static inline void bswap4(void* p)
62{
63 uint_4 tmp = *(uint_4*)p;
64 *(uint_4*)p = ((tmp >> 24) & 0x000000FF) |
65 ((tmp >> 8) & 0x0000FF00) |
66 ((tmp & 0x0000FF00) << 8) |
67 ((tmp & 0x000000FF) << 24);
68}
69
70static inline void bswap2(void* p)
71{
72 uint_2 tmp = *(uint_2*)p;
73 *(uint_2*)p = ((tmp >> 8) & 0x00FF) |
74 ((tmp & 0x00FF) << 8);
75}
76
77
78#define azimuthPendul "azimuthPendul"
79#define anglePendul "anglePendul"
80#define azimuthAxis "azimuthAxis"
81#define elvAxis "deltaZenith"
82#define alphaAxis "alphaZenith"
83#define deltaAxis "deltaZenith"
84#define azimuthFPC "azimuthFPC"
85#define elvFPC "elvFPC"
86#define alphaFPC "alphaFPC"
87#define deltaFPC "deltaFPC"
88#define azimuthBolo "azimuthBolo"
89#define elvBolo "elvBolo"
90#define alphaBolo "alphaBolo"
91#define deltaBolo "deltaBolo"
92#define azimuthSST "azimuthSST"
93#define elvSST "elvSST"
94#define alphaSST "alphaSST"
95#define deltaSST "deltaSST"
96
97
98StarMatcher::StarMatcher() {
99 possibleTOIs.insert(TOI(azimuthSST, TOI::unspec, "interp", "degrees","sstmatch"));
100 possibleTOIs.insert(TOI(elvSST, TOI::unspec, "interp", "degrees","sstmatch"));
101 possibleTOIs.insert(TOI(alphaSST, TOI::unspec, "interp", "hours","sstmatch"));
102 possibleTOIs.insert(TOI(deltaSST, TOI::unspec, "interp", "degrees","sstmatch"));
103 possibleTOIs.insert(TOI(azimuthAxis, TOI::unspec, "interp", "degrees","sstmatch"));
104 possibleTOIs.insert(TOI(elvAxis, TOI::unspec, "interp", "degrees","sstmatch"));
105 possibleTOIs.insert(TOI(alphaAxis, TOI::unspec, "interp", "hours","sstmatch"));
106 possibleTOIs.insert(TOI(deltaAxis, TOI::unspec, "interp", "degrees","sstmatch"));
107 possibleTOIs.insert(TOI(azimuthPendul, TOI::unspec, "interp", "degrees","sstmatch"));
108 possibleTOIs.insert(TOI(anglePendul, TOI::unspec, "interp", "degrees","sstmatch"));
109 possibleTOIs.insert(TOI(azimuthFPC, TOI::unspec, "interp", "degrees", "sstmatch"));
110 possibleTOIs.insert(TOI(elvFPC, TOI::unspec, "interp", "degrees", "sstmatch"));
111 possibleTOIs.insert(TOI(alphaFPC, TOI::unspec, "interp", "hours", "sstmatch"));
112 possibleTOIs.insert(TOI(deltaFPC, TOI::unspec, "interp", "degrees", "sstmatch"));
113 possibleTOIs.insert(TOI(azimuthBolo, TOI::all, "interp", "degrees", "sstmatch"));
114 possibleTOIs.insert(TOI(elvBolo, TOI::all, "interp", "degrees", "sstmatch"));
115 possibleTOIs.insert(TOI(alphaBolo, TOI::all, "interp", "hours", "sstmatch"));
116 possibleTOIs.insert(TOI(deltaBolo, TOI::all, "interp", "degrees", "sstmatch"));
117
118 FILE* f;
119
120 f = fopen("gsc7.dat","r");
121 if (!f) throw ArchExc("Error opening gsc7.dat");
122
123 int4 n4;
124 fread(&n4, sizeof(int4), 1 , f);
125
126#ifdef SWAP
127 bswap4(&n4);
128#endif
129 nstars = n4;
130
131 stars = new gscStar[nstars];
132 char* compdata = new char[10*nstars];
133 fread(compdata, 10, nstars, f);
134 fclose(f);
135
136 for (int i=0; i<nstars; i++) {
137#ifdef SWAP
138 ((char*)&(stars[i].ra))[0] = compdata[10*i+3];
139 ((char*)&(stars[i].ra))[1] = compdata[10*i+2];
140 ((char*)&(stars[i].ra))[2] = compdata[10*i+1];
141 ((char*)&(stars[i].ra))[3] = compdata[10*i+0];
142 ((char*)&(stars[i].dec))[0] = compdata[10*i+7];
143 ((char*)&(stars[i].dec))[1] = compdata[10*i+6];
144 ((char*)&(stars[i].dec))[2] = compdata[10*i+5];
145 ((char*)&(stars[i].dec))[3] = compdata[10*i+4];
146 ((char*)&(stars[i].mag))[0] = compdata[10*i+9];
147 ((char*)&(stars[i].mag))[1] = compdata[10*i+8];
148#else
149 ((char*)&(stars[i].ra))[0] = compdata[10*i+0];
150 ((char*)&(stars[i].ra))[1] = compdata[10*i+1];
151 ((char*)&(stars[i].ra))[2] = compdata[10*i+2];
152 ((char*)&(stars[i].ra))[3] = compdata[10*i+3];
153 ((char*)&(stars[i].dec))[0] = compdata[10*i+4];
154 ((char*)&(stars[i].dec))[1] = compdata[10*i+5];
155 ((char*)&(stars[i].dec))[2] = compdata[10*i+6];
156 ((char*)&(stars[i].dec))[3] = compdata[10*i+7];
157 ((char*)&(stars[i].mag))[0] = compdata[10*i+8];
158 ((char*)&(stars[i].mag))[1] = compdata[10*i+9];
159#endif
160 }
161
162 delete[] compdata;
163
164 TOIProducer* prod = TOIManager::findTOIProducer(TOI("sstStarCount"));
165 if (!prod) {
166 cerr << "StarMatcher : cannot find producer for sstStarCount" << endl;
167 exit(-1);
168 }
169
170 SSTStarFinder* sprod = dynamic_cast<SSTStarFinder*>(prod);
171 if (!sprod) {
172 cerr << "StarMatcher : producer for sstStarCount is not a SSTStarFinder" << endl;
173 exit(-1);
174 }
175
176 lastSeq = 0;
177
178 sprod->registerProcessor(this);
179
180}
181
182string StarMatcher::getName() {
183 return("StarMatcher 1.0");
184}
185
186#ifdef STARDUMP
187static ofstream starstream("stars.dat");
188static ofstream cstarstream("cstars.dat");
189static ofstream pendstream("pendul.dat");
190#endif
191static ofstream logstream("starmatch.log");
192
193void StarMatcher::dataFeed(SSTEtoile const& x) {
194 lastStars.push_back(x);
195}
196
197static long lastCleanSave=0;
198
199void nrerror(char * error_text) {
200 throw(string(error_text));
201}
202
203
204void StarMatcher::processStars() {
205
206 if (lastStars.empty()) return;
207
208 map<TOI, TOIProducer*> & m = (*neededTOIs.begin()).second;
209 while (!lastStars.empty()) {
210 SSTEtoile lastStar = lastStars.front();
211 lastStars.pop_front();
212
213 double lat, lon, ts, alpha, delta, az, rspeed;
214
215 long snstar = (long) lastStar.TEchan;
216
217 for (map<TOI, TOIProducer*>::iterator i = m.begin(); i != m.end(); i++) {
218 TOI const& inToi = (*i).first;
219 TOIProducer* prod = (*i).second;
220 if (inToi.name == "latitude") lat = prod->getValue(snstar, inToi);
221 if (inToi.name == "longitude") lon = prod->getValue(snstar, inToi);
222 if (inToi.name == "tsid") ts = prod->getValue(snstar, inToi);
223 if (inToi.name == "alphaSST") alpha = prod->getValue(snstar, inToi);
224 if (inToi.name == "deltaSST") delta = prod->getValue(snstar, inToi);
225 if (inToi.name == "azimuthSST") az = prod->getValue(snstar, inToi);
226 if (inToi.name == "rotSpeed") rspeed = prod->getValue(snstar, inToi);
227 }
228
229 // correct azimuth using fractional value of TEchan
230
231 az -= rspeed * archParam.acq.perEch * (lastStar.TEchan-snstar);
232
233 // find all stars +- 2 deg boresight
234 double dist = 2;
235 double dmin = delta - dist; if (dmin<-90) dmin=-90;
236 double dmax = delta + dist; if (dmax> 90) dmax= 90;
237 double amin = alpha - dist / cos(delta * 3.1415926/180) / 15.;
238 if (amin<0) amin += 24;
239 double amax = alpha + dist / cos(delta * 3.1415926/180) / 15.;
240 if (amax>24) amax -= 24;
241
242 int a,b,c;
243 a=0; c=nstars-1;
244 while (a+1<c) {
245 b = (a+c)/2;
246 if (stars[b].dec < dmin) a=b; else c=b;
247 }
248 int imin = a;
249 a=0; c=nstars;
250 while (a+1<c) {
251 b = (a+c)/2;
252 if (stars[b].dec < dmax) a=b; else c=b;
253 }
254 int imax = c;
255
256 for (int i=imin; i<=imax; i++) {
257 if (stars[i].ra >= amin && stars[i].ra <= amax) {
258 double ha = (ts/3600. - stars[i].ra) * 15. * 3.1415926/180.;
259 double elv, azim;
260 hadec_aa(lat * 3.1415926/180., ha, stars[i].dec * 3.1415926/180.,
261 &elv, &azim);
262 elv *= 180/3.1415926;
263 azim *= 180/3.1415926;
264 if (azim<0) azim += 360;
265
266 double da = azim-az; if (da>360) da -= 360;
267 if (da < -0.6 || da > 0.4) continue;
268 double elv0 = elv - 1.41/45. * lastStar.NoDiode;
269 if (fabs(elv0-GondolaGeom::elevSST0) > 0.25) continue; // Might be too strong
270
271#ifdef STARDUMP
272 starstream << setprecision(10) << lastStar.TEchan << " " <<
273 lastStar.NoDiode << " " <<
274 alpha << " " << delta << " " <<
275 az << " " <<
276 stars[i].ra << " " << stars[i].dec << " " <<
277 elv << " " << azim << " " <<
278 lastStar.InpCurrent << " " << stars[i].mag << "\n";
279#endif
280
281 matchStar s;
282 lastSeq++;
283 s.SN = lastStar.TEchan;
284 s.raGSC = stars[i].ra;
285 s.decGSC = stars[i].dec;
286 s.azGSC = azim;
287 s.elvGSC = elv;
288 s.nDiode = lastStar.NoDiode;
289 s.ok = true;
290 s.seq = lastSeq;
291 s.lon = lon;
292 s.lat = lat;
293 s.ts = ts;
294
295 matchStars.push_back(s);
296 }
297 }
298 }
299
300 // new set of matched stars... Clean, and get parameters...
301 // We don't want more than 30 seconds of data
302
303 if (matchStars.empty()) return;
304
305
306 double snEnd = matchStars.back().SN;
307 deque<matchStar>::iterator it;
308 for (it = matchStars.begin(); it!=matchStars.end(); it++) {
309 if ((snEnd - (*it).SN)*archParam.acq.perEch < 30 ||
310 (*it).seq > lastCleanSave)
311 break;
312 }
313 if (it != matchStars.begin()) {
314 it--;
315 }
316 if (it != matchStars.begin()) {
317 matchStars.erase(matchStars.begin(), it);
318 }
319
320 // we want to clean on the last 5 seconds of data.
321
322 int nskip=0;
323 for (it = matchStars.begin(); it!=matchStars.end(); it++) {
324 if ((snEnd - (*it).SN)*archParam.acq.perEch < 5)
325 break;
326 nskip++;
327 }
328
329 if (matchStars.size()-nskip < 30) return; // pas assez d'etoiles
330
331 // we remove "bursts" of stars, ie more than 4 stars in the same samplenum
332
333 long lastSN = 0;
334 deque<matchStar>::iterator lastIt = it;
335 long burstLen = 0;
336 for (deque<matchStar>::iterator it1 = it ; it1!=matchStars.end(); it1++) {
337 matchStar s = (*it1);
338 if ((long) s.SN == lastSN) {
339 burstLen++;
340 continue;
341 }
342 if (burstLen >= 4) {
343 for (deque<matchStar>::iterator it2=lastIt; it2 != it1; it2++) {
344 //if ((*it2).ok)
345 // logstream << " kill " << (*it2).seq << " " << setprecision(11) << (*it2).SN << " burst" << '\n';
346 (*it2).ok=false;
347 }
348 }
349 burstLen = 1;
350 lastIt = it1;
351 lastSN = s.SN;
352 }
353 // we fit the data to a polynomial, with clipping...
354
355 //double* sn = ::vector(1, matchStars.size());
356 double* elv0 = ::vector(1, matchStars.size());
357 double* azi = ::vector(1, matchStars.size());
358 double* sig = ::vector(1, matchStars.size());
359 //double* ae = ::vector(1, 3);
360 double* aa = ::vector(1, 3);
361 int* ia = ivector(1, 3);
362 double** cov = matrix(1, 3, 1, 3);
363 int ndata;
364
365 //long sn0 = matchStars.front().SN;
366 long sn0 = (*it).SN;
367 PolFitClip2 fitElvAz(matchStars.size(), 2); fitElvAz.setClip(0.1,0,2,3);
368 ndata = 0;
369
370 double oldAz = -1;
371 for (deque<matchStar>::iterator it1 = it ; it1!=matchStars.end(); it1++) {
372 matchStar s1 = (*it1);
373 if (!s1.ok) continue;
374 double az = s1.azGSC;
375 if (ndata > 0 && az - oldAz > 180) az -= 360;
376 if (ndata > 0 && az - oldAz < -180) az += 360;
377 fitElvAz.addData(s1.SN-sn0, s1.elvGSC - s1.nDiode*1.41/45., az);
378 oldAz = az;
379 ndata++;
380 }
381
382 double celv[3], caz[3];
383 if (fitElvAz.doFit(celv,caz)) return;
384 //if (fitElvAz.doFit()) return;
385
386 //logstream << "*** Fit sig=" << fitElvAz.getSigmaY() << " " << fitElvAz.getSigmaZ()
387 // << " n =" << fitElvAz.getNData() << " " << fitElvAz.getNDataUsed()
388 // << " SN :" << fitElvAz.getXMin() + sn0 << " - " << fitElvAz.getXMax() + sn0 << '\n';
389 //logstream << " sn0 = " << sn0 << "; snmin =" << fitElvAz.getXMin() + sn0 << "; snmax ="
390 // << fitElvAz.getXMax() + sn0 << '\n';
391 //logstream << " fitelv[x_] := " << celv[2] << " x^2 + " << celv[1] << " x + " << celv[0] << '\n';
392 //logstream << " fitaz[x_] := " << caz[2] << " x^2 + " << caz[1] << " x + " << caz[0] << '\n';
393
394 //if (fitElvAz.getSigmaY() > 0.05 || fitElvAz.getSigmaZ() > 0.05) return;
395 if (fitElvAz.getNDataUsed() < 5 ||
396 (double)fitElvAz.getNDataUsed()/fitElvAz.getNData() < .5) return;
397
398 double dcutElv = fitElvAz.getSigmaY()*3;
399 double dcutAz = fitElvAz.getSigmaZ()*3;
400
401 if (dcutElv < 0.05) dcutElv = 0.05;
402 if (dcutAz < 0.05) dcutAz = 0.05;
403
404 // don't kill borders of fit....
405 //if (matchStars.end() - it > 6)
406 // for (deque<matchStar>::iterator it1 = it+3 ; it1!=matchStars.end()-3; it1++) {
407 for (deque<matchStar>::iterator it1 = it ; it1!=matchStars.end(); it1++) {
408 matchStar sss = (*it1);
409 if (!sss.ok) continue;
410 if (fabs(fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.)) > dcutElv) {
411 (*it1).ok = false;
412 //logstream << " kill " << sss.seq << " " << setprecision(11) << sss.SN << " "
413 // << fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.) << '\n';
414 continue;
415 }
416 double daz = fitElvAz.valueZ(sss.SN-sn0) - sss.azGSC;
417 if (daz>=180) daz -= 360;
418 if (daz<-180) daz += 360;
419 if (fabs(daz) > dcutAz) (*it1).ok = false;
420 if (!(*it1).ok) {
421 //logstream << " kill " << sss.seq << " " << setprecision(11) << sss.SN << " "
422 // << fitElvAz.valueY(sss.SN-sn0)-(sss.elvGSC - sss.nDiode*1.41/45.) << " "
423 // << daz << '\n';
424 }
425 }
426
427 bool gotNewStars = false;
428 for (deque<matchStar>::iterator it1 = matchStars.begin() ; it1!=it; it1++) {
429 if ((*it1).ok && (*it1).seq > lastCleanSave) {
430 gotNewStars = true;
431 lastCleanSave = (*it1).seq;
432#ifdef STARDUMP
433 cstarstream << (*it1).seq << "\n";
434#endif
435 posInfo info;
436 info.SN = (*it1).SN;
437 info.azStar = (*it1).azGSC;
438 info.elvStar = (*it1).elvGSC;
439 info.diodStar= (*it1).nDiode;
440 info.lon = (*it1).lon;
441 info.lat = (*it1).lat;
442 info.ts = (*it1).ts;
443 posInfos[info.SN] = info;
444 }
445 }
446
447 if (!gotNewStars) return;
448
449 // On a des etoiles nettoyees, on va trouver amplitude et phase du
450 // signal en elevation, ce qui va nous donner les deux angles d'Euler
451 // de la pendulation (au premier ordre en theta)
452
453 // Il faut avoir une periode entiere ou pas loin, sinon on ne peut
454 // rien dire simplement.... -> we want to run on the last 18 seconds of
455 // data before the last fully cleaned star (it).
456
457 deque<matchStar>::iterator itstart;
458
459 for (itstart = matchStars.begin(); itstart != it; itstart++) {
460 if (((*it).SN - (*itstart).SN)*archParam.acq.perEch < 19)
461 break;
462 }
463
464 if (((*it).SN - (*itstart).SN)*archParam.acq.perEch < 15) return;
465
466
467 // it = matchStars.end(); it--;
468 // if (((*it).SN - matchStars.front().SN)*archParam.acq.perEch < 17) return;
469
470 // $CHECK$ utiliser plutot le SN moyen/median de tous les points effectivement utilises.
471 long snmid = ((*it).SN + (*itstart).SN)/2;
472
473 ndata=0;
474 double snmean = 0;
475
476 logstream << "PendFit : " << setprecision(11) << (*itstart).SN << '-' << (*it).SN << " "
477 << setprecision(4)
478 << ((*it).SN - (*itstart).SN)*archParam.acq.perEch << " " ;
479
480 for (deque<matchStar>::iterator it1 = itstart ; it1!=it; it1++) {
481 matchStar st = *it1;
482 if (!st.ok) continue;
483 ndata++;
484 snmean += st.SN;
485 azi[ndata] = st.azGSC * 3.1415926/180;
486 elv0[ndata] = st.elvGSC - st.nDiode*1.41/45.;
487 sig[ndata] = 0.01;
488 }
489 if (ndata) snmean /= ndata;
490
491 ia[1] = ia[2] = 1;
492 ia[3] = 0;
493 aa[3] = GondolaGeom::elevSST0;// do not fit elv0
494
495 if (ndata<5) return;
496 double chi2;
497 try {
498 lfit(azi, elv0, sig, ndata, aa, ia, 3, cov, &chi2, sinfunc);
499 } catch(string st) {
500 return;
501 }
502
503 double cc = aa[1];
504 double ss = aa[2];
505
506 logstream << setprecision(11) << snmean << setprecision(4)
507 << " cs=" << cc << " " << ss << " chi2r=" << chi2/ndata
508 << " cov " << cov[1][1] << " " << cov[2][2] << '\n';
509
510 // Get rid of bad fits. The cuts are rather ad hoc
511
512 //if (aa[3] < 39.64 || aa[3] > 39.68) return;
513 if (chi2/ndata > 9) return;
514 if (cov[1][1] > 0.0001) return;
515 if (cov[2][2] > 0.0001) return;
516
517 double ampl = sqrt(cc*cc+ss*ss);
518 double phase = atan2(cc,ss)/(3.1415926/180);
519
520
521 pendulInfo info;
522 info.SN = snmean;
523 info.azPendul = 180-phase;
524 if (info.azPendul > 360) info.azPendul -= 360;
525 if (info.azPendul < 0) info.azPendul += 360;
526 info.angPendul = ampl;
527 pendulInfos[info.SN] = info;
528
529#ifdef STARDUMP
530 pendstream << setprecision(11) << snmean << " "
531 << setprecision(4) << ampl << " " << info.azPendul << " " << ndata << " " << chi2/ndata << " "
532 << cov[1][1] << " " << cov[2][2] << '\n';
533#endif
534 /*
535 double snum = (matchStars.front().SN + matchStars.back().SN)/2-sn0;
536 if (snmin > snum || snmax < snum) return;
537 double elsst = polval(snum, ae, 3);
538 double azsst = polval(snum, aa, 3);
539
540 if (azsst > 360) azsst -= 360;
541 if (azsst < 0 ) azsst += 360;
542 */
543
544// for (set<TOI>::iterator i = producedTOIs.begin(); i!=producedTOIs.end(); i++) {
545// if ((*i).name == "azimuthSST") computedValue((*i), snum+sn0, azsst);
546// if ((*i).name == "elvSST") computedValue((*i), snum+sn0, elsst);
547// }
548
549 //free_vector(sn, 1, matchStars.size());
550 free_vector(elv0, 1, matchStars.size());
551 free_vector(azi, 1, matchStars.size());
552 free_vector(sig, 1, matchStars.size());
553 //free_vector(ae, 1, 3);
554 free_vector(aa, 1, 3);
555 free_ivector(ia, 1, matchStars.size());
556 free_matrix(cov, 1, 3, 1, 3);
557}
558
559
560// $CHECK$ do a polynomial fit with several points...
561int StarMatcher::getPendulInfo(double sampleNum, pendulInfo& info) {
562
563 PolFitClip2 fitPendul(30,2);
564
565 map<double, pendulInfo>::iterator i = pendulInfos.lower_bound(sampleNum);
566 if (i == pendulInfos.begin() && (*i).second.SN >= sampleNum) return -1;
567 if (i == pendulInfos.end() && (*i).second.SN <= sampleNum) return -1;
568
569 if ((*i).second.SN > sampleNum) i--;
570
571 int nn=0;
572 double aziprev=0, azicur=0, azi0=0;
573 for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.begin(); ii--) {
574 nn++;
575 pendulInfo inf1 = (*ii).second;
576 aziprev = azicur;
577 azicur = inf1.azPendul;
578 if (nn==1) azi0 = azicur;
579 if (nn>1 && azicur - aziprev > 180) azicur -= 360;
580 if (nn>1 && azicur - aziprev < -180) azicur += 360;
581 fitPendul.addData(inf1.SN, inf1.angPendul, azicur);
582 if (nn>=5) break;
583 }
584
585 azicur = azi0;
586 if (i != pendulInfos.end()) i++;
587 for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.end(); ii++) {
588 nn++;
589 pendulInfo inf1 = (*ii).second;
590 aziprev = azicur;
591 azicur = inf1.azPendul;
592 if (nn>1 && azicur - aziprev > 180) azicur -= 360;
593 if (nn>1 && azicur - aziprev < -180) azicur += 360;
594 fitPendul.addData(inf1.SN, inf1.angPendul, azicur);
595 if (nn>=10) break;
596 }
597
598 if (fitPendul.doFit()) return -1;
599
600 info.SN = sampleNum;
601 info.azPendul = fitPendul.valueZ(sampleNum);
602 if (info.azPendul > 360) info.azPendul -= 360;
603 if (info.azPendul < 0) info.azPendul += 360;
604 info.angPendul = fitPendul.valueY(sampleNum);
605 return 0;
606}
607
608#if 0
609
610int StarMatcher::getPendulInfo(double sampleNum, pendulInfo& info) {
611 static double* sn = ::vector(1, 100);
612 static double* aziP = ::vector(1, 100);
613 static double* ampP = ::vector(1, 100);
614 static double* sig = ::vector(1, 100);
615 static double* aAzi = ::vector(1, 3);
616 static double* aAmp = ::vector(1, 3);
617 static int* ia = ::ivector(1,3);
618 static double** cov = ::matrix(1,3,1,3);
619 int ndata = 0;
620 map<double, pendulInfo>::iterator i = pendulInfos.lower_bound(sampleNum);
621 if (i == pendulInfos.begin() && (*i).second.SN >= sampleNum) return -1;
622 if (i == pendulInfos.end() && (*i).second.SN <= sampleNum) return -1;
623
624 if ((*i).second.SN > sampleNum) i--;
625
626 int nn=0;
627 for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.begin(); ii--) {
628 nn++;
629 ndata++;
630 pendulInfo inf1 = (*ii).second;
631 sn[ndata] = inf1.SN;
632 ampP[ndata] = inf1.angPendul;
633 aziP[ndata] = inf1.azPendul;
634 int prev = ndata-1;
635 if (ndata>1 && aziP[ndata] - aziP[prev] > 180) aziP[ndata] -= 360;
636 if (ndata>1 && aziP[ndata] - aziP[prev] < -180) aziP[ndata] += 360;
637 sig[ndata] = 1;
638 if (nn>=50) break;
639 }
640
641 nn=0;
642 if (i != pendulInfos.end()) i++;
643 for (map<double, pendulInfo>::iterator ii=i; ii != pendulInfos.end(); ii++) {
644 nn++;
645 ndata++;
646 pendulInfo inf1 = (*ii).second;
647 sn[ndata] = inf1.SN;
648 ampP[ndata] = inf1.angPendul;
649 aziP[ndata] = inf1.azPendul;
650 int prev = ndata-1;
651 if (nn==1) prev=1;
652 if (ndata>1 && aziP[ndata] - aziP[prev] > 180) aziP[ndata] -= 360;
653 if (ndata>1 && aziP[ndata] - aziP[prev] < -180) aziP[ndata] += 360;
654 sig[ndata] = 1;
655 if (nn>=50) break;
656 }
657
658 if (ndata < 3) return -1;
659
660 ia[1] = ia[2] = ia[3] = 1;
661 double chi2;
662 try {
663 lfit(sn, aziP, sig, ndata, aAzi, ia, 3, cov, &chi2, polfunc);
664 lfit(sn, ampP, sig, ndata, aAmp, ia, 3, cov, &chi2, polfunc);
665 } catch(string st) {
666 return -1;
667 }
668
669 info.SN = sampleNum;
670 info.azPendul = polval(sampleNum, aAzi, 3);
671 if (info.azPendul > 360) info.azPendul -= 360;
672 if (info.azPendul < 0) info.azPendul += 360;
673 info.angPendul = polval(sampleNum, aAmp, 3);
674 return 0;
675}
676
677#endif
678
679#if 0
680
681int StarMatcher::getPendulInfo(double sampleNum, pendulInfo& info) {
682 map<double, pendulInfo>::iterator i = pendulInfos.lower_bound(sampleNum);
683 if (i == pendulInfos.begin() && (*i).second.SN >= sampleNum) return -1;
684 if (i == pendulInfos.end() && (*i).second.SN <= sampleNum) return -1;
685
686 if ((*i).second.SN > sampleNum) i--;
687 pendulInfo inf1 = (*i).second;
688 i++;
689 pendulInfo inf2 = (*i).second;
690
691 info.SN = sampleNum;
692 if (inf2.azPendul - inf1.azPendul > 180) inf2.azPendul -= 360;
693 if (inf2.azPendul - inf1.azPendul < -180) inf2.azPendul += 360;
694 info.azPendul = inf1.azPendul + (inf2.azPendul - inf1.azPendul) * (sampleNum - inf1.SN) / (inf2.SN - inf1.SN);
695 if (info.azPendul<0) info.azPendul += 360;
696 if (info.azPendul>360) info.azPendul += 360;
697 info.angPendul = inf1.angPendul + (inf2.angPendul - inf1.angPendul) * (sampleNum - inf1.SN) / (inf2.SN - inf1.SN);
698 return 0;
699}
700
701#endif
702
703
704double StarMatcher::getValue(long sampleNum, TOI const& toi) {
705 processStars();
706
707 // 1. Interpoler la valeur de pendulation
708 // 2. Interpoler la position en azimuth avec les etoiles encadrant
709
710 pendulInfo pendul;
711 int rc = getPendulInfo(sampleNum, pendul);
712 if (rc) return -99999;
713 if (toi.name == azimuthPendul) return pendul.azPendul;
714 if (toi.name == anglePendul) return pendul.angPendul;
715
716 // find nearest matched star
717 map<double, posInfo>::iterator i = posInfos.lower_bound(sampleNum);
718 if (i == posInfos.begin() && (*i).second.SN >= sampleNum) return -1;
719 if (i == posInfos.end() && (*i).second.SN <= sampleNum) return -1;
720 if ((*i).second.SN > sampleNum) i--;
721
722 // $CHECK$ if time spent here, can keep a GondolaGeom object for several
723 // samples...
724 GondolaGeom geom;
725 geom.setEarthPos((*i).second.lon,(*i).second.lat);
726 geom.setTSid((*i).second.ts);
727 geom.setPendulation(pendul.azPendul, pendul.angPendul);
728
729 int ns=0;
730 for (map<double, posInfo>::iterator it=i; it != posInfos.end(); it++) {
731 posInfo s = (*it).second;
732 double delsn = s.SN - sampleNum;
733 ns++;
734 //if (delsn * archParam.acq.perEch > 1 && ns > 4) break;
735 if (delsn * archParam.acq.perEch > 5) break;
736 geom.addStar(delsn, s.azStar, s.elvStar, s.diodStar);
737 }
738
739 if (i != posInfos.begin()) i--;
740 ns = 0;
741 for (map<double, posInfo>::iterator it=i; it != posInfos.begin(); it--) {
742 posInfo s = (*it).second;
743 double delsn = s.SN - sampleNum;
744 ns++;
745 //if (-delsn * archParam.acq.perEch > 1 && ns > 4) break;
746 if (-delsn * archParam.acq.perEch > 5) break;
747 geom.addStar(delsn, s.azStar, s.elvStar, s.diodStar);
748 }
749
750 if (geom.solveStars()) return -99999;
751
752 if (toi.name == azimuthAxis) return geom.getAzimutAxis();
753 if (toi.name == elvAxis) return geom.getElvAxis();
754 if (toi.name == alphaAxis) return geom.getAlphaAxis();
755 if (toi.name == deltaAxis) return geom.getDeltaAxis();
756
757 if (toi.name == azimuthSST) return geom.getAzimutSST();
758 if (toi.name == elvSST) return geom.getElvSST();
759 if (toi.name == alphaSST) return geom.getAlphaSST();
760 if (toi.name == deltaSST) return geom.getDeltaSST();
761
762 if (toi.name == azimuthFPC) return geom.getAzimutCenter();
763 if (toi.name == elvFPC) return geom.getElvCenter();
764 if (toi.name == alphaFPC) return geom.getAlphaCenter();
765 if (toi.name == deltaFPC) return geom.getDeltaCenter();
766
767 if (toi.name == azimuthBolo) return geom.getAzimutBolo(toi.index);
768 if (toi.name == elvBolo) return geom.getElvBolo(toi.index);
769 if (toi.name == alphaBolo) return geom.getAlphaBolo(toi.index);
770 if (toi.name == deltaBolo) return geom.getDeltaBolo(toi.index);
771
772 return -99999;
773}
774
775bool StarMatcher::canGetValue(long sampleNum, TOI const& /*toi*/) {
776 processStars();
777
778 map<double, pendulInfo>::iterator i = pendulInfos.begin();
779 if (i == pendulInfos.end()) return false;
780 if (sampleNum < (*i).second.SN) return false;
781 i = pendulInfos.end(); i--;
782 if (sampleNum > (*i).second.SN) return false;
783
784 return true;
785}
786
787bool StarMatcher::canGetValueLater(long sampleNum, TOI const& /*toi*/) {
788 processStars();
789
790 map<double, pendulInfo>::iterator i = pendulInfos.end();
791 if (i == pendulInfos.begin()) return true;
792 i--;
793 return (sampleNum+4000> (*i).second.SN);
794}
795
796
797
798set<TOI> StarMatcher::reqTOIFor(TOI const&) {
799 set<TOI> t;
800 t.insert(TOI("latitude", TOI::unspec, "interp"));
801 t.insert(TOI("longitude", TOI::unspec, "interp"));
802 t.insert(TOI("tsid", TOI::unspec));
803 t.insert(TOI("alphaSST", TOI::unspec, "galcross0"));
804 t.insert(TOI("deltaSST", TOI::unspec, "galcross0"));
805 t.insert(TOI("azimuthSST",TOI::unspec, "galcross0"));
806 t.insert(TOI("elvSST", TOI::unspec, "galcross0"));
807 t.insert(TOI("rotSpeed", TOI::unspec, "galcross0"));
808 return t;
809}
810
811void StarMatcher::propagateLowBound(TOI const& toi, long sampleNum) {
812 // we want to keep some past information to interpolate...
813 // keep 1000 sampleNums (easier than a number of posinfos...)
814
815 sampleNum -= 4000;
816
817 map<double, posInfo>::iterator i = posInfos.begin();
818 while (i != posInfos.end() && (*i).first < sampleNum) i++;
819 if (i != posInfos.begin()) {
820 i--;
821 posInfos.erase(posInfos.begin(), i);
822 }
823
824 map<double, pendulInfo>::iterator j = pendulInfos.begin();
825 while (j != pendulInfos.end() && (*j).first < sampleNum) j++;
826 if (j != pendulInfos.begin()) {
827 j--;
828 pendulInfos.erase(pendulInfos.begin(), j);
829 }
830
831 TOIDerivProducer::propagateLowBound(toi, sampleNum);
832}
833
834
835// 1. processStars seulement quand au moins 10 etoiles nouvelles
836// 2. Nettoyer avec fit parabolique sur les 5 dernieres seconde de donnees
837// 3. Garder periodeRotation secondes de donnees nettoyees
838// 4. TF ordre 0 sur ces donnees, amplitude et phase -> theta et phi pendulation,
839// elevationSST = elv-theta Sin[azimut-phi]
840// azimutSST = azimut+theta Cos[azimut-phi] Tan[elv] (+ OFFSET GALCROSS)
841// le signal le plus propre est l'elevation -> fit dessus, puis
842// correction azimut SST a partir seconde equation, sans utiliser azimut galcross
843
844
Note: See TracBrowser for help on using the repository browser.