Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Class Members | File Members

orbitbk.cc

Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 1999-2003 Bert Kampes
00003  * Copyright (c) 1999-2003 Delft University of Technology, The Netherlands
00004  *
00005  * This file is part of Doris, the Delft o-o radar interferometric software.
00006  *
00007  * Doris program is free software; you can redistribute it and/or modify
00008  * it under the terms of the GNU General Public License as published by
00009  * the Free Software Foundation; either version 2 of the License, or
00010  * (at your option) any later version.
00011  *
00012  * Doris is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program; if not, write to the Free Software
00019  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  *
00021  * Publications that contain results produced by the Doris software should
00022  * contain an acknowledgment. (For example: The interferometric processing
00023  * was performed using the freely available Doris software package developed
00024  * by the Delft Institute for Earth-Oriented Space Research (DEOS), Delft
00025  * University of Technology, or include a reference to: Bert Kampes and
00026  * Stefania Usai. \"Doris: The Delft Object-oriented Radar Interferometric
00027  * software.\" In: proceedings 2nd ITC ORS symposium, August 1999. (cdrom)).
00028  *
00029  */
00030 /****************************************************************
00031  * $Source: /users/kampes/DEVELOP/DORIS/doris/src/RCS/orbitbk.cc,v $    *
00032  * $Revision: 3.17 $                                            *
00033  * $Date: 2005/04/11 13:47:45 $                                 *
00034  * $Author: kampes $                                            *
00035  *                                                              *
00036  * implementation of orbit class.                               *
00037  * - orbit interpolation (spline and polynomial).               * 
00038  * - baseline estimation.                                       *
00039  * - utility dumping etc.                                       *
00040  *                                                              *
00041  * Compilation with: make testorbit                             *
00042  *  g++ -D__TESTMAIN__ orbit.cc -o testorbit                    *
00043  * creates a standalone executable for testing of the functions *
00044  * of the orbit class. Please see below: "main program".        *
00045  ****************************************************************/
00046 
00047 
00048 #include "orbitbk.hh"                   // declarations, matrix class
00049 #include "constants.hh"                 // global constants
00050 #include "refsystems.hh"                // wgs etc. ellipsoid
00051 #include "ioroutines.hh"                // error messages
00052 #include "conversion.hh"                // ?
00053 #include "utilities.hh"                 // solve33
00054 #include "exceptions.hh"                 // my exceptions class
00055 
00056 #include <cstdio>                       // some compilers, remove function
00057 #include <strstream>                    // for memory stream
00058 #include <iomanip>                      // setw
00059 
00060 
00061 
00062 // ====== Prototypes in this file ======
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  *    orbit::initialize                                         *
00075  * Fills data vectors, computes coefficients for interpolation  *
00076  * Expects time sorted data in file after string:               *
00077  * "NUMBER_OF_DATAPOINTS:   3"                                  *
00078  *  t1.xxx x1.xxx y1.xxx z1.xxx                                 *
00079  *  t2.xxx x2.xxx y2.xxx z2.xxx                                 *
00080  *  t3.xxx x3.xxx y3.xxx z3.xxx                                 *
00081  *                                                              *
00082  *    Bert Kampes, 11-Dec-1998                                  *
00083  #%// BK 17-Jul-2000: changed to class                          *
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   // ______ Open file ______
00094   //ifstream infile(file, ios::in | ios::nocreate);
00095   ifstream infile(file, ios::in);
00096   bk_assert(infile,file,__FILE__,__LINE__);
00097   numberofpoints = 0;
00098 
00099   // ====== Search file for data section ======
00100   while (infile)
00101     {
00102     infile >> word;
00103     if (strcmp("NUMBER_OF_DATAPOINTS:",word))           // no pattern match.
00104       {
00105       infile.getline(dummyline,ONE27,'\n');             // goto next line.
00106       }
00107     else                                                // in data section
00108       {
00109       foundsection=true;
00110       infile >> numberofpoints;
00111       klo=0;                            // initial guess
00112       khi=1;                            // initial guess, place in constructor
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   // ______ Actually read data ______
00120       for (i=0;i<numberofpoints;i++)
00121         {
00122         infile.getline(dummyline,ONE27,'\n');           // goto next record (start data)
00123         // for (register int32 j=0;j<field;j++)         // get data (field)
00124           infile >> time(i,0)
00125                  >> data_x(i,0)
00126                  >> data_y(i,0)
00127                  >> data_z(i,0);
00128         }
00129       } // else not foundsection
00130     } // file
00131 
00132   // ______ Tidy up ______
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   // ______ Compute interpolation coefficients ______
00146   DEBUG << "value of interp_method: " << interp_method;
00147   DEBUG.print();
00148   if (foundsection)
00149     {
00150     // ______ Check if time sorted data ______
00151     for (i=0; i<numberofpoints-1; ++i)
00152       {
00153       const real8 h = time(i+1,0) - time(i,0);  // delta_t, might not be const.
00154       if (h < EPS)                              // ~= 0.)
00155         {
00156         PRINT_ERROR("orbit time axis: require distinct, time sorted data")
00157         throw(input_error);// could simply warn as alternative
00158         }
00159       }
00160     // ___ set default degree if not spline or set explicitly ___
00161     if (interp_method==ORB_DEFAULT)
00162       {
00163       INFO.print("Setting default orbit interpolation method.");
00164       // use interpolation, since datapoints are result of orbit 
00165       // propogator, but don't be stupid.
00166       // use some redundancy to be able to detect outlier!
00167       //interp_method = (numberofpoints>5) ? 5 : numberofpoints-1;// exactly through points
00168       interp_method = (numberofpoints>6) ? 5 : numberofpoints-2;// 1 redundant!
00169       // for radarsat however, do use interpolation, seems to work
00170       // even when there are warnings?
00171       //if (numberofpoints==15 && (time(1,0)-time(0,0))>479.9)// RSAT?
00172       if ((time(1,0)-time(0,0))>479.9 && (time(1,0)-time(0,0))<481.1)// RSAT?
00173         {
00174         WARNING.print("Assuming RADARSAT: use highest polyfit recommended.");
00175         interp_method = numberofpoints-1;// even polyfit(14) seems to work..
00176         }
00177       }
00178     // ___ call function that computes coefficients ___
00179     computecoefficients();              // cubic spline or polynomial
00180     PROGRESS.print("Orbit: interpolation coefficients computed.");
00181     }
00182   #ifdef __DEBUG
00183   showdata();
00184   //const real8 t_tmp = (time(0,0)+time(1,0))/2.0;
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   } // END initialize
00202 
00203 
00204 
00205 /****************************************************************
00206  *    Compute coefficients for piecewise polynomial.            *
00207  * time axis and x,y,z should be filled already.                *
00208  * call splineinterpol routine.                                 *
00209  #%// BK 18-Jul-2000                                            *
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)// either spline method or degree of polynomial
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);// method==degree
00234       coef_y = polyfit(time,data_y,interp_method);// method==degree
00235       coef_z = polyfit(time,data_z,interp_method);// method==degree
00236     }
00237   } // END computecoefficients
00238 
00239 
00240 
00241 /****************************************************************
00242  *    private getklokhi                                         *
00243  *                                                              *
00244  * Returns klo/khi for spline interpolation routines.           *
00245  * klo is index in time axis before point t, khi is index after * 
00246  * old values are checked first because lot of successive       *
00247  * interpolation between same t0 t1 is expected.                *
00248  *                                                              *
00249  *    Bert Kampes, 11-Dec-1998                                  *
00250  #%// BK 18-Jul-2000: orbit class                               *
00251  ****************************************************************/
00252 void orbit::getklokhi(real8 t)
00253   {
00254   #ifdef __DEBUG
00255   // ___ Called a lot, within debug ___
00256   TRACE_FUNCTION("orbit::getinterval (BK 18-Jul-2000)")
00257   #endif
00258 
00259   // ______ Check if last interval still applies, init. to [0;1] ______
00260   if (time(klo,0) <= t && time(khi,0) >= t) 
00261     return;
00262 
00263   // _____ Else compute correct interval ______
00264   int32 k;
00265   klo = 0;
00266   khi = numberofpoints - 1;
00267   while(khi-klo>1)
00268     {
00269     k = (khi+klo) >> 1;                         // bisection, divide by two
00270     if (time(k,0) > t) 
00271       khi = k;
00272     else
00273       klo = k;
00274     }
00275   } // END getinterval
00276 
00277 
00278 
00279 /****************************************************************
00280  * public getxyz                                                *
00281  *                                                              *
00282  * Returns (natural) cubic spline interpolant.                  *
00283  * based on numerical recipes routine: splint pp.113            *
00284  * input:                                                       *
00285  *  - matrix time and position info                             *
00286  *  - matrix from splineinterpol                                *
00287  *  - t to be evaluated                                         *
00288  * output:                                                      *
00289  *  - interpolated value at t                                   *
00290  *                                                              *
00291  * use routine getsatpos, later we will make an orbit class...  *
00292  *                                                              *
00293  *    Bert Kampes, 11-Dec-1998                                  *
00294  *    Bert Kampes, 02-Jun-1999 streamlined, const variables etc.*
00295  ****************************************************************/
00296 cn orbit::getxyz(
00297         real8 t) // const // not, klo,khi
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     // ______ Compute correct interval ______
00313     getklokhi(t);                               // return correct interval
00314     const real8 h = time(khi,0) - time(klo,0);  // delta_t, might not be const.
00315     const real8 a = (time(khi,0) - t) / h;
00316     const real8 b = 1. - a;
00317     // ______ Evaluate function in x,y,z ______
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   // ______ Orbit interpolator is simple polynomial ______
00326   // ______ unfortunately, i normalize data here each time.
00327   // ______ we could normalize input t, but then also t_azi1
00328   // ______ and maybe more consequences for code.  see howlong this takes
00329   // ______ we normalize here only by a shift to avoid rescaling xdot and xddot
00330   // ______ (it seems else vdot=vdot*.5(min+max) for some reason)
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   } // END getxyz
00340 
00341 
00342 /****************************************************************
00343  *    getxyzdot                                                 *
00344  *                                                              *
00345  * Returns 1st derivative natural cubic spline interpolant.     *
00346  * Numerical recipes splint pp113                               *
00347  * input:                                                       *
00348  *  - t to be evaluated                                         *
00349  * output:                                                      *
00350  *  - interpolated value at t for x,y,z                         *
00351  *                                                              *
00352  *    Bert Kampes, 11-Dec-1998                                  *
00353  *    Bert Kampes, 02-Jun-1999 streamlined, const variables etc.*
00354  *    Bert Kampes, 06-Jul-2000 bugfix dt!=1                     *
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   // ______ Orbit interpolator is splines piecewise polynomial ______
00371   if (interp_method==ORB_SPLINE)
00372     {
00373     // ______ Compute correct interval ______
00374     getklokhi(t);                                       // return correct interval
00375     const real8 h = time(khi,0) - time(klo,0);  // delta_t, might not be const.
00376     const real8 a = (time(khi,0) - t) / h;
00377     const real8 b = 1. - a;
00378     // ______ Evaluate 1st derivative of function in x,y,z ______
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   // ______ Orbit interpolator is simple polynomial ______
00387   else
00388     {
00389     const int32 DEGREE = coef_x.lines()-1;
00390     satvel.x = coef_x(1,0);// a_1*t^0 + 2a_2*t^1 + 3a_3*t^2 + ...
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);// normalization
00402     satvel.y /= real8(10.0);// normalization
00403     satvel.z /= real8(10.0);// normalization
00404     }
00405   return satvel;
00406   } // END getxyzdot
00407 
00408 
00409 
00410 /****************************************************************
00411  *    getxyzddot                                                *
00412  *                                                              *
00413  * Returns 2nd derivative natural cubic spline interpolant.     *
00414  * Numerical recipes splint pp113                               *
00415  * input:                                                       *
00416  *  - t to be evaluated                                         *
00417  * output:                                                      *
00418  *  - interpolated value at t for x,y,z                         *
00419  *                                                              *
00420  *    Bert Kampes, 11-Dec-1998                                  *
00421  *    Bert Kampes, 02-Jun-1999 streamlined, const variables etc.*
00422  *    Bert Kampes, 06-Jul-2000 bugfix dt!=1                     *
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   // ______ Orbit interpolator is splines piecewise polynomial ______
00439   if (interp_method==ORB_SPLINE)
00440     {
00441     // ______ Compute correct interval ______
00442     getklokhi(t);                                       // return correct interval
00443     const real8 h = time(khi,0) - time(klo,0);  // delta_t, might not be const.
00444     const real8 a = (time(khi,0) - t) / h;
00445     const real8 b = 1. - a;
00446 
00447     // ______ Evaluate 2nd derivative of function in x,y,z ______
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   // ______ Orbit interpolator is simple polynomial ______
00453   else
00454     {
00455     // 2a_2 + 2*3a_3*t^1 + 3*4a_4*t^2...
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);//normalization
00469     satacc.y /= real8(100.0);//normalization
00470     satacc.z /= real8(100.0);//normalization
00471     }
00472   return satacc;
00473   } // END getxyzddot
00474 
00475 
00476 // ====== Friends ======
00477 /****************************************************************
00478  *    lp2xyz                                                    *
00479  *                                                              *
00480  * converts line,pixels to x,y,z on (which) ellipsoid?          *
00481  *                                                              *
00482  * input:                                                       *
00483  *  - MAXITER (default = 10)                                    *
00484  *  - CRITER  (default ~ 10-6 m ?)                              *
00485  * output:                                                      *
00486  *  - cn (x,y,z) (returnpos is filled)                          *
00487  *  - number of iterations is returned                          *
00488  *                                                              *
00489  *    Bert Kampes, 04-Jan-1999                                  *
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   // ______ Convert lp to time ______
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   // ______ Set up system of equations and iterate ______
00511   returnpos.x  = image.approxcentreoriginal.x;  // iteration 0
00512   returnpos.y  = image.approxcentreoriginal.y;  // iteration 0
00513   returnpos.z  = image.approxcentreoriginal.z;  // iteration 0
00514 
00515 
00516 // ______ Save some alloc, init and dealloc time by declaring static (15%?) ______
00517   static matrix<real8> solxyz(3,1);             // solution of system
00518   static matrix<real8> equationset(3,1);        // observations
00519   static matrix<real8> partialsxyz(3,3);        // partials to position due to nonlinear
00520 
00521   register int32 iter;
00522   for (iter=0; iter<=MAXITER; ++iter)
00523     {
00524 
00525     // ______ Update equations + solve system ______
00526     const cn dsat_P  =  returnpos.min(possat);  // vector satellite, P on ellipsoid
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     // ______ Solve system ______
00541     solve33(solxyz, equationset,partialsxyz);
00542 
00543     // ______Update solution______
00544     returnpos.x += solxyz(0,0);                         // update approx. value
00545     returnpos.y += solxyz(1,0);                         // update approx. value
00546     returnpos.z += solxyz(2,0);                         // update approx. value
00547 
00548     // ______Check convergence______
00549     if (abs(solxyz(0,0)) < CRITERPOS &&                         // dx
00550         abs(solxyz(1,0)) < CRITERPOS &&                         // dy
00551         abs(solxyz(2,0)) < CRITERPOS   )                        // dz
00552       break; // converged
00553     }
00554 
00555   // ______ Check number of iterations ______
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   // ______ (Converged) Result is in returnpos ______
00565   return iter;
00566   } // END lp2xyz
00567 
00568 
00569 /****************************************************************
00570  *    xyz2orb                                                   *
00571  *                                                              *
00572  * converts xyz (which) ellipsoid? to orbital coordinates       *
00573  * ellips, xyz is in system of orbit ephemerides                * 
00574  *                                                              *
00575  * input:                                                       *
00576  *  - MAXITER (default = 10)                                    *
00577  *  - CRITER  (default = 10-15s)                                *
00578  * output:                                                      *
00579  *  - x,y,z                                                     *
00580  *  - number of iterations                                      *
00581  *                                                              *
00582  #%// BK 22-Sep-2000                                            *
00583  ****************************************************************/
00584 int32 xyz2orb(
00585         cn              &possat,
00586         const slcimage  &image,
00587         orbit           &orb,       // non const, khi/klo
00588         const cn        &pointonellips,
00589         int32            MAXITER,   // defaults
00590         real8            CRITERTIM) // seconds
00591   {
00592   TRACE_FUNCTION("xyz2orb (BK 22-Sep-2000)");
00593   // ______ Initial value azimuth time ______
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)           // break at convergence
00598     {
00599     // ______ Update equations ______
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     // ______ Update solution ______
00606     sol   = -eq1_doppler(velsat, delta) /
00607              eq1_doppler_dt(delta, velsat, accsat);
00608     tazi +=  sol;
00609 
00610     // ______ Check convergence ______
00611     if (abs(sol) < CRITERTIM)                   // dta
00612       break;
00613     }
00614 
00615   // ______ Check number of iterations _____
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   // ====== Compute range time ======
00625   // ______ Update equations ______
00626   possat = orb.getxyz(tazi);
00627   return iter;
00628   } // END xyz2orb
00629 
00630 
00631 
00632 
00633 /****************************************************************
00634  *    xyz2t                                                     *
00635  *                                                              *
00636  * converts xyz (which) ellipsoid? to azimuth time with         *
00637  *  zero doppler equation, use slant range eq. for range time   *
00638  #%// BK 18-Jul-2000                                            *
00639  * ellips, xyz is in system of orbit ephemerides                * 
00640  *                                                              *
00641  * input:                                                       *
00642  *  - MAXITER (default = 10)                                    *
00643  *  - CRITER  (default = 10-15s)                                *
00644  * output:                                                      *
00645  *  - (updated tazi,tran)                                       *
00646  *  - number of iterations                                      *
00647  *                                                              *
00648  *    Bert Kampes, 04-Jan-1999                                  *
00649  ****************************************************************/
00650 int32 xyz2t(
00651         real8           &tazi,      // azimuth
00652         real8           &tran,      // and range time
00653         const slcimage  &image,
00654         orbit           &orb,       // non const, khi/klo
00655         const cn        &pos,
00656         int32            MAXITER,   // defaults
00657         real8            CRITERTIM) // seconds
00658   {
00659   TRACE_FUNCTION("xyz2t (BK 04-Jan-1999)")
00660 
00661   // ______ Compute initial value ______
00662   tazi = image.line2ta(.5*(image.currentwindow.linehi-image.currentwindow.linelo));       // iter0
00663   // _____ Start _______
00664   real8 sol;
00665   register int32 iter;
00666   for(iter=0 ;iter<=MAXITER; ++iter)           // break at convergence
00667     {
00668     // ______ Update equations ______
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     // ______ Update solution ______
00675     sol   = -eq1_doppler(velsat, delta) /
00676              eq1_doppler_dt(delta, velsat, accsat);
00677     tazi +=  sol;
00678 
00679     // ______ Check convergence ______
00680     if (abs(sol) < CRITERTIM)                   // dta
00681       break;
00682     }
00683 
00684   // ______ Check number of iterations _____
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   // ====== Compute range time ======
00694   // ______ Update equations ______
00695   const cn possat = orb.getxyz(tazi);
00696   const cn delta  = pos.min(possat);
00697   tran      = delta.norm() / SOL;
00698 
00699   return iter;
00700   } // END xyz2t
00701 
00702 
00703 
00704 /****************************************************************
00705  *    xyz2lp                                                    *
00706  *                                                              *
00707  * converts xyz (which) ellipsoid? to line,pixels               *
00708  *                                                              *
00709  * input:                                                       *
00710  *  - MAXITER (default = 10)                                    *
00711  *  - CRITER  (default ~ 10-15s)                                *
00712  * output:                                                      *
00713  *  - cn (x,y,z) (returnpos is filled)                          *
00714  *  - number of iterations                                      *
00715  *                                                              *
00716  *    Bert Kampes, 04-Jan-1999                                  *
00717  ****************************************************************/
00718 int32 xyz2lp(
00719         real8           &returnline,
00720         real8           &returnpixel,
00721         const slcimage &image,
00722         orbit           &orb,
00723         const cn        &pos,               // point at ground
00724         int32            MAXITER,
00725         real8            CRITERTIM)
00726   {
00727   TRACE_FUNCTION("xyz2lp (BK 04-Jan-1999)");
00728   real8 tazi;
00729   real8 tran;
00730 
00731   // ______ Compute tazi, tran ______
00732   int32 iter = xyz2t(tazi,tran,image,orb,pos,MAXITER,CRITERTIM);
00733 
00734   // ______ Convert time to pixel ______
00735   // ______ (Converged) Result is in returnline/pixel ______
00736   returnline  = image.ta2line(tazi);
00737   returnpixel = image.tr2pix (tran);
00738   //returnline  = ta2line(tazi,image.t_azi1,image.prf);
00739   //returnpixel = tr2pix (tran,image.t_range1,image.rsr2x);
00740   return iter;
00741   } // END xyz2lp
00742 
00743 
00744 
00745 /****************************************************************
00746  *    ell2lp                                                    *
00747  *                                                              *
00748  * converts ell to x,y,z, to l,p                                *
00749  *                                                              *
00750  * input:                                                       *
00751  *  - ??                                                        *
00752  *  - MAXITER (default = 10)                                    *
00753  *  - CRITER  (default ~ 10-6 m ?)                              *
00754  * output:                                                      *
00755  *  - phi,lambda,height (updated)                               *
00756  *  - number of iterations                                      *
00757  *                                                              *
00758  *    Bert Kampes, 27-Jan-1999                                  *
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   // ______ Transform ell2xyz ______
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   // ______ Transform xyz2lp ______
00781   int32 n_iter = xyz2lp(returnline, returnpixel,
00782             image, orb, returnpos,
00783             MAXITER, CRITERTIM);
00784   return n_iter;                            // number of iterations
00785   } // END ell2lp
00786 
00787 
00788 
00789 /****************************************************************
00790  *    lp2ell                                                    *
00791  *                                                              *
00792  * converts line,pixels to x,y,z to ell                         *
00793  *                                                              *
00794  * input:                                                       *
00795  *  - ??                                                        *
00796  *  - MAXITER (default = 10)                                    *
00797  *  - CRITER  (default ~ 10-6 m ?)                              *
00798  * output:                                                      *
00799  *  - phi,lambda,height (updated)                               *
00800  *  - number of iterations                                      *
00801  *                                                              *
00802  *    Bert Kampes, 27-Jan-1999                                  *
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   // ______ Transform lp2xyz ______
00819   cn returnpos;
00820   int32 iter = lp2xyz(line, pixel, ell, image, orb,
00821                        returnpos, MAXITER, CRITERPOS);
00822 
00823   // ______ Transform xyz2ell, compute phi,lambda,h ______
00824   xyz2ell(ell, returnpos, returnphi, returnlambda, returnheight);
00825 
00826   return iter;
00827   } // END lp2ell
00828 
00829 
00830 
00831 
00832 /****************************************************************
00833  *    compbaseline                                              *
00834  *                                                              *
00835  * Compute baseline parametrizations at grid:                   *
00836  *  - Bhorizontal, Bvertical, (B)                               *
00837  *  - Bparallel, Bperpendicular, (B)                            *
00838  *  - alpha, B                                                  *
00839  *  - angle between tracks, height ambiguity                    *
00840  *                                                              *
00841  * input:                                                       *
00842  *  - info structs                                              *
00843  *  - orbits                                                    *
00844  * output:                                                      *
00845  *  - void (INFO to screen)                                     *
00846  *                                                              *
00847  *    Bert Kampes, 21-Jun-1999                                  *
00848  * added grid option                                            *
00849  *    Bert Kampes, 06-Jul-2000                                  *
00850  * added modelling output for Bperp, theta, range               *
00851  #%// BK 21-Mar-2001                                            *
00852  ****************************************************************/
00853 /*
00854 void compbaseline(
00855         const input_gen &generalinput,
00856         const slcimage &master,
00857         const slcimage &slave,
00858               orbit     &masterorbit,
00859               orbit     &slaveorbit)
00860   {
00861   TRACE_FUNCTION("orbit::compbaseline (BK 21-Mar-2001)")
00862   // ___ prevent error if called after readfiles, but no orbit section ___
00863   if (masterorbit.numberofpoints==0 || slaveorbit.numberofpoints==0)
00864     {
00865     INFO.print("Exiting compbaseline, no orbit data master,slave available.");
00866     return;
00867     }
00868   const int32 numpointsl = generalinput.dumpbaselineL;  // number of lines to eval. baseline
00869   const int32 numpointsp = generalinput.dumpbaselineP;  // number of pixels to eval. baseline
00870   const real8 deltalines = master.currentwindow.lines()  / (numpointsl-1.);
00871   const real8 deltapixel = master.currentwindow.pixels() / (numpointsp-1.);
00872   if (numpointsl==0 || numpointsp==0)
00873     {
00874     INFO.print("Exiting compbaseline, no output requested with DUMPBASELINE card.");
00875     return;
00876     }
00877 
00878   const int32 MAXITER   = 10;
00879   const real8 CRITERPOS = 1e-6;
00880   const real8 CRITERTIM = 1e-10;
00881   INFO << "compbaseline: MAXITER: "   << MAXITER   << "; "
00882                      << "CRITERPOS: " << CRITERPOS << " m; "
00883                      << "CRITERTIM: " << CRITERTIM << " s";
00884   INFO.print();
00885 
00886   cn P;                         // point, returned by lp2xyz
00887   input_ell ellips;
00888   ellips.a   = WGS84_A;
00889   ellips.b   = WGS84_B;
00890   ellips.e2  = NaN;                     // not used
00891   ellips.e2b = NaN;                     // not used
00892   real8 s_tazi;                         // returned by xyz2t
00893   real8 s_trange;                       // returned by xyz2t
00894 
00895   // ______ Matrices for modeling Bperp (BK 21-mar-01) ______
00896   int32 cnt=0;
00897   matrix<real8> BPERP(numpointsl*numpointsp,1);
00898   matrix<real8> THETA(numpointsl*numpointsp,1);
00899   //matrix<real8> RANGE(numpointsl*numpointsp,1);
00900   //matrix<real8> THETA(numpointsl*numpointsp,1);
00901   matrix<real8> AMATRIX(numpointsl*numpointsp,3);
00902   matrix<real8> COEFFS(3,1);
00903 
00904   // ______ Loop over a few lines, pixels to compute baseline param. ______
00905   real8 line = master.currentwindow.linelo;
00906   bool compconverge = false;
00907   for (register int32 i=0; i<numpointsl; ++i) // azimuthlines
00908     {
00909     // ______ Azimuth time for this line ______
00910     //const real8 m_tazi   = line2ta(line,master.t_azi1,master.prf);
00911     const real8 m_tazi   = master.line2ta(line);
00912 
00913     // ______ xyz for master satelite from time ______
00914     // cn M = getsatpos(m_tazi,m_time,m_x,m_y,m_z,m_coefx,m_coefy,m_coefz);
00915     const cn M  = masterorbit.getxyz(m_tazi);
00916     real8 pixel = master.currentwindow.pixlo;
00917 
00918     // ______ Loop over a pixels to compute baseline param. ______
00919     for (register int32 j=0; j<numpointsp; ++j) // rangepixels
00920       {
00921       // ______ Range time for this pixel ______
00922       //const real8 m_trange = pix2tr(pixel,master.t_range1,master.rsr2x);
00923       const real8 m_trange = master.pix2tr(pixel);
00924       lp2xyz(line,pixel,ellips,master,masterorbit,P,MAXITER,CRITERPOS);
00925 
00926       // ______ Compute xyz for slave satelite from P ______
00927       xyz2t(s_tazi,s_trange,slave,slaveorbit,P,MAXITER,CRITERTIM);
00928 
00929       // ______ Slave position ______
00930       const cn S = slaveorbit.getxyz(s_tazi);
00931 
00932       // ______ Compute angle between near parallel orbits ______
00933       if (compconverge==false)
00934         {
00935         compconverge=true;
00936         const cn Mdot = masterorbit.getxyzdot(m_tazi);
00937         const cn Sdot = slaveorbit.getxyzdot(s_tazi);
00938         real8 angleorbits = rad2deg(Mdot.angle(Sdot));
00939         INFO << "Angle between orbits master-slave (at l,p="
00940              << line << "," << pixel << ") = "
00941              << angleorbits << " deg";
00942         INFO.print();
00943         }
00944 
00945       // ====== The baseline parameters, derived from the positions (x,y,z) ======
00946       // ______ alpha is angle counterclockwize(B, vlak met normal=rho1=rho2)
00947       // ______ theta is angle counterclockwize(rho1=M, r1=M-P, r2=S-P)
00948       const real8 B    = M.dist(S);                                     // abs. value
00949       // const real8 Bpar = P.dist(M) - P.dist(S);                      // sign ok
00950       const real8 Bpar = SOL*(m_trange-s_trange);               // sign ok
00951 
00952 
00953       // ______ if (MP>SP) then S is to the right of slant line, then B perp is positive.
00954       const cn r1 = M.min(P);
00955       const cn r2 = S.min(P);
00956       const real8 theta = M.angle(r1);
00957       const real8 Bperp = (theta > M.angle(r2)) ?                       // sign ok
00958            sqrt(sqr(B)-sqr(Bpar)) :
00959           -sqrt(sqr(B)-sqr(Bpar));
00960 
00961       // ______ Modelling of Bperp(l,p) = a00 + a10*l + a01*p ______
00962       BPERP(cnt,0)   = Bperp;
00963       THETA(cnt,0)   = theta;
00964       AMATRIX(cnt,0) = 1.;
00965       AMATRIX(cnt,1) = real8(line);
00966       AMATRIX(cnt,2) = real8(pixel);
00967       cnt++;
00968 
00969       // ______ ______
00970       const real8 alfa  = theta - atan2(Bpar,Bperp);            // ok atan2
00971 
00972       //    real8 Bh  = Bperp*cos(theta) + Bpar*sin(theta);
00973       //    real8 Bv  = Bperp*sin(theta) - Bpar*cos(theta);
00974       const real8 Bh  = B * cos(alfa);                          // sign ok
00975       const real8 Bv  = B * sin(alfa);                          // sign ok
00976 
00977       // ______ Height ambiguity: [h] = -lambda/4pi * (r1sin(theta)/Bperp) * phi==2pi ______
00978       const real8 hambiguity = -master.wavelength*r1.norm()*sin(theta) / (2*Bperp);
00979 
00980       // ====== Write output to screen as INFO ======
00981       INFO << "The baseline parameters for (l,p) = " << line << ", " << pixel;
00982       INFO.print();
00983       INFO << "\talpha (deg), baseline: \t" << rad2deg(alfa) << " \t" << B;
00984       INFO.print();
00985       INFO << "\tBpar, Bperp: \t\t" << Bpar << " \t" << Bperp;
00986       INFO.print();
00987       INFO << "\tBh, Bv: \t\t" << Bh << " \t" << Bv;
00988       INFO.print();
00989       INFO << "\tHeight ambiguity: \t" << hambiguity;
00990       INFO.print();
00991       INFO << "\ttheta (deg): \t\t" << rad2deg(theta);
00992       INFO.print();
00993 
00994 
00995       // ______ Some extra info if in debug mode ______
00996       #ifdef __DEBUG
00997       INFO.precision(10);
00998       INFO.width(11);
00999       INFO.rewind();
01000       INFO << "\tM (x,y,z) = "
01001            << M.x << ", " << M.y << ", " << M.z;
01002       INFO.print();
01003       INFO << "\tS (x,y,z) = "
01004            << S.x << ", " << S.y << ", " << S.z;
01005       INFO.print();
01006       INFO << "\tP (x,y,z) = "
01007            << P.x << ", " << P.y << ", " << P.z;
01008       INFO.print();
01009       INFO.reset();// reset width/precision/pointer
01010       #endif
01011 
01012 
01013       // ______ update pixel counter ______
01014       pixel+=deltapixel;
01015       } // loop pixels
01016 
01017     // ______ update line counter ______
01018     line+=deltalines;
01019     if (master.currentwindow.linehi == 0)         // unlikely but possible
01020       break;
01021     } // loop lines
01022  
01023   // ====== Model the Bperp as 2d polynomial of degree 1 ======
01024   matrix<real8> N      = matTxmat(AMATRIX,AMATRIX);
01025   matrix<real8> rhsB   = matTxmat(AMATRIX,BPERP);
01026   matrix<real8> rhsT   = matTxmat(AMATRIX,THETA);
01027   matrix<real8> Qx_hat = N;
01028   choles(Qx_hat);               // Cholesky factorisation normalmatrix
01029   solvechol(Qx_hat,rhsB);       // Solution Bperp coefficients in rhsB
01030   solvechol(Qx_hat,rhsT);       // Solution Theta coefficients in rhsT
01031   invertchol(Qx_hat);           // Covariance matrix of unknwons
01032 
01033   // ______Test inverse______
01034   //int32 i,j;
01035   for (int32 i=0; i<Qx_hat.lines(); i++)
01036     for (int32 j=0; j<i; j++)
01037       Qx_hat(j,i) = Qx_hat(i,j);
01038   const real8 maxdev = max(abs(N*Qx_hat-eye(real8(Qx_hat.lines()))));
01039   INFO << "baseline: max(abs(N*inv(N)-I)) = " << maxdev;
01040   INFO.print();
01041   //matrix<real8> unity = N * Qx_hat;
01042   //real8 maxdev = abs(unity(0,0)-1);
01043   //for (int32 i=1; i<unity.lines(); i++)
01044   //  {
01045   //  if (abs(unity(i,i)-1) > maxdev)
01046   //    maxdev = abs(unity(i,i)-1);
01047   //  for (int32 j=0; j<i-1; j++)
01048   //    {
01049   //    if (abs(unity(i,j)) > maxdev)
01050   //      maxdev = abs(unity(i,j));
01051   //    if (abs(unity(j,i)) > maxdev)
01052   //      maxdev = abs(unity(j,i));
01053   //    }
01054   //  }
01055   if (maxdev > .01)
01056     {
01057     WARNING << "compbaseline: maximum deviation N*inv(N) from unity = "
01058        << maxdev << ". This is larger than .01: do not use this.";
01059     WARNING.print();// no error, no problem
01060     }
01061   else if (maxdev > .001)
01062     {
01063     WARNING << "compbaseline: maximum deviation N*inv(N) from unity = "
01064        << maxdev << ". This is between 0.01 and 0.001 (do not use it)";
01065     WARNING.print();
01066     }
01067   else
01068     {
01069     INFO << "compbaseline: maximum deviation N*inv(N) from unity = "
01070        << maxdev << ". This is ok.";
01071     INFO.print();
01072     }
01073   // ______Some other stuff, scale is ok______
01074   //matrix<real8> Qy_hat = AMATRIX * (matxmatT(Qx_hat,AMATRIX));
01075   matrix<real8> y_hatB  = AMATRIX * rhsB;
01076   matrix<real8> e_hatB  = BPERP - y_hatB;
01077   //matrix<real8> Qe_hat  = Qy - Qy_hat;
01078 
01079   matrix<real8> y_hatT  = AMATRIX * rhsT;
01080   matrix<real8> e_hatT  = THETA - y_hatT;
01081 
01082   // ______ Output solution and give max error ______
01083   INFO.print("--------------------");
01084   INFO.print("Result of modeling: Bperp(l,p) = a00 + a10*l + a01*p");
01085   INFO.print("Bperp for reference ellipsoid, in meters.");
01086   INFO.print("l and p in unnormalized, absolute, coordinates (1:N).");
01087   INFO << "Bperp_a00 = " << rhsB(0,0);
01088   INFO.print();
01089   INFO << "Bperp_a10 = " << rhsB(1,0);
01090   INFO.print();
01091   INFO << "Bperp_a01 = " << rhsB(2,0);
01092   INFO.print();
01093   real8 maxerr = max(abs(e_hatB));
01094   if (maxerr > 2.00)// 
01095     {
01096     WARNING << "Max. error bperp modeling at datapoints: " << maxerr << "m";
01097     WARNING.print();
01098     }
01099   else
01100     {
01101     INFO << "Max. error bperp modeling at datapoints: " << maxerr << "m";
01102     INFO.print();
01103     }
01104   INFO.print();
01105   INFO.print("--------------------");
01106   INFO.print("Result of modeling: theta(l,p) = a00 + a10*l + a01*p");
01107   INFO.print("Theta for reference ellipsoid, in radians.");
01108   INFO.print("l and p in unnormalized, absolute, coordinates (1:N).");
01109   INFO << "theta_a00 = " << rhsT(0,0);
01110   INFO.print();
01111   INFO << "theta_a10 = " << rhsT(1,0);
01112   INFO.print();
01113   INFO << "theta_a01 = " << rhsT(2,0);
01114   INFO.print();
01115   maxerr = max(abs(e_hatT));
01116   INFO << "Max. error modeling theta at datapoints: " << maxerr;
01117   INFO.print();
01118   INFO.print("--------------------");
01119   INFO.print("Range: r(p) = r0 + dr*p");
01120   INFO.print("l and p in unnormalized, absolute, coordinates (1:N).");
01121   //const real8 range1    = pix2range(1,master.t_range1,master.rsr2x);
01122   //const real8 range5000 = pix2range(5000,master.t_range1,master.rsr2x);
01123   const real8 range1    = master.pix2range(1.0);
01124   const real8 range5000 = master.pix2range(5000.0);
01125   const real8 drange    = (range5000-range1)/5000;
01126   INFO << "range = " << range1-drange << " + " << drange << "*p";
01127   INFO.print();
01128 
01129   // ====== Tidy up ======
01130   } // END compbaseline
01131 */
01132 
01133 
01134 /****************************************************************
01135  *    dumporbit                                                 *
01136  *                                                              *
01137  * rescale data to interval [-1,1]                              *
01138  * data -> X(data-min)/(max-min) ; data+SS                      *
01139  * X=1--1==EE-SS; SS=-1, EE=1                                   *
01140  *                                                              *
01141  * input:                                                       *
01142  * output:                                                      *
01143  *    Bert Kampes, 03-Jul-2000                                  *
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   // ___ prevent error if called after readfiles, but no orbit section ___
01151   if (numberofpoints==0)
01152     {
01153     INFO.print("Exiting dumporbit, no orbit data available.");
01154     return;
01155     }
01156   // ====== Compute positions for center pixel ======
01157   // ______ *dumporbit initialized to -1.0 in readinput.c ______
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   //  ______ Evaluate polynomial orbit for t1:dt:tN ______
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   // this is ok, but test if not: fo.setf(ios::left);
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   // ______ dump coeff. as well for testing ... ______
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   } // END dumporbit
01229 
01230 
01231 
01232 
01233 // ====== helper functions ======
01234 /****************************************************************
01235  *    splineinterpol                                            *
01236  *                                                              *
01237  * Based on Numerical recipes spline pp.113                     *
01238  * time <-> x                                                   *
01239  * data <-> y                                                   *
01240  * pp   <-> y2                                                  *
01241  * rhs  <-> u                                                   *
01242  *                                                              *
01243  * Compute coefficients of piecewize polynomials for            *
01244  *  cubic splines. Use natural ones if __NATURALSPLINE__ is     *
01245  *  defined (by default internal in this routine.)              *
01246  * Else use boundary condition from nrc pp113.                  *
01247  * (fix first derivative.)                                      *
01248  * There does not seem to be a big difference between both      *
01249  * boundary conditions, particularly for middle segment.        *
01250  *  A pp = rhs; L Lt pp = rhs; (?)                              *
01251  *  pptmp=Lt pp); L pptmp = rhs; Lt pp = pptmp; (?)             *
01252  * input:                                                       *
01253  *  - matrix by getdata with time and position info             *
01254  * output:                                                      *
01255  *  - matrix with 2nd order derivatives                         *
01256  *    (input for interp. routines)                              *
01257  *                                                              *
01258  *    Bert Kampes, 11-Dec-1998                                  *
01259  #%// BK 17-Jul-2000: added nonnatural                          *
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();                       // number of points
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())                                // number of points
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);                        // init to 0.
01281 
01282   // #define __NATURALSPLINE__ // in Makefile or here if so desired.
01283 #define __NATURALSPLINE__
01284   // ====== Set boundary condition first datapoint ======
01285   #ifdef __NATURALSPLINE__
01286     pp(0,0)  = 0.;                      // natural spline boundary condition
01287   #else
01288     // ______ yp1 is first der. at point 0 ______
01289     // estimate it by dy/dx: nearly linear for sat. orbit...
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   // ====== Decomposition loop ======
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   // ====== Set boundary condition last datapoint ======
01312   #ifdef __NATURALSPLINE__
01313     pp(N-1,0) = 0.;             // natural spline oundary condition
01314   #else
01315     // ______ ypN is first derivative at point N-1 ______
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   // ====== Backsub loop ======
01324   for (i=N-2; i>=0; --i)
01325     pp(i,0) = pp(i,0)*pp(i+1,0) + rhs(i,0);
01326     // pp(i,0) *= pp(i+1,0) + rhs(i,0); // ??
01327 
01328   return pp;            // contains second derivatives at datapoints
01329   } // END splineinterpol
01330 
01331 
01332 /****************************************************************
01333  *    polyfit                                                   *
01334  *                                                              *
01335  * Compute coefficients of x=a0+a1*t+a2*t^2+a3*t3 polynomial    *
01336  * for orbit interpolation.  Do this to facilitate a method     *
01337  * in case only a few datapoints are given.                     *
01338  * Data t is temporarily normalized [-2,2], then polynomial     *
01339  * is recomputed for normal t input (secofday).                 *
01340  * (since othwise requires too much changes in the code         *
01341  * input:                                                       *
01342  *  - matrix by getdata with time and position info             *
01343  * output:                                                      *
01344  *  - matrix with coeff.                                        *
01345  *    (input for interp. routines)                              *
01346  *                                                              *
01347  *    Bert Kampes, 16-Jun-2003                                  *
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();   // number of points
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())             // number of points
01362     {
01363     PRINT_ERROR("code 902: polyfit: require same size vectors.");
01364     throw(input_error);
01365     }
01366  
01367   // ______ Normalize t for numerical reasons ______
01368   DEBUG.print("normalizing t axis for least squares fit");
01369   matrix<real8> t=(time-time(Npoints/2,0))/real8(10.0);
01370 
01371   // ______ Check redundancy ______
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   // ______ Set up system of equations to solve coeff. ______
01386   DEBUG.print("Setting up lin. system of equations");
01387   matrix<real8> A(Npoints,Nunk);// designmatrix
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);               // Cholesky factorisation normalmatrix
01400   solvechol(Qx_hat,rhs);        // Estimate of unknowns in rhs
01401   invertchol(Qx_hat);           // Covariance matrix
01402   // ______Test inverse______
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);// repair matrix
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   matrix<real8> unity = N * Qx_hat;
01411   real8 maxdev = abs(unity(0,0)-1);
01412   for (i=1; i<unity.lines(); i++)
01413     {
01414     if (abs(unity(i,i)-1) > maxdev)
01415     maxdev = abs(unity(i,i)-1);
01416     for (j=0; j<i-1; j++)
01417       {
01418       if (abs(unity(i,j)) > maxdev)
01419       maxdev = abs(unity(i,j));
01420       if (abs(unity(j,i)) > maxdev)
01421       maxdev = abs(unity(j,i));
01422       }
01423     }
01424   */
01425   // ___ report max error... (seems sometimes this can be extremely large) ___
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)// 0.05 is already 1 wavelength! (?)
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   // ___ report computations in detail ___
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     //DEBUG << i << " " << time(i,0) << " " 
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   // ___ seems sometimes dt is wrong?? --->log it. ___
01452   for (i=0; i<Npoints-1; i++)
01453     {
01454     // ___ check if dt is constant, not necessary for me, but may ___
01455     // ___ signal error in header data of SLC image ___
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)// 1ms i will allow...
01460       WARNING.print("Orbit: data does not have equidistant time interval?");
01461     }
01462   DEBUG.reset();
01463 
01464   // ___ return the coefficients wrt. normalized t ___
01465   return rhs;
01466   } // END polyfit
01467 
01468 
01469 
01470 /****************************************************************
01471  *    orbit::showdata                                           *
01472  #%// BK 17-Jul-2000                                            *
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   } // END showdata
01493 
01494 
01495 #ifdef __TESTMAIN__
01496 /****************************************************************
01497  *    test program for orbit routine                            *
01498  #%// BK 18-Jul-2000                                            *
01499  ****************************************************************/
01500 int32 main()
01501   {
01502   orbit masterorbit;
01503   orbit slaveorbit;
01504 
01505   // file required named "master.out" with string
01506   // "NUMBER_OF_DATAPOINTS:" and ephemerides required.
01507   masterorbit.initialize("master.out");
01508   cerr << "\nShowing master orbit: "
01509        << masterorbit.npoints() << " points.\n";
01510   masterorbit.showdata();
01511 
01512   // file required named "slave.out" with string
01513   // "NUMBER_OF_DATAPOINTS:" and ephemerides required.
01514   slaveorbit.initialize("slave.out");
01515   cerr << "\nShowing slave orbit: "
01516        << slaveorbit.npoints() << " points.\n";
01517   slaveorbit.showdata();
01518 
01519   // start interpolation tests
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   } // END main
01543 #endif // TESTMAIN
01544 
01545 
01546 

Generated on Fri Apr 22 15:57:56 2005 for Doris by doxygen 1.3.6