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

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

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