00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 #include "orbitbk.hh"
00049 #include "constants.hh"
00050 #include "refsystems.hh"
00051 #include "ioroutines.hh"
00052 #include "conversion.hh"
00053 #include "utilities.hh"
00054 #include "exceptions.hh"
00055
00056 #include <cstdio>
00057 #include <strstream>
00058 #include <iomanip>
00059
00060
00061
00062
00063 matrix<real8> splineinterpol(
00064 const matrix<real8> &time,
00065 const matrix<real8> &data);
00066 matrix<real8> polyfit(
00067 const matrix<real8> &time,
00068 const matrix<real8> &y,
00069 const int32 DEGREE);
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085 void orbit::initialize(const char* file)
00086 {
00087 TRACE_FUNCTION("orbit::initialize (BK 17-Jul-2000)")
00088 char dummyline[ONE27];
00089 char word[EIGHTY];
00090 bool foundsection = false;
00091 register int32 i;
00092
00093
00094
00095 ifstream infile(file, ios::in);
00096 bk_assert(infile,file,__FILE__,__LINE__);
00097 numberofpoints = 0;
00098
00099
00100 while (infile)
00101 {
00102 infile >> word;
00103 if (strcmp("NUMBER_OF_DATAPOINTS:",word))
00104 {
00105 infile.getline(dummyline,ONE27,'\n');
00106 }
00107 else
00108 {
00109 foundsection=true;
00110 infile >> numberofpoints;
00111 klo=0;
00112 khi=1;
00113 time.resize(numberofpoints,1);
00114 data_x.resize(numberofpoints,1);
00115 data_y.resize(numberofpoints,1);
00116 data_z.resize(numberofpoints,1);
00117
00118
00119
00120 for (i=0;i<numberofpoints;i++)
00121 {
00122 infile.getline(dummyline,ONE27,'\n');
00123
00124 infile >> time(i,0)
00125 >> data_x(i,0)
00126 >> data_y(i,0)
00127 >> data_z(i,0);
00128 }
00129 }
00130 }
00131
00132
00133 infile.close();
00134 if (!foundsection)
00135 {
00136 WARNING << "string: \"NUMBER_OF_DATAPOINTS:\" not found (i.e., orbit) in file: "
00137 << file << ".";
00138 WARNING.print();
00139 }
00140 INFO << numberofpoints << " datapoints (t,x,y,z) read from: \""
00141 << file << "\"";
00142 INFO.print();
00143
00144
00145
00146 DEBUG << "value of interp_method: " << interp_method;
00147 DEBUG.print();
00148 if (foundsection)
00149 {
00150
00151 for (i=0; i<numberofpoints-1; ++i)
00152 {
00153 const real8 h = time(i+1,0) - time(i,0);
00154 if (h < EPS)
00155 {
00156 PRINT_ERROR("orbit time axis: require distinct, time sorted data")
00157 throw(input_error);
00158 }
00159 }
00160
00161 if (interp_method==ORB_DEFAULT)
00162 {
00163 INFO.print("Setting default orbit interpolation method.");
00164
00165
00166
00167
00168 interp_method = (numberofpoints>6) ? 5 : numberofpoints-2;
00169
00170
00171
00172 if ((time(1,0)-time(0,0))>479.9 && (time(1,0)-time(0,0))<481.1)
00173 {
00174 WARNING.print("Assuming RADARSAT: use highest polyfit recommended.");
00175 interp_method = numberofpoints-1;
00176 }
00177 }
00178
00179 computecoefficients();
00180 PROGRESS.print("Orbit: interpolation coefficients computed.");
00181 }
00182 #ifdef __DEBUG
00183 showdata();
00184
00185 const real8 t_tmp = time(0,0);
00186 DEBUG.width(22);
00187 DEBUG.precision(20);
00188 DEBUG.rewind();
00189 DEBUG << "check: computing orbit for t between t0 and t1 = " << t_tmp;
00190 DEBUG.print();
00191 cn pos_tmp = getxyz(t_tmp);
00192 cn vel_tmp = getxyzdot(t_tmp);
00193 cn acc_tmp = getxyzddot(t_tmp);
00194 DEBUG << "interp. pos: " << pos_tmp.x << " " << pos_tmp.y << " " << pos_tmp.z;
00195 DEBUG.print();
00196 DEBUG << "interp. vel: " << vel_tmp.x << " " << vel_tmp.y << " " << vel_tmp.z;
00197 DEBUG.print();
00198 DEBUG << "interp. acc: " << acc_tmp.x << " " << acc_tmp.y << " " << acc_tmp.z;
00199 DEBUG.print();
00200 #endif
00201 }
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211 void orbit::computecoefficients()
00212 {
00213 TRACE_FUNCTION("orbit::computecoefficients (BK 18-Jul-2000)")
00214 DEBUG << "value of interp_method: " << interp_method;
00215 DEBUG.print();
00216 switch (interp_method)
00217 {
00218 case ORB_SPLINE:
00219 INFO.print("Computing coefficients for orbit spline interpolation");
00220 coef_x = splineinterpol(time,data_x);
00221 coef_y = splineinterpol(time,data_y);
00222 coef_z = splineinterpol(time,data_z);
00223 break;
00224 default:
00225 INFO << "Computing coefficients for orbit polyfit degree: "
00226 << interp_method;
00227 INFO.print();
00228 if (interp_method < 0)
00229 {
00230 PRINT_ERROR("degree of polynomial < 0")
00231 throw(input_error);
00232 }
00233 coef_x = polyfit(time,data_x,interp_method);
00234 coef_y = polyfit(time,data_y,interp_method);
00235 coef_z = polyfit(time,data_z,interp_method);
00236 }
00237 }
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252 void orbit::getklokhi(real8 t)
00253 {
00254 #ifdef __DEBUG
00255
00256 TRACE_FUNCTION("orbit::getinterval (BK 18-Jul-2000)")
00257 #endif
00258
00259
00260 if (time(klo,0) <= t && time(khi,0) >= t)
00261 return;
00262
00263
00264 int32 k;
00265 klo = 0;
00266 khi = numberofpoints - 1;
00267 while(khi-klo>1)
00268 {
00269 k = (khi+klo) >> 1;
00270 if (time(k,0) > t)
00271 khi = k;
00272 else
00273 klo = k;
00274 }
00275 }
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296 cn orbit::getxyz(
00297 real8 t)
00298 {
00299 #ifdef __DEBUG
00300 TRACE_FUNCTION("orbit::getxyz (BK 02-Jun-1999)")
00301 if (t < time(0,0) || t > time(numberofpoints-1,0))
00302 {
00303 WARNING << "interpolation at: " << t << " is outside interval time axis: ("
00304 << time(0,0) << ", " << time(numberofpoints-1,0) << ").";
00305 WARNING.print();
00306 }
00307 #endif
00308
00309 cn satpos;
00310 if (interp_method==ORB_SPLINE)
00311 {
00312
00313 getklokhi(t);
00314 const real8 h = time(khi,0) - time(klo,0);
00315 const real8 a = (time(khi,0) - t) / h;
00316 const real8 b = 1. - a;
00317
00318 satpos.x = a*data_x(klo,0)+b*data_x(khi,0) +
00319 (((sqr(a)*a-a)*coef_x(klo,0)+(sqr(b)*b-b)*coef_x(khi,0))*sqr(h))/6.;
00320 satpos.y = a*data_y(klo,0)+b*data_y(khi,0) +
00321 (((sqr(a)*a-a)*coef_y(klo,0)+(sqr(b)*b-b)*coef_y(khi,0))*sqr(h))/6.;
00322 satpos.z = a*data_z(klo,0)+b*data_z(khi,0) +
00323 (((sqr(a)*a-a)*coef_z(klo,0)+(sqr(b)*b-b)*coef_z(khi,0))*sqr(h))/6.;
00324 }
00325
00326
00327
00328
00329
00330
00331 else
00332 {
00333 const real8 t_tmp = (t-time(numberofpoints/2,0))/real8(10.0);
00334 satpos.x = polyval1d(t_tmp, coef_x);
00335 satpos.y = polyval1d(t_tmp, coef_y);
00336 satpos.z = polyval1d(t_tmp, coef_z);
00337 }
00338 return satpos;
00339 }
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356 cn orbit::getxyzdot(
00357 real8 t)
00358 {
00359 #ifdef __DEBUG
00360 TRACE_FUNCTION("orbit::getxyzddot (BK 06-Jul-2000)")
00361 if (t < time(0,0) || t > time(numberofpoints-1,0))
00362 {
00363 WARNING << "interpolation at: " << t << " is outside interval time axis: ("
00364 << time(0,0) << ", " << time(numberofpoints-1,0) << ").";
00365 WARNING.print();
00366 }
00367 #endif
00368
00369 cn satvel;
00370
00371 if (interp_method==ORB_SPLINE)
00372 {
00373
00374 getklokhi(t);
00375 const real8 h = time(khi,0) - time(klo,0);
00376 const real8 a = (time(khi,0) - t) / h;
00377 const real8 b = 1. - a;
00378
00379 satvel.x = ((data_x(khi,0)-data_x(klo,0)) / h) +
00380 (h * (((1-(3*sqr(a)))*coef_x(klo,0)) + (((3*sqr(b))-1)*coef_x(khi,0)))) / 6.;
00381 satvel.y = ((data_y(khi,0)-data_y(klo,0)) / h) +
00382 (h * (((1-(3*sqr(a)))*coef_y(klo,0)) + (((3*sqr(b))-1)*coef_y(khi,0)))) / 6.;
00383 satvel.z = ((data_z(khi,0)-data_z(klo,0)) / h) +
00384 (h * (((1-(3*sqr(a)))*coef_z(klo,0)) + (((3*sqr(b))-1)*coef_z(khi,0)))) / 6.;
00385 }
00386
00387 else
00388 {
00389 const int32 DEGREE = coef_x.lines()-1;
00390 satvel.x = coef_x(1,0);
00391 satvel.y = coef_y(1,0);
00392 satvel.z = coef_z(1,0);
00393 const real8 t_tmp = (t-time(numberofpoints/2,0))/real8(10.0);
00394 for (int32 i=2; i<=DEGREE; ++i)
00395 {
00396 real8 powt = real8(i)*pow(t_tmp,real8(i-1));
00397 satvel.x += coef_x(i,0)*powt;
00398 satvel.y += coef_y(i,0)*powt;
00399 satvel.z += coef_z(i,0)*powt;
00400 }
00401 satvel.x /= real8(10.0);
00402 satvel.y /= real8(10.0);
00403 satvel.z /= real8(10.0);
00404 }
00405 return satvel;
00406 }
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424 cn orbit::getxyzddot(
00425 real8 t)
00426 {
00427 #ifdef __DEBUG
00428 TRACE_FUNCTION("orbit::getxyzddot (BK 06-Jul-2000)")
00429 if (t < time(0,0) || t > time(numberofpoints-1,0))
00430 {
00431 WARNING << "interpolation at: " << t << " is outside interval time axis: ("
00432 << time(0,0) << ", " << time(numberofpoints-1,0) << ").";
00433 WARNING.print();
00434 }
00435 #endif
00436
00437 cn satacc;
00438
00439 if (interp_method==ORB_SPLINE)
00440 {
00441
00442 getklokhi(t);
00443 const real8 h = time(khi,0) - time(klo,0);
00444 const real8 a = (time(khi,0) - t) / h;
00445 const real8 b = 1. - a;
00446
00447
00448 satacc.x = a*coef_x(klo,0) + b*coef_x(khi,0);
00449 satacc.y = a*coef_y(klo,0) + b*coef_y(khi,0);
00450 satacc.z = a*coef_z(klo,0) + b*coef_z(khi,0);
00451 }
00452
00453 else
00454 {
00455
00456 const int32 DEGREE = coef_x.lines()-1;
00457 satacc.x = 0.0;
00458 satacc.y = 0.0;
00459 satacc.z = 0.0;
00460 const real8 t_tmp = (t-time(numberofpoints/2,0))/real8(10.0);
00461 for (int32 i=2; i<=DEGREE; ++i)
00462 {
00463 real8 powt = real8((i-1)*i)*pow(t_tmp,real8(i-2));
00464 satacc.x += coef_x(i,0)*powt;
00465 satacc.y += coef_y(i,0)*powt;
00466 satacc.z += coef_z(i,0)*powt;
00467 }
00468 satacc.x /= real8(100.0);
00469 satacc.y /= real8(100.0);
00470 satacc.z /= real8(100.0);
00471 }
00472 return satacc;
00473 }
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491 int32 lp2xyz(
00492 real8 line,
00493 real8 pixel,
00494 const input_ell &ell,
00495 const slcimage &image,
00496 orbit &orb,
00497 cn &returnpos,
00498 int32 MAXITER,
00499 real8 CRITERPOS)
00500 {
00501 TRACE_FUNCTION("lp2xyz (BK 04-Jan-1999)")
00502
00503
00504 const real8 aztime = image.line2ta(line);
00505 const real8 ratime = image.pix2tr(pixel);
00506
00507 const cn possat = orb.getxyz(aztime);
00508 const cn velsat = orb.getxyzdot(aztime);
00509
00510
00511 returnpos.x = image.approxcentreoriginal.x;
00512 returnpos.y = image.approxcentreoriginal.y;
00513 returnpos.z = image.approxcentreoriginal.z;
00514
00515
00516
00517 static matrix<real8> solxyz(3,1);
00518 static matrix<real8> equationset(3,1);
00519 static matrix<real8> partialsxyz(3,3);
00520
00521 register int32 iter;
00522 for (iter=0; iter<=MAXITER; ++iter)
00523 {
00524
00525
00526 const cn dsat_P = returnpos.min(possat);
00527 equationset(0,0) = -eq1_doppler(velsat, dsat_P);
00528 equationset(1,0) = -eq2_range(dsat_P, ratime);
00529 equationset(2,0) = -eq3_ellipsoid(returnpos,ell.a,ell.b);
00530 partialsxyz(0,0) = velsat.x;
00531 partialsxyz(0,1) = velsat.y;
00532 partialsxyz(0,2) = velsat.z;
00533 partialsxyz(1,0) = 2*dsat_P.x;
00534 partialsxyz(1,1) = 2*dsat_P.y;
00535 partialsxyz(1,2) = 2*dsat_P.z;
00536 partialsxyz(2,0) = (2*returnpos.x)/sqr(ell.a);
00537 partialsxyz(2,1) = (2*returnpos.y)/sqr(ell.a);
00538 partialsxyz(2,2) = (2*returnpos.z)/sqr(ell.b);
00539
00540
00541 solve33(solxyz, equationset,partialsxyz);
00542
00543
00544 returnpos.x += solxyz(0,0);
00545 returnpos.y += solxyz(1,0);
00546 returnpos.z += solxyz(2,0);
00547
00548
00549 if (abs(solxyz(0,0)) < CRITERPOS &&
00550 abs(solxyz(1,0)) < CRITERPOS &&
00551 abs(solxyz(2,0)) < CRITERPOS )
00552 break;
00553 }
00554
00555
00556 if (iter >= MAXITER)
00557 {
00558 WARNING << "line, pix -> x,y,z: maximum iterations (" << MAXITER << ") reached. "
00559 << "Criterium (m): "<< CRITERPOS
00560 << "dx,dy,dz=" << solxyz(0,0) << ", " << solxyz(1,0) << ", " << solxyz(2,0);
00561 WARNING.print();
00562 }
00563
00564
00565 return iter;
00566 }
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584 int32 xyz2orb(
00585 cn &possat,
00586 const slcimage &image,
00587 orbit &orb,
00588 const cn &pointonellips,
00589 int32 MAXITER,
00590 real8 CRITERTIM)
00591 {
00592 TRACE_FUNCTION("xyz2orb (BK 22-Sep-2000)");
00593
00594 real8 sol;
00595 register int32 iter;
00596 real8 tazi = image.line2ta(.5*(image.currentwindow.linehi-image.currentwindow.linelo));
00597 for(iter=0 ;iter<=MAXITER; ++iter)
00598 {
00599
00600 possat = orb.getxyz(tazi);
00601 const cn velsat = orb.getxyzdot(tazi);
00602 const cn accsat = orb.getxyzddot(tazi);
00603 const cn delta = pointonellips.min(possat);
00604
00605
00606 sol = -eq1_doppler(velsat, delta) /
00607 eq1_doppler_dt(delta, velsat, accsat);
00608 tazi += sol;
00609
00610
00611 if (abs(sol) < CRITERTIM)
00612 break;
00613 }
00614
00615
00616 if (iter >= MAXITER)
00617 {
00618 WARNING << "x,y,z -> line, pix: maximum iterations (" << MAXITER << ") reached. "
00619 << "Criterium (s):" << CRITERTIM
00620 << "dta (s)=" << sol;
00621 WARNING.print();
00622 }
00623
00624
00625
00626 possat = orb.getxyz(tazi);
00627 return iter;
00628 }
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650 int32 xyz2t(
00651 real8 &tazi,
00652 real8 &tran,
00653 const slcimage &image,
00654 orbit &orb,
00655 const cn &pos,
00656 int32 MAXITER,
00657 real8 CRITERTIM)
00658 {
00659 TRACE_FUNCTION("xyz2t (BK 04-Jan-1999)")
00660
00661
00662 tazi = image.line2ta(.5*(image.currentwindow.linehi-image.currentwindow.linelo));
00663
00664 real8 sol;
00665 register int32 iter;
00666 for(iter=0 ;iter<=MAXITER; ++iter)
00667 {
00668
00669 const cn possat = orb.getxyz(tazi);
00670 const cn velsat = orb.getxyzdot(tazi);
00671 const cn accsat = orb.getxyzddot(tazi);
00672 const cn delta = pos.min(possat);
00673
00674
00675 sol = -eq1_doppler(velsat, delta) /
00676 eq1_doppler_dt(delta, velsat, accsat);
00677 tazi += sol;
00678
00679
00680 if (abs(sol) < CRITERTIM)
00681 break;
00682 }
00683
00684
00685 if (iter >= MAXITER)
00686 {
00687 WARNING << "x,y,z -> line, pix: maximum iterations (" << MAXITER << ") reached. "
00688 << "Criterium (s):" << CRITERTIM
00689 << "dta (s)=" << sol;
00690 WARNING.print();
00691 }
00692
00693
00694
00695 const cn possat = orb.getxyz(tazi);
00696 const cn delta = pos.min(possat);
00697 tran = delta.norm() / SOL;
00698
00699 return iter;
00700 }
00701
00702
00703
00704
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718 int32 xyz2lp(
00719 real8 &returnline,
00720 real8 &returnpixel,
00721 const slcimage &image,
00722 orbit &orb,
00723 const cn &pos,
00724 int32 MAXITER,
00725 real8 CRITERTIM)
00726 {
00727 TRACE_FUNCTION("xyz2lp (BK 04-Jan-1999)");
00728 real8 tazi;
00729 real8 tran;
00730
00731
00732 int32 iter = xyz2t(tazi,tran,image,orb,pos,MAXITER,CRITERTIM);
00733
00734
00735
00736 returnline = image.ta2line(tazi);
00737 returnpixel = image.tr2pix (tran);
00738
00739
00740 return iter;
00741 }
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760 int32 ell2lp(
00761 real8 &returnline,
00762 real8 &returnpixel,
00763 const input_ell &ell,
00764 const slcimage &image,
00765 orbit &orb,
00766 real8 phi,
00767 real8 lambda,
00768 real8 height,
00769 int32 MAXITER,
00770 real8 CRITERTIM)
00771 {
00772 TRACE_FUNCTION("ell2lp (BK 27-Jan-1999)")
00773
00774 cn returnpos;
00775 ell2xyz(ell, returnpos, phi, lambda, height);
00776 DEBUG << "tmp result phi,lambda,h --> x,y,z: "
00777 << phi << ", " << lambda << ", " << height << " --> "
00778 << returnpos.x << " " << returnpos.y << " " << returnpos.z;
00779 DEBUG.print();
00780
00781 int32 n_iter = xyz2lp(returnline, returnpixel,
00782 image, orb, returnpos,
00783 MAXITER, CRITERTIM);
00784 return n_iter;
00785 }
00786
00787
00788
00789
00790
00791
00792
00793
00794
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804 int32 lp2ell(
00805 real8 line,
00806 real8 pixel,
00807 const input_ell &ell,
00808 const slcimage &image,
00809 orbit &orb,
00810 real8 &returnphi,
00811 real8 &returnlambda,
00812 real8 &returnheight,
00813 int32 MAXITER,
00814 real8 CRITERPOS)
00815 {
00816 TRACE_FUNCTION("lp2ell (BK: 27-Jan-1999)");
00817
00818
00819 cn returnpos;
00820 int32 iter = lp2xyz(line, pixel, ell, image, orb,
00821 returnpos, MAXITER, CRITERPOS);
00822
00823
00824 xyz2ell(ell, returnpos, returnphi, returnlambda, returnheight);
00825
00826 return iter;
00827 }
00828
00829
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839
00840
00841
00842
00843
00844
00845
00846
00847
00848
00849
00850
00851
00852
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878
00879
00880
00881
00882
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906
00907
00908
00909
00910
00911
00912
00913
00914
00915
00916
00917
00918
00919
00920
00921
00922
00923
00924
00925
00926
00927
00928
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
00946
00947
00948
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972
00973
00974
00975
00976
00977
00978
00979
00980
00981
00982
00983
00984
00985
00986
00987
00988
00989
00990
00991
00992
00993
00994
00995
00996
00997
00998
00999
01000
01001
01002
01003
01004
01005
01006
01007
01008
01009
01010
01011
01012
01013
01014
01015
01016
01017
01018
01019
01020
01021
01022
01023
01024
01025
01026
01027
01028
01029
01030
01031
01032
01033
01034
01035
01036
01037
01038
01039
01040
01041
01042
01043
01044
01045
01046
01047
01048
01049
01050
01051
01052
01053
01054
01055
01056
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066
01067
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081
01082
01083
01084
01085
01086
01087
01088
01089
01090
01091
01092
01093
01094
01095
01096
01097
01098
01099
01100
01101
01102
01103
01104
01105
01106
01107
01108
01109
01110
01111
01112
01113
01114
01115
01116
01117
01118
01119
01120
01121
01122
01123
01124
01125
01126
01127
01128
01129
01130
01131
01132
01133
01134
01135
01136
01137
01138
01139
01140
01141
01142
01143
01144
01145 void orbit::dumporbit(
01146 const input_pr_orbits &inputorb,
01147 const int16 ID)
01148 {
01149 TRACE_FUNCTION("orbit::dumporbit (BK 03-Jul-2000)")
01150
01151 if (numberofpoints==0)
01152 {
01153 INFO.print("Exiting dumporbit, no orbit data available.");
01154 return;
01155 }
01156
01157
01158 real8 dt=0.;
01159 char ofile[EIGHTY];
01160 if (ID==MASTERID)
01161 {
01162 if (inputorb.dumpmasterorbit<0) return;
01163 dt = inputorb.dumpmasterorbit;
01164 strcpy(ofile,"masterorbit.dat");
01165 PROGRESS.print("Dumping master orbit.");
01166 }
01167 else if (ID==SLAVEID)
01168 {
01169 dt = inputorb.dumpslaveorbit;
01170 if (inputorb.dumpslaveorbit<0) return;
01171 strcpy(ofile,"slaveorbit.dat");
01172 PROGRESS.print("Dumping slave orbit.");
01173 }
01174 else
01175 {
01176 PRINT_ERROR("Panic: not possible.")
01177 throw(unhandled_case_error);
01178 }
01179 const int32 MAXITER = 10;
01180 const real8 CRITERPOS = 1e-6;
01181 const real8 CRITERTIM = 1e-10;
01182 INFO << "dumporbits: MAXITER: " << MAXITER << "; "
01183 << "CRITERPOS: " << CRITERPOS << " m; "
01184 << "CRITERTIM: " << CRITERTIM << " s";
01185 INFO.print();
01186
01187
01188 int32 outputlines = 1 + int32((time(numberofpoints-1,0)-time(0,0)) / dt);
01189 real8 tazi = time(0,0);
01190 ofstream fo(ofile,ios::out | ios::trunc);
01191 matassert(fo,ofile,__FILE__,__LINE__);
01192 fo.precision(3);
01193 fo.width(11);
01194
01195 fo.setf(ios::fixed);
01196 fo.setf(ios::showpoint);
01197 for (register int32 i=0; i<outputlines; ++i)
01198 {
01199 const cn position = getxyz(tazi);
01200 const cn velocity = getxyzdot(tazi);
01201 const cn accerela = getxyzddot(tazi);
01202 fo << tazi << " "
01203 << position.x << " " << position.y << " " << position.z << " "
01204 << velocity.x << " " << velocity.y << " " << velocity.z << " "
01205 << accerela.x << " " << accerela.y << " " << accerela.z << "\n";
01206 tazi += dt;
01207 }
01208 fo.close();
01209
01210
01211
01212 #ifdef __DEBUG
01213 if (ID==MASTERID)
01214 {
01215 DEBUG.print("dumping files m_t, m_x, m_y, m_z, m_cx, m_cy, m_cz for spline interpolation.");
01216 dumpasc("m_t",time);
01217 dumpasc("m_x",data_x); dumpasc("m_y",data_y); dumpasc("m_z",data_z);
01218 dumpasc("m_cx",coef_x); dumpasc("m_cy",coef_y); dumpasc("m_cz",coef_z);
01219 }
01220 else
01221 {
01222 DEBUG.print("dumping files s_t, s_x, s_y, s_z, s_cx, s_cy, s_cz for spline interpolation.");
01223 dumpasc("s_t",time);
01224 dumpasc("s_x",data_x); dumpasc("s_y",data_y); dumpasc("s_z",data_z);
01225 dumpasc("s_cx",coef_x); dumpasc("s_cy",coef_y); dumpasc("s_cz",coef_z);
01226 }
01227 #endif
01228 }
01229
01230
01231
01232
01233
01234
01235
01236
01237
01238
01239
01240
01241
01242
01243
01244
01245
01246
01247
01248
01249
01250
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260
01261 matrix<real8> splineinterpol(
01262 const matrix<real8> &time,
01263 const matrix<real8> &data)
01264 {
01265 TRACE_FUNCTION("splineinterpol (BK 17-Jul-2000)")
01266 const int32 N = time.lines();
01267 if (time.pixels() != 1 || data.pixels() != 1)
01268 {
01269 PRINT_ERROR("code 901: splineinterpol: wrong input.");
01270 throw(input_error);
01271 }
01272 if (N != data.lines())
01273 {
01274 PRINT_ERROR("code 901: splineinterpol: require same size vectors.");
01275 throw(input_error);
01276 }
01277
01278
01279 matrix<real8> rhs(N-1,1);
01280 matrix<real8> pp(N,1);
01281
01282
01283 #define __NATURALSPLINE__
01284
01285 #ifdef __NATURALSPLINE__
01286 pp(0,0) = 0.;
01287 #else
01288
01289
01290 const real8 yp1 = (data(1,0)-data(0,0)) / (time(1,0)-time(0,0));
01291 pp(0,0) = -0.5;
01292 rhs(0,0) = (3. / (time(1,0)-time(0,0))) *
01293 ((data(1,0)-data(0,0)) / (time(1,0)-time(0,0)) - yp1);
01294 #endif
01295
01296
01297 register int32 i;
01298 for (i=1; i<=N-2; ++i)
01299 {
01300 const int32 ip1 = i + 1;
01301 const int32 im1 = i - 1;
01302 const real8 sig = (time(i,0)-time(im1,0)) / (time(ip1,0)-time(im1,0));
01303 const real8 p = sig*pp(im1,0) + 2.;
01304
01305 pp(i,0) = (sig-1.) / p;
01306 rhs(i,0) = (data(ip1,0)-data(i,0)) / (time(ip1,0)-time(i,0))
01307 - (data(i,0)-data(im1,0)) / (time(i,0)-time(im1,0));
01308 rhs(i,0) = (6.*rhs(i,0)/(time(ip1,0)-time(im1,0))-sig*rhs(im1,0))/p;
01309 }
01310
01311
01312 #ifdef __NATURALSPLINE__
01313 pp(N-1,0) = 0.;
01314 #else
01315
01316 const real8 ypN = (data(N-1,0)-data(N-2,0)) / (time(N-1,0)-time(N-2,0));
01317 const real8 qn = 0.5;
01318 const real8 un = (3. / (time(N-1,0) - time(N-2,0))) *
01319 (ypN - (data(N-1,0) - data(N-2,0)) / (time(N-1,0) - time(N-2,0)));
01320 pp(N-1,0) = (un - qn*rhs(N-2,0)) / (qn*pp(N-2,0) + 1.);
01321 #endif
01322
01323
01324 for (i=N-2; i>=0; --i)
01325 pp(i,0) = pp(i,0)*pp(i+1,0) + rhs(i,0);
01326
01327
01328 return pp;
01329 }
01330
01331
01332
01333
01334
01335
01336
01337
01338
01339
01340
01341
01342
01343
01344
01345
01346
01347
01348
01349 matrix<real8> polyfit(
01350 const matrix<real8> &time,
01351 const matrix<real8> &y,
01352 const int32 DEGREE)
01353 {
01354 TRACE_FUNCTION("polyfit (BK 16-Jun-2003)")
01355 const int32 Npoints = time.lines();
01356 if (time.pixels() != 1 || y.pixels() != 1)
01357 {
01358 PRINT_ERROR("code 902: polyfit: wrong input.");
01359 throw(input_error);
01360 }
01361 if (Npoints != y.lines())
01362 {
01363 PRINT_ERROR("code 902: polyfit: require same size vectors.");
01364 throw(input_error);
01365 }
01366
01367
01368 DEBUG.print("normalizing t axis for least squares fit");
01369 matrix<real8> t=(time-time(Npoints/2,0))/real8(10.0);
01370
01371
01372 int32 Nunk = DEGREE+1;
01373 DEBUG << "Degree of orbit interpolating polynomial: " << DEGREE;
01374 DEBUG.print();
01375 DEBUG << "Number of unknowns: " << Nunk;
01376 DEBUG.print();
01377 DEBUG << "Number of data points (orbit): " << Npoints;
01378 DEBUG.print();
01379 if (Npoints < Nunk)
01380 {
01381 PRINT_ERROR("polyfit: Number of points is smaller than parameters solved for.")
01382 throw(input_error);
01383 }
01384
01385
01386 DEBUG.print("Setting up lin. system of equations");
01387 matrix<real8> A(Npoints,Nunk);
01388 int32 i,j;
01389 for (j=0; j<=DEGREE; j++)
01390 {
01391 matrix<real8> t_tmp = t;
01392 t_tmp.mypow(real8(j));
01393 A.setcolumn(j,t_tmp);
01394 }
01395 DEBUG.print("Solving lin. system of equations with cholesky.");
01396 matrix<real8> N = matTxmat(A,A);
01397 matrix<real8> rhs = matTxmat(A,y);
01398 matrix<real8> Qx_hat = N;
01399 choles(Qx_hat);
01400 solvechol(Qx_hat,rhs);
01401 invertchol(Qx_hat);
01402
01403 for (i=0; i<Qx_hat.lines(); i++)
01404 for (j=0; j<i; j++)
01405 Qx_hat(j,i) = Qx_hat(i,j);
01406 const real8 maxdev = max(abs(N*Qx_hat-eye(real8(Qx_hat.lines()))));
01407 DEBUG << "polyfit orbit: max(abs(N*inv(N)-I)) = " << maxdev;
01408 DEBUG.print();
01409
01410
01411
01412
01413
01414
01415
01416
01417
01418
01419
01420
01421
01422
01423
01424
01425
01426 if (maxdev > 1e-6) WARNING.print("polyfit orbit interpolation instable!");
01427 matrix<real8> y_hat = A * rhs;
01428 matrix<real8> e_hat = y - y_hat;
01429 if (max(abs(e_hat)) > 0.02)
01430 {
01431 WARNING << "Max. approximation error at datapoints (x,y,or z?): " << max(abs(e_hat)) << "m";
01432 WARNING.print();
01433 }
01434 else
01435 {
01436 INFO << "Max. approximation error at datapoints (x,y,or z?): " << max(abs(e_hat)) << "m";
01437 INFO.print();
01438 }
01439
01440 DEBUG.width(16);
01441 DEBUG.precision(15);
01442 DEBUG.print("REPORTING POLYFIT LEAST SQUARES ERRORS");
01443 DEBUG.print(" time y yhat ehat");
01444 for (i=0; i<Npoints; i++)
01445 {
01446
01447 DEBUG << setw(16) << setprecision(15) << time(i,0) << " "
01448 << y(i,0) << " " << y_hat(i,0) << " " << e_hat(i,0);
01449 DEBUG.print();
01450 }
01451
01452 for (i=0; i<Npoints-1; i++)
01453 {
01454
01455
01456 real8 dt = time(i+1,0)-time(i,0);
01457 DEBUG << "dt between point " << i+1 << " and " << i << "= " << dt;
01458 DEBUG.print();
01459 if(abs(dt-(time(1,0)-time(0,0))) > 0.001)
01460 WARNING.print("Orbit: data does not have equidistant time interval?");
01461 }
01462 DEBUG.reset();
01463
01464
01465 return rhs;
01466 }
01467
01468
01469
01470
01471
01472
01473
01474 void orbit::showdata()
01475 {
01476 TRACE_FUNCTION("orbit::showdata (BK 17-Jul-2000)")
01477 cout << "showdata orbit:\n";
01478 cout << "Time:\n";
01479 time.showdata();
01480 cout << "Datax:\n";
01481 data_x.showdata();
01482 cout << "Datay:\n";
01483 data_y.showdata();
01484 cout << "Dataz:\n";
01485 data_z.showdata();
01486 cout << "Coefx:\n";
01487 coef_x.showdata();
01488 cout << "Coefy:\n";
01489 coef_y.showdata();
01490 cout << "Coefz:\n";
01491 coef_z.showdata();
01492 }
01493
01494
01495 #ifdef __TESTMAIN__
01496
01497
01498
01499
01500 int32 main()
01501 {
01502 orbit masterorbit;
01503 orbit slaveorbit;
01504
01505
01506
01507 masterorbit.initialize("master.out");
01508 cerr << "\nShowing master orbit: "
01509 << masterorbit.npoints() << " points.\n";
01510 masterorbit.showdata();
01511
01512
01513
01514 slaveorbit.initialize("slave.out");
01515 cerr << "\nShowing slave orbit: "
01516 << slaveorbit.npoints() << " points.\n";
01517 slaveorbit.showdata();
01518
01519
01520 real8 t = 38002.341;
01521 cn pos = masterorbit.getxyz(t);
01522 cn vel = masterorbit.getxyzdot(t);
01523 cn acc = masterorbit.getxyzddot(t);
01524 cerr << "pos(" << t << "): " << pos.x << ", " << pos.y << ", " << pos.z << endl;
01525 cerr << "vel(" << t << "): " << vel.x << ", " << vel.y << ", " << vel.z << endl;
01526 cerr << "acc(" << t << "): " << acc.x << ", " << acc.y << ", " << acc.z << endl;
01527
01528 input_pr_orbits orbitinput;
01529 masterorbit.dumporbit(orbitinput,MASTERID);
01530
01531 slcimage masterinfo;
01532 slcimage slaveinfo;
01533 input_gen generalinput;
01534 generalinput.dumpbaselineL = 6;
01535 generalinput.dumpbaselineP = 4;
01536
01537 cerr << "\ncompbaseline\n";
01538 compbaseline(generalinput,masterinfo,slaveinfo,masterorbit,slaveorbit);
01539
01540 cerr << "\nOK!\n";
01541 return 0;
01542 }
01543 #endif // TESTMAIN
01544
01545
01546