orsa_orbit_gsl.cc

Go to the documentation of this file.
00001 /* 
00002    ORSA - Orbit Reconstruction, Simulation and Analysis
00003    Copyright (C) 2002-2004 Pasquale Tricarico
00004    
00005    This program is free software; you can redistribute it and/or
00006    modify it under the terms of the GNU General Public License
00007    as published by the Free Software Foundation; either version 2
00008    of the License, or (at your option) any later version.
00009    
00010    As a special exception, Pasquale Tricarico gives permission to
00011    link this program with Qt commercial edition, and distribute the
00012    resulting executable, without including the source code for the Qt
00013    commercial edition in the source distribution.
00014    
00015    This program is distributed in the hope that it will be useful,
00016    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018    GNU General Public License for more details.
00019    
00020    You should have received a copy of the GNU General Public License
00021    along with this program; if not, write to the Free Software
00022    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
00023 */
00024 
00025 #include "orsa_orbit_gsl.h"
00026 
00027 #ifdef HAVE_CONFIG_H
00028 #include <config.h>
00029 #endif // HAVE_CONFIG_H
00030 
00031 #ifdef HAVE_MPI
00032 #include <mpi.h>
00033 #endif // HAVE_MPI
00034 
00035 #include <gsl/gsl_roots.h>
00036 #include <gsl/gsl_errno.h>
00037 #include <gsl/gsl_siman.h>
00038 #include <gsl/gsl_multimin.h>
00039 #include <gsl/gsl_rng.h>
00040 #include <gsl/gsl_multifit_nlin.h>
00041 #include <gsl/gsl_blas.h>
00042 #include <gsl/gsl_diff.h>
00043 #include <gsl/gsl_deriv.h>
00044 #include <gsl/gsl_linalg.h>
00045 #include <gsl/gsl_cblas.h>
00046 #include <gsl/gsl_randist.h>
00047 #include <gsl/gsl_eigen.h>
00048 
00049 #include "orsa_file.h"
00050 #include "orsa_error.h"
00051 
00052 #include <sys/time.h>
00053 #include <time.h>
00054 
00055 #include <string.h> // memcpy()
00056 #include <algorithm> // sort()
00057 #include <limits>
00058 
00059 using namespace std;
00060 
00061 namespace orsa {
00062   
00063   void PreliminaryOrbit::ComputeRMS(const vector<Observation> &obs) {
00064     rms = RMS_residuals(obs,*this);
00065   }
00066   
00067   //
00068   
00069   double Weight(const Observation &obs1, const Observation &obs2, const double optimal_dt=FromUnits(1,DAY)) {
00070     double w;
00071     double dt = fabs(FromUnits(obs1.date.GetJulian()-obs2.date.GetJulian(),DAY));
00072     /* 
00073        if (dt<optimal_dt) {
00074        w = optimal_dt/dt;
00075        } else {
00076        w = 1+dt/optimal_dt;
00077        }
00078     */
00079     //
00080     if (dt<optimal_dt) {
00081       w = dt/optimal_dt;
00082     } else {
00083       w = 1+optimal_dt/dt;
00084     }
00085     //
00086     return w;
00087   }
00088   
00089   double Weight(vector<Observation> &obs, const double optimal_dt=FromUnits(1,DAY)) {
00090     if (obs.size() < 2) return 0; // error...
00091     double w=0;
00092     sort(obs.begin(),obs.end());
00093     unsigned int k=1;
00094     while (k < obs.size()) {
00095       w += Weight(obs[k-1],obs[k],optimal_dt);
00096       ++k;
00097     }
00098     return w;
00099   }
00100   
00101   class ThreeObservations : public vector<Observation> {
00102   public:
00103     /* 
00104        inline bool operator < (const ThreeObservations &triobs) const {
00105        return (w < triobs.w);
00106     } 
00107     */
00108     //
00109     inline bool operator < (const ThreeObservations &triobs) const {
00110       return (triobs.w < w);
00111     }
00112   private:
00113     double w;
00114   public:
00115     double Weight() const { return w; }
00116   public:
00117     void ComputeWeight(const double optimal_dt=FromUnits(1,DAY)) {
00118       w = orsa::Weight(*this,optimal_dt);
00119     }
00120   };
00121   
00122   void BuildObservationTriplets(const vector<Observation> & obs, vector<ThreeObservations> &triplets, const double optimal_dt=FromUnits(1,DAY)) {
00123     // sort(obs.begin(),obs.end());
00124     triplets.clear();
00125     unsigned int l,m,n;
00126     const unsigned int s = obs.size();
00127     // build only triplets with observations delays smaller than and obs_delay_max
00128     const double obs_delay_max = FromUnits(180,DAY);
00129     // max ration between dalays
00130     const double obs_delay_ration_max = 10.0;
00131     //
00132     Observation ol,om,on;
00133     double dt12, dt23, dt_ratio;
00134     ThreeObservations triobs; triobs.resize(3);
00135     l=0;
00136     while (l<s) {
00137       ol=obs[l];
00138       triobs[0] = ol;
00139       m=l+1;
00140       while (m<s) {
00141         om=obs[m];
00142         triobs[1] = om;
00143         n=m+1;
00144         while (n<s) {
00145           on=obs[n];
00146           triobs[2] = on;
00147           
00148           triobs.ComputeWeight(optimal_dt);
00149           
00150           dt12 = FromUnits(om.date.GetJulian()-ol.date.GetJulian(),DAY);
00151           dt23 = FromUnits(on.date.GetJulian()-om.date.GetJulian(),DAY);
00152           
00153           if (dt12>dt23) 
00154             dt_ratio = dt12/dt23;
00155           else           
00156             dt_ratio = dt23/dt12;
00157           
00158           if ((dt12<obs_delay_max) && (dt23<obs_delay_max) && (dt_ratio<obs_delay_ration_max)) triplets.push_back(triobs);
00159           
00160           // debug
00161           std::cerr << l << "-" << m << "-" << n << std::endl;
00162           
00163           ++n;
00164         }
00165         ++m;
00166       }
00167       ++l;
00168     }
00169     
00170     sort(triplets.begin(),triplets.end());
00171   }
00172   
00173   // MOID
00174   
00175   class MOID_parameters {
00176   public:
00177     Orbit *o1, *o2;
00178   };
00179   
00180   double MOID_f (const gsl_vector *v, void *params) {
00181     
00182     double M1 = gsl_vector_get(v,0);
00183     double M2 = gsl_vector_get(v,1);
00184     
00185     MOID_parameters *p = (MOID_parameters*) params;
00186     
00187     p->o1->M = M1;
00188     p->o2->M = M2;
00189     
00190     Vector r1,r2,v1,v2;
00191     
00192     p->o1->RelativePosVel(r1,v1);
00193     p->o2->RelativePosVel(r2,v2);
00194     
00195     return (r1-r2).Length();
00196   }
00197   
00198   class MOID_solution {
00199   
00200   public:
00201     MOID_solution() { active=false; }
00202     
00203   public: 
00204     void Insert(const double moid, const double M1, const double M2) {
00205       if ((!active) || ((active) && (moid < _moid))) {
00206         _moid   = moid;
00207         _M1     = M1;
00208         _M2     = M2;
00209         active = true;
00210       }
00211     }
00212     
00213   public:  
00214     double M1()   const { return _M1; }
00215     double M2()   const { return _M2; }
00216     double moid() const { return _moid; }
00217     
00218   private:
00219     double _moid,_M1,_M2;
00220     bool   active;
00221   };
00222   
00223   double MOID(const Orbit &o1_in, const Orbit &o2_in, Vector &r1, Vector &r2) {
00224     
00225     Orbit o1(o1_in);
00226     Orbit o2(o2_in);
00227     
00228     MOID_parameters par;
00229     par.o1 = &o1;
00230     par.o2 = &o2;
00231     
00232     gsl_multimin_fminimizer *s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex,2);
00233     
00234     gsl_multimin_function moid_function;
00235     
00236     moid_function.f      = &MOID_f;
00237     moid_function.n      = 2;
00238     moid_function.params = &par;
00239     
00240     gsl_vector *x         = gsl_vector_alloc(2);
00241     gsl_vector *step_size = gsl_vector_alloc(2);
00242     
00243     // gsl_vector_set(step_size,0,pi);
00244     // gsl_vector_set(step_size,1,pi);
00245     
00246     MOID_solution best_solution;
00247     
00248     int gsl_solutions_iter=0,max_gsl_solutions_iter=7;
00249     while (gsl_solutions_iter<max_gsl_solutions_iter) {
00250       
00251       // gsl_vector_set(x,0,o1.M);
00252       // gsl_vector_set(x,1,o2.M);
00253       // gsl_vector_set(x,0,0.0);
00254       // gsl_vector_set(x,1,0.0);
00255       // gsl_vector_set(x,0,fmod(10*twopi   -o1.omega_node-o1.omega_pericenter,twopi));
00256       // gsl_vector_set(x,1,fmod(10*twopi+pi-o2.omega_node-o2.omega_pericenter,twopi));
00257       gsl_vector_set(x,0,fmod(10*twopi+(twopi/(2*max_gsl_solutions_iter))*gsl_solutions_iter   -o1.omega_node-o1.omega_pericenter,twopi));
00258       gsl_vector_set(x,1,fmod(10*twopi+(twopi/(2*max_gsl_solutions_iter))*gsl_solutions_iter+pi-o2.omega_node-o2.omega_pericenter,twopi));
00259       
00260       gsl_vector_set(step_size,0,pi);
00261       gsl_vector_set(step_size,1,pi);
00262       
00263       gsl_multimin_fminimizer_set (s, &moid_function, x, step_size);
00264       
00265       size_t iter = 0;
00266       int status;
00267       do {
00268         
00269         iter++;
00270         
00271         // iter status
00272         status = gsl_multimin_fminimizer_iterate(s);
00273         
00274         // convergence status
00275         status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),1.0e-6);
00276         
00277         // if (status == GSL_SUCCESS) printf ("Minimum found at:\n");
00278         
00279         /* printf ("%5d %.5f %.5f %10.5f\n", iter,
00280            (180/pi)*gsl_vector_get (s->x, 0), 
00281            (180/pi)*gsl_vector_get (s->x, 1), 
00282            gsl_multimin_fminimizer_minimum(s));
00283         */
00284         
00285         // } while (status == GSL_CONTINUE && iter < 500);
00286       } while (status == GSL_CONTINUE && iter < 200);
00287       
00288       // const double moid = gsl_multimin_fminimizer_minimum(s);
00289       
00290       // cerr << "MOID iters: " << iter << endl;
00291       
00292       if (status == GSL_SUCCESS) {
00293         best_solution.Insert(gsl_multimin_fminimizer_minimum(s),gsl_vector_get(s->x,0),gsl_vector_get(s->x,1));
00294       } 
00295       
00296       ++gsl_solutions_iter;
00297     }
00298     
00299     const double moid = best_solution.moid();
00300     o1.M              = best_solution.M1();
00301     o2.M              = best_solution.M2();
00302     Vector v1, v2;
00303     // update r1 and r2 for output
00304     o1.RelativePosVel(r1,v1);
00305     o2.RelativePosVel(r2,v2);
00306     
00307     /*
00308       printf ("%5d %.5f %.5f %.5f %.5f %10.5f\n", iter,
00309       (180/pi)*gsl_vector_get (s->x, 0), (180/pi)*o1.M,
00310       (180/pi)*gsl_vector_get (s->x, 1), (180/pi)*o2.M,
00311       gsl_multimin_fminimizer_minimum(s));
00312     */
00313     
00314     // free
00315     gsl_multimin_fminimizer_free(s);
00316     gsl_vector_free(x);
00317     gsl_vector_free(step_size);
00318     
00319     // check 
00320     // WARNING! some vars can be modified by
00321     // this check area --> comment out when not debugging!
00322     if (0) {
00323       
00324       Orbit o1(o1_in);
00325       Orbit o2(o2_in);
00326       
00327       // random number generator
00328       // const int random_seed = 124323;
00329       struct timeval  tv;
00330       struct timezone tz;
00331       gettimeofday(&tv,&tz); 
00332       const int random_seed = tv.tv_usec;
00333       gsl_rng * rnd = gsl_rng_alloc(gsl_rng_gfsr4);
00334       gsl_rng_set(rnd,random_seed);
00335       int j,k;
00336       Vector r1,v1,r2,v2;
00337       double d, min_d=moid;
00338       //
00339       /* 
00340          double angle;
00341          j=0;
00342          while (j < 500) {
00343          // coordinated test
00344          angle = twopi*gsl_rng_uniform(rnd);
00345          o1.M = fmod(10*twopi+angle-o1.omega_node-o1.omega_pericenter,twopi);
00346          o2.M = fmod(10*twopi+angle-o2.omega_node-o2.omega_pericenter,twopi);
00347          o1.RelativePosVel(r1,v1);
00348          o2.RelativePosVel(r2,v2);
00349          d = (r1-r2).Length();
00350          if (d < min_d) {
00351          cerr << "(C) ";
00352          min_d = d;
00353          }
00354          ++j;
00355          }
00356       */
00357       //
00358       // random test
00359       j=0;
00360       while (j < 50) {
00361         o1.M = twopi*gsl_rng_uniform(rnd);
00362         o1.RelativePosVel(r1,v1);
00363         k=0;
00364         while (k < 50) {
00365           // o1.M = twopi*gsl_rng_uniform(rnd);
00366           o2.M = twopi*gsl_rng_uniform(rnd);
00367           // o1.RelativePosVel(r1,v1);
00368           o2.RelativePosVel(r2,v2);
00369           d = (r1-r2).Length();
00370           if (d < min_d) {
00371             // cerr << "WARNING: found another solution for the MOID: " << d << " < " << moid << "  delta = " << d-moid << endl;
00372             // printf("WARNING: found another solution for the MOID: %.10f < %.10f   delta: %.10f\n",d,moid,d-moid);
00373             // exit(0);
00374             std::cerr << "(R) ";
00375             min_d = d;
00376           }
00377           ++k;
00378         }
00379         ++j;
00380       }
00381       //
00382       if (min_d < moid) {
00383         // cerr << "WARNING: found another solution for the MOID: " << d << " < " << moid << "  delta = " << d-moid << endl;
00384         printf("\n WARNING: found another solution for the MOID: %.10f < %.10f   delta: %.10f\n",min_d,moid,min_d-moid);
00385         // exit(0);
00386       }
00387       // free random number generator
00388       gsl_rng_free(rnd);
00389       
00390     }
00391     
00392     return moid;
00393   }
00394   
00395   
00396   // MOID2RB
00397   
00398   class MOID2RB_parameters {
00399   public:
00400     Orbit *o1, *o2;
00401     Vector rb1, rb2;
00402   };
00403   
00404   double MOID2RB_f (const gsl_vector *v, void *params) {
00405     
00406     double M1 = gsl_vector_get(v,0);
00407     double M2 = gsl_vector_get(v,1);
00408     
00409     MOID2RB_parameters *p = (MOID2RB_parameters*) params;
00410     
00411     p->o1->M = M1;
00412     p->o2->M = M2;
00413     
00414     Vector r1,r2,v1,v2;
00415     
00416     p->o1->RelativePosVel(r1,v1);
00417     p->o2->RelativePosVel(r2,v2);
00418     
00419     return ((p->rb1+r1)-(p->rb2+r2)).Length();
00420   }
00421   
00422   class MOID2RB_solution {
00423   
00424   public:
00425     MOID2RB_solution() { active=false; }
00426     
00427   public: 
00428     void Insert(const double moid2rb, const double M1, const double M2) {
00429       if ((!active) || ((active) && (moid2rb < _moid2rb))) {
00430         _moid2rb   = moid2rb;
00431         _M1     = M1;
00432         _M2     = M2;
00433         active = true;
00434       }
00435     }
00436     
00437   public:  
00438     double M1()   const { return _M1; }
00439     double M2()   const { return _M2; }
00440     double moid2rb() const { return _moid2rb; }
00441     
00442   private:
00443     double _moid2rb,_M1,_M2;
00444     bool   active;
00445   };
00446   
00447   double MOID2RB(const Vector &rb1, const Vector &rb2, const Orbit &o1_in, const Orbit &o2_in, Vector &r1, Vector &r2) {
00448     
00449     Orbit o1(o1_in);
00450     Orbit o2(o2_in);
00451     
00452     MOID2RB_parameters par;
00453     par.o1 = &o1;
00454     par.o2 = &o2;
00455     par.rb1 = rb1;
00456     par.rb2 = rb2;
00457     
00458     gsl_multimin_fminimizer *s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex,2);
00459     
00460     gsl_multimin_function moid2rb_function;
00461     
00462     moid2rb_function.f      = &MOID2RB_f;
00463     moid2rb_function.n      = 2;
00464     moid2rb_function.params = &par;
00465     
00466     gsl_vector *x         = gsl_vector_alloc(2);
00467     gsl_vector *step_size = gsl_vector_alloc(2);
00468     
00469     // gsl_vector_set(step_size,0,pi);
00470     // gsl_vector_set(step_size,1,pi);
00471     
00472     MOID2RB_solution best_solution;
00473     
00474     int gsl_solutions_iter=0,max_gsl_solutions_iter=7;
00475     while (gsl_solutions_iter<max_gsl_solutions_iter) {
00476       
00477       gsl_vector_set(x,0,fmod(10*twopi+(twopi/(2*max_gsl_solutions_iter))*gsl_solutions_iter   -o1.omega_node-o1.omega_pericenter,twopi));
00478       gsl_vector_set(x,1,fmod(10*twopi+(twopi/(2*max_gsl_solutions_iter))*gsl_solutions_iter+pi-o2.omega_node-o2.omega_pericenter,twopi));
00479       
00480       gsl_vector_set(step_size,0,pi);
00481       gsl_vector_set(step_size,1,pi);
00482       
00483       gsl_multimin_fminimizer_set (s, &moid2rb_function, x, step_size);
00484       
00485       size_t iter = 0;
00486       int status;
00487       do {
00488         
00489         iter++;
00490         
00491         // iter status
00492         status = gsl_multimin_fminimizer_iterate(s);
00493         
00494         // convergence status
00495         status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),1.0e-6);
00496         
00497         // if (status == GSL_SUCCESS) printf ("Minimum found at:\n");
00498         
00499         /* printf ("%5d %.5f %.5f %10.5f\n", iter,
00500            (180/pi)*gsl_vector_get (s->x, 0), 
00501            (180/pi)*gsl_vector_get (s->x, 1), 
00502            gsl_multimin_fminimizer_minimum(s));
00503         */
00504         
00505         // } while (status == GSL_CONTINUE && iter < 500);
00506       } while (status == GSL_CONTINUE && iter < 200);
00507       
00508       // const double moid2rb = gsl_multimin_fminimizer_minimum(s);
00509       
00510       // cerr << "MOID2RB iters: " << iter << endl;
00511       
00512       if (status == GSL_SUCCESS) {
00513         best_solution.Insert(gsl_multimin_fminimizer_minimum(s),gsl_vector_get(s->x,0),gsl_vector_get(s->x,1));
00514       } 
00515       
00516       ++gsl_solutions_iter;
00517     }
00518     
00519     const double moid2rb = best_solution.moid2rb();
00520     o1.M              = best_solution.M1();
00521     o2.M              = best_solution.M2();
00522     Vector v1, v2;
00523     // update r1 and r2 for output
00524     o1.RelativePosVel(r1,v1);
00525     o2.RelativePosVel(r2,v2);
00526     // not here...
00527     // r1 += rb1;
00528     // r2 += rb2;
00529     
00530     /*
00531       printf ("%5d %.5f %.5f %.5f %.5f %10.5f\n", iter,
00532       (180/pi)*gsl_vector_get (s->x, 0), (180/pi)*o1.M,
00533       (180/pi)*gsl_vector_get (s->x, 1), (180/pi)*o2.M,
00534       gsl_multimin_fminimizer_minimum(s));
00535     */
00536     
00537     // free
00538     gsl_multimin_fminimizer_free(s);
00539     gsl_vector_free(x);
00540     gsl_vector_free(step_size);
00541     
00542     return moid2rb;
00543   }
00544   
00545   // Simulated Annealing tests
00546   
00547   /* how many points do we try before stepping */
00548 #define N_TRIES  10           
00549   
00550   /* how many iterations for each T? */
00551 #define ITERS_FIXED_T 3
00552   
00553   /* max step size in random walk */
00554 #define STEP_SIZE 1.0
00555   
00556   /* Boltzmann constant */
00557 #define K        1.0e5             
00558   
00559   /* initial temperature */
00560 #define T_INITIAL 0.01
00561   
00562   /* damping factor for temperature */
00563 #define MU_T  1.005             
00564 #define T_MIN 1.0e-6
00565   
00566   const gsl_siman_params_t params = {N_TRIES, ITERS_FIXED_T, STEP_SIZE, K, T_INITIAL, MU_T, T_MIN};
00567   
00568   struct data {
00569     OrbitWithEpoch      *orbit;
00570     vector<Observation> *obs;
00571   };
00572   
00573   double E1(void *p) {
00574     data *d = (data*) p;
00575     return (RMS_residuals(*d->obs,*d->orbit));
00576   }
00577   
00578   // metric coefficients
00579   const double d_a                = 0.1; // il comando FromUnits(0.1,AU) pare dare problemi...
00580   const double d_e                = 0.01;
00581   const double d_i                = 0.1*(pi/180);
00582   const double d_omega_node       = 1.0*(pi/180);
00583   const double d_omega_pericenter = 1.0*(pi/180);
00584   const double d_M                = 5.0*(pi/180);
00585   //
00586   const double c_a                = secure_pow(1.0/d_a,                2);
00587   const double c_e                = secure_pow(1.0/d_e,                2);
00588   const double c_i                = secure_pow(1.0/d_i,                2);
00589   const double c_omega_node       = secure_pow(1.0/d_omega_node,       2);
00590   const double c_omega_pericenter = secure_pow(1.0/d_omega_pericenter, 2);
00591   const double c_M                = secure_pow(1.0/d_M,                2);
00592   
00593   double M1(void *p1, void *p2) {
00594     data *d1 = (data*) p1;
00595     data *d2 = (data*) p2;
00596     
00597     double dist = sqrt( c_a                * secure_pow( d1->orbit->a                - d2->orbit->a,               2) +
00598                         c_e                * secure_pow( d1->orbit->e                - d2->orbit->e,               2) +
00599                         c_i                * secure_pow( d1->orbit->i                - d2->orbit->i,               2) +
00600                         c_omega_node       * secure_pow( d1->orbit->omega_node       - d2->orbit->omega_node,      2) +
00601                         c_omega_pericenter * secure_pow( d1->orbit->omega_pericenter - d2->orbit->omega_pericenter,2) +
00602                         c_M                * secure_pow( d1->orbit->M                - d2->orbit->M,               2) ) / sqrt(6.0);
00603     
00604     std::cerr << "M1: " << dist << endl;
00605     
00606     return dist;
00607   }
00608   
00609   void S1(const gsl_rng *r, void *p, double step_size) {
00610     
00611     // double u = gsl_rng_uniform(r);
00612     // new_x = u * 2 * step_size - step_size + old_x;
00613     
00614     // memcpy(xp, &new_x, sizeof(new_x));
00615     
00616     data *d = (data*) p;
00617     
00618     // cerr << "d_a = " << d_a << endl;
00619     
00620     /* 
00621        data old_d;
00622        Orbit o = *d->orbit; old_d.orbit = &o;
00623        vector<Observation> obs = *d->obs; old_d.obs = &obs;
00624     */
00625     
00626     /* 
00627        double u, total = step_size;
00628        u = 0.40*total*(gsl_rng_uniform(r)-0.5); total -= u; new_d.orbit->a += u;
00629        u = 0.05*total*(gsl_rng_uniform(r)-0.5); total -= u; new_d.orbit->e += u;
00630        u = 0.50*total*(gsl_rng_uniform(r)-0.5); total -= u; new_d.orbit->i += u;
00631        u = 1.00*total*(gsl_rng_uniform(r)-0.5); total -= u; new_d.orbit->omega_node += u;
00632        u = 1.00*total*(gsl_rng_uniform(r)-0.5); total -= u; new_d.orbit->omega_pericenter += u; 
00633        u = 1.00*total*(gsl_rng_uniform(r)-0.5); total -= u; new_d.orbit->M += u;  
00634     */
00635     
00636     // cerr << "requested step size: " << step_size << endl;
00637     
00638     d->orbit->a                += step_size*(gsl_rng_uniform(r)-0.5)*d_a;
00639     d->orbit->e                += step_size*(gsl_rng_uniform(r)-0.5)*d_e;
00640     d->orbit->i                += step_size*(gsl_rng_uniform(r)-0.5)*d_i;
00641     d->orbit->omega_node       += step_size*(gsl_rng_uniform(r)-0.5)*d_omega_node;
00642     d->orbit->omega_pericenter += step_size*(gsl_rng_uniform(r)-0.5)*d_omega_pericenter;
00643     d->orbit->M                += step_size*(gsl_rng_uniform(r)-0.5)*d_M;
00644     
00645     // checks
00646     while (d->orbit->a < 0) d->orbit->a += 0.1*step_size*gsl_rng_uniform(r)*d_a;
00647     while (d->orbit->e < 0) d->orbit->e += 0.1*step_size*gsl_rng_uniform(r)*d_e;
00648     //
00649     do { d->orbit->i                = fmod(d->orbit->i+pi,pi);                      } while (d->orbit->i < 0);
00650     do { d->orbit->omega_node       = fmod(d->orbit->omega_node+twopi,twopi);       } while (d->orbit->omega_node < 0);
00651     do { d->orbit->omega_pericenter = fmod(d->orbit->omega_pericenter+twopi,twopi); } while (d->orbit->omega_pericenter < 0);
00652     do { d->orbit->M                = fmod(d->orbit->M+twopi,twopi);                } while (d->orbit->M < 0);
00653     
00654     // cerr << "proposed step size: " << M1(p,(void*)(&old_d)) << endl;
00655     
00656     // *(d->orbit) = *(new_d.orbit);
00657     // *(d->obs)   = *(new_d.obs);
00658     
00659   }
00660   
00661   /* 
00662      void P1(void *p) {
00663      data *d = (data*) p;
00664      printf(" [ %g %g %g ] ",d->orbit->a,d->orbit->e,d->orbit->i*(180/pi));
00665      }
00666   */
00667   
00668   /* 
00669      void OrbitSimulatedAnnealing(OrbitWithEpoch &orbit, vector<Observation> &obs) {
00670      
00671      // gsl_rng_env_setup();
00672      
00673      gsl_rng *r = gsl_rng_alloc(gsl_rng_gfsr4);
00674      
00675      data d;
00676      d.orbit = &orbit;
00677      d.obs   = &obs;
00678      
00679      gsl_siman_solve(r, &d, E1, S1, M1, P1,
00680      NULL, NULL, NULL, 
00681      sizeof(data), params);
00682      
00683      gsl_rng_free(r);
00684      }
00685   */
00686   
00687   // 
00688   
00689   /* 
00690      class par_class {
00691      public:
00692      UniverseTypeAwareTime epoch;
00693      vector<Observation>  *obs;
00694      };
00695   */
00696   
00697   /* 
00698      double OrbDiffCorr_f (const gsl_vector *v, void *params) {
00699      
00700      OrbitWithEpoch orbit;
00701      
00702      orbit.a                = gsl_vector_get(v,0);
00703      orbit.e                = gsl_vector_get(v,1);
00704      orbit.i                = gsl_vector_get(v,2);
00705      orbit.omega_node       = gsl_vector_get(v,3);
00706      orbit.omega_pericenter = gsl_vector_get(v,4);
00707      orbit.M                = gsl_vector_get(v,5);
00708      
00709      // orbit.ref_body = Body("Sun",GetMSun(),Vector(0,0,0),Vector(0,0,0));
00710      orbit.mu       = GetG()*GetMSun();
00711      
00712      par_class *par = (par_class*) params;
00713      
00714      orbit.epoch = par->epoch;
00715      
00716      vector<Observation> *obs = par->obs;
00717      
00718      // cerr << "f: obs->size() = " << obs->size() << endl;
00719      
00720      const double rms = RMS_residuals(*obs,orbit);
00721      
00722      // cerr << "f: RMS = " << rms << endl;
00723      
00724      return (rms);
00725      
00726      }
00727   */
00728   
00729   /* 
00730      void OrbitDifferentialCorrectionsMultimin(OrbitWithEpoch &orbit, vector<Observation> &obs) {
00731      
00732      par_class par;
00733      
00734      par.epoch = orbit.epoch;
00735      par.obs   = &obs;
00736      
00737      gsl_multimin_fminimizer *s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex,6);
00738      
00739      gsl_multimin_function diff_corr_function;
00740      
00741      diff_corr_function.f      = &OrbDiffCorr_f;
00742      diff_corr_function.n      = 6;
00743      diff_corr_function.params = &par;
00744      
00745      gsl_vector *x = gsl_vector_alloc(6);
00746      
00747      gsl_vector_set(x,0,orbit.a);
00748      gsl_vector_set(x,1,orbit.e);
00749      gsl_vector_set(x,2,orbit.i);
00750      gsl_vector_set(x,3,orbit.omega_node);
00751      gsl_vector_set(x,4,orbit.omega_pericenter);
00752      gsl_vector_set(x,5,orbit.M);
00753      
00754      gsl_vector *step_size = gsl_vector_alloc(6);
00755      
00756      gsl_vector_set(step_size,0, 10.0);
00757      gsl_vector_set(step_size,1,  1.0);
00758      gsl_vector_set(step_size,2, 10.0);
00759      gsl_vector_set(step_size,3,100.0);
00760      gsl_vector_set(step_size,4,100.0);
00761      gsl_vector_set(step_size,5,100.0);
00762      
00763      gsl_multimin_fminimizer_set (s, &diff_corr_function, x, step_size);
00764      
00765      size_t iter = 0;
00766      int status;
00767      do {
00768      
00769      iter++;
00770      
00771      status = gsl_multimin_fminimizer_iterate(s);
00772      
00773      if (status) break;
00774      
00775      status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),1.0e-6);
00776      
00777      if (status == GSL_SUCCESS) printf ("Minimum found at:\n");
00778      
00779      printf ("%5d %.5f %.5f %.5f %10.5f\n", iter,
00780      gsl_vector_get (s->x, 0), 
00781      gsl_vector_get (s->x, 1), 
00782      gsl_vector_get (s->x, 2)*(180/pi), 
00783      // s->f);
00784      gsl_multimin_fminimizer_minimum(s));
00785      
00786      } while (status == GSL_CONTINUE && iter < 1000);
00787      
00788      // update orbit?
00789      orbit.a                = gsl_vector_get (s->x,0);
00790      orbit.e                = gsl_vector_get (s->x,1);
00791      orbit.i                = gsl_vector_get (s->x,2);
00792      orbit.omega_node       = gsl_vector_get (s->x,3);
00793      orbit.omega_pericenter = gsl_vector_get (s->x,4);
00794      orbit.M                = gsl_vector_get (s->x,5);
00795      
00796      // free
00797      gsl_multimin_fminimizer_free (s);
00798      gsl_vector_free (x);
00799      gsl_vector_free (step_size);
00800      
00801      }
00802   */
00803   
00804   // least sq.
00805   
00806   class par_class {
00807   public:
00808     vector<Observation> * obs;
00809   public:
00810     // IMPORTANT!
00811     // The orbit epoch must be placed here (as well as mu, if it can be
00812     // different from G*Msun) because is a constant part of the orbit.
00813     Date orbit_epoch;
00814     
00815     // another test, with the covariance matrix,
00816     // to have an estimate of the error on each orbial element
00817     gsl_matrix * covar;
00818     bool use_covar;
00819   };
00820   
00821   class least_sq_diff_par_class {
00822   public:
00823     OrbitWithEpoch orbit;
00824     Observation obs;
00825     int var_index;
00826   };
00827   
00828   int least_sq_f (const gsl_vector * v, void * params, gsl_vector * f) {
00829     
00830     OrbitWithEpoch orbit;
00831     
00832     orbit.a                = gsl_vector_get(v,0);
00833     orbit.e                = gsl_vector_get(v,1);
00834     orbit.i                = gsl_vector_get(v,2);
00835     orbit.omega_node       = gsl_vector_get(v,3);
00836     orbit.omega_pericenter = gsl_vector_get(v,4);
00837     orbit.M                = gsl_vector_get(v,5);
00838     
00839     {
00840       ORSA_DEBUG(
00841               "least_sq_f():\n"
00842               "orbit.a = %g\n"
00843               "orbit.e = %g\n"
00844               "orbit.i = %g\n",
00845               orbit.a,orbit.e,orbit.i*(180/pi));
00846     }
00847     
00848     orbit.mu = GetG()*GetMSun();
00849     
00850     par_class * par = (par_class *) params;
00851     
00852     vector<Observation> * obs = par->obs;
00853     
00854     orbit.epoch = par->orbit_epoch;
00855     
00856     OptimizedOrbitPositions opt(orbit);
00857     
00858     size_t i;
00859     double arcsec;
00860     for (i=0;i<obs->size();++i) {
00861       arcsec = opt.PropagatedSky_J2000((*obs)[i].date,(*obs)[i].obscode).delta_arcsec((*obs)[i]);
00862       gsl_vector_set(f,i,arcsec);
00863       
00864       ORSA_ERROR("f[%02i] = %g",i,arcsec);
00865     }
00866     
00867     return GSL_SUCCESS;
00868   }
00869   
00870   double least_sq_diff_f(double x, void * params) {
00871     
00872     least_sq_diff_par_class * diff_par = (least_sq_diff_par_class *) params;
00873     
00874     OrbitWithEpoch orbit(diff_par->orbit);
00875     
00876     switch(diff_par->var_index) {
00877     case 0: orbit.a                = x; break;
00878     case 1: orbit.e                = x; break;
00879     case 2: orbit.i                = x; break;
00880     case 3: orbit.omega_node       = x; break;
00881     case 4: orbit.omega_pericenter = x; break;
00882     case 5: orbit.M                = x; break;
00883     }
00884     
00885     /* 
00886        ORSA_ERROR("least_sq_diff_f(...):   diff_par->var_index = %i   x = %26.20f",diff_par->var_index,x);
00887     */
00888     
00889     return (PropagatedSky_J2000(orbit,diff_par->obs.date,diff_par->obs.obscode).delta_arcsec(diff_par->obs));
00890   }
00891   
00892   int least_sq_df(const gsl_vector * v, void * params, gsl_matrix * J) {
00893     
00894     OrbitWithEpoch orbit;
00895     
00896     orbit.a                = gsl_vector_get(v,0);
00897     orbit.e                = gsl_vector_get(v,1);
00898     orbit.i                = gsl_vector_get(v,2);
00899     orbit.omega_node       = gsl_vector_get(v,3);
00900     orbit.omega_pericenter = gsl_vector_get(v,4);
00901     orbit.M                = gsl_vector_get(v,5);
00902     
00903     orbit.mu = GetG()*GetMSun();
00904     
00905     par_class * par = (par_class*) params;
00906     
00907     vector<Observation> * obs = par->obs;
00908     
00909     orbit.epoch = par->orbit_epoch;
00910     
00911     least_sq_diff_par_class diff_par;
00912     
00913     diff_par.orbit = orbit;
00914     
00915     gsl_function diff_f;
00916     double result, abserr;
00917     
00918     diff_f.function = &least_sq_diff_f;
00919     diff_f.params   = &diff_par;
00920     
00921     size_t i,j;
00922     for (i=0;i<obs->size();++i) {
00923       diff_par.obs = (*obs)[i];
00924       for (j=0;j<6;++j) {
00925         diff_par.var_index = j;
00926         
00927         /* 
00928            // gsl_diff_central(&diff_f, gsl_vector_get(v,j), &result, &abserr);
00929            gsl_deriv_central(&diff_f, gsl_vector_get(v,j), 1.0e-6, &result, &abserr);
00930            gsl_matrix_set (J, i, j, result);  
00931            //
00932            fprintf(stderr,"[-6] diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
00933            i,j,result,abserr,gsl_vector_get(v,j));
00934            
00935            
00936            
00937            // gsl_diff_central(&diff_f, gsl_vector_get(v,j), &result, &abserr);
00938            gsl_deriv_central(&diff_f, gsl_vector_get(v,j), 1.0e-8, &result, &abserr);
00939            gsl_matrix_set (J, i, j, result);  
00940            //
00941            fprintf(stderr,"[-8] diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
00942            i,j,result,abserr,gsl_vector_get(v,j));
00943            
00944            
00945            
00946            // gsl_diff_central(&diff_f, gsl_vector_get(v,j), &result, &abserr);
00947            gsl_deriv_central(&diff_f, gsl_vector_get(v,j), 1.0e-9, &result, &abserr);
00948            gsl_matrix_set (J, i, j, result);  
00949            //
00950            fprintf(stderr,"[-9] diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
00951            i,j,result,abserr,gsl_vector_get(v,j));
00952            
00953            // gsl_diff_central(&diff_f, gsl_vector_get(v,j), &result, &abserr);
00954            gsl_deriv_central(&diff_f, gsl_vector_get(v,j), 1.0e-12, &result, &abserr);
00955            gsl_matrix_set (J, i, j, result);  
00956            //
00957            fprintf(stderr,"[-12]diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
00958            i,j,result,abserr,gsl_vector_get(v,j));
00959         */
00960         
00961         /* 
00962            // gsl_diff_central(&diff_f, gsl_vector_get(v,j), &result, &abserr);
00963            gsl_deriv_central(&diff_f, gsl_vector_get(v,j), 1.0e-13, &result, &abserr);
00964            gsl_matrix_set (J, i, j, result);  
00965            //
00966            fprintf(stderr,"[-13]diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
00967            i,j,result,abserr,gsl_vector_get(v,j));
00968            
00969            
00970            // gsl_diff_central(&diff_f, gsl_vector_get(v,j), &result, &abserr);
00971            gsl_deriv_central(&diff_f, gsl_vector_get(v,j), 1.0e-14, &result, &abserr);
00972            gsl_matrix_set (J, i, j, result);  
00973            //
00974            fprintf(stderr,"[-14]diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
00975            i,j,result,abserr,gsl_vector_get(v,j));
00976         */
00977         
00978         /* 
00979            // a smarter one? (with numeric limits)
00980            // gsl_diff_central(&diff_f, gsl_vector_get(v,j), &result, &abserr);
00981            gsl_deriv_central(&diff_f, gsl_vector_get(v,j), gsl_vector_get(v,j)*100.0*std::numeric_limits<double>::epsilon(), &result, &abserr);
00982            gsl_matrix_set (J, i, j, result);  
00983            //
00984            fprintf(stderr,"[lim]diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
00985            i,j,result,abserr,gsl_vector_get(v,j));
00986         */
00987         
00988         /* 
00989            if (par->use_covar) {
00990            
00991            cerr << "cov: j=" << j << " cov: " << sqrt(gsl_matrix_get(par->covar,j,j)) << endl;
00992            
00993            // again, but using the covar matrix to have an estimate of the step
00994            gsl_deriv_central(&diff_f, gsl_vector_get(v,j), 0.001*sqrt(gsl_matrix_get(par->covar,j,j)), &result, &abserr);
00995            gsl_matrix_set (J, i, j, result);  
00996            //
00997            fprintf(stderr,"[cov]diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
00998            i,j,result,abserr,gsl_vector_get(v,j));
00999            } else {
01000         */
01001         
01002         gsl_deriv_central(&diff_f, gsl_vector_get(v,j), gsl_vector_get(v,j)*1.0e4*std::numeric_limits<double>::epsilon(), &result, &abserr);
01003         gsl_matrix_set (J, i, j, result);  
01004         //
01005         fprintf(stderr,"[lim]diff[%03i][%i] = %20f +/- %20f at %20.12f\n",
01006                 i,j,result,abserr,gsl_vector_get(v,j));
01007         
01008         // }
01009         
01010       }
01011     }
01012     
01013     return GSL_SUCCESS;
01014   }
01015   
01016   int least_sq_fdf (const gsl_vector *v, void *params, gsl_vector *f, gsl_matrix *J) {
01017     least_sq_f (v,params,f);
01018     least_sq_df(v,params,J);
01019     return GSL_SUCCESS;
01020   }
01021   
01022   // void OrbitDifferentialCorrectionsLeastSquares(OrbitWithEpoch &orbit, const vector<Observation> &obs) {
01023   void OrbitDifferentialCorrectionsLeastSquares(OrbitWithCovarianceMatrixGSL &orbit, const vector<Observation> &obs) {
01024     
01025     cerr << "inside odcls..." << endl;
01026     
01027     if (obs.size() < 6) {
01028       cerr << "at least 6 observations are needed for OrbitDifferentialCorrectionsLeastSquares..." << endl;
01029       return;
01030     }
01031     
01032     par_class par;
01033     
01034     par.orbit_epoch = orbit.epoch;
01035     
01036     par.obs = const_cast< vector<Observation>* > (&obs);
01037     
01038     par.covar = gsl_matrix_alloc(6,6);
01039     par.use_covar = false;
01040     
01041     /* 
01042        { // check par.obs
01043        cerr << "par.obs->size()=" << par.obs->size() << endl;
01044        int y,m,d;
01045        for (unsigned int k=0;k<par.obs->size();++k) {
01046        (*par.obs)[k].date.GetGregor(y,m,d);
01047        cerr << "(*par.obs)[" << k << "] date: " << y << " " << m << " " << d << "  t: " << (*par.obs)[k].date.GetJulian() << endl;
01048        }
01049        }
01050     */
01051     
01052     gsl_multifit_fdfsolver * s = gsl_multifit_fdfsolver_alloc(gsl_multifit_fdfsolver_lmsder, obs.size(), 6);
01053     // gsl_multifit_fdfsolver * s = gsl_multifit_fdfsolver_alloc(gsl_multifit_fdfsolver_lmder, obs.size(), 6);
01054     
01055     gsl_multifit_function_fdf least_sq_function;
01056     
01057     least_sq_function.f      = &least_sq_f;
01058     least_sq_function.df     = &least_sq_df;
01059     least_sq_function.fdf    = &least_sq_fdf;
01060     least_sq_function.n      = obs.size();
01061     least_sq_function.p      = 6;
01062     least_sq_function.params = &par;
01063     
01064     gsl_vector * x = gsl_vector_alloc(6);
01065     
01066     gsl_vector_set(x, 0, orbit.a);
01067     gsl_vector_set(x, 1, orbit.e);
01068     gsl_vector_set(x, 2, orbit.i);
01069     gsl_vector_set(x, 3, orbit.omega_node);
01070     gsl_vector_set(x, 4, orbit.omega_pericenter);
01071     gsl_vector_set(x, 5, orbit.M);
01072     
01073     gsl_multifit_fdfsolver_set(s,&least_sq_function,x);
01074     
01075     size_t iter = 0;
01076     int status;
01077     do {
01078       
01079       ++iter;
01080       
01081       status = gsl_multifit_fdfsolver_iterate(s);
01082       
01083       printf("itaration status = %s\n",gsl_strerror(status));
01084       
01085       /* 
01086          printf ("iter: %3u  %15.8f  %15.8f  %15.8f   |f(x)| = %g\n",
01087          iter,
01088          gsl_vector_get (s->x, 0), 
01089          gsl_vector_get (s->x, 1),
01090          gsl_vector_get (s->x, 2), 
01091          gsl_blas_dnrm2 (s->f));
01092       */
01093       
01094       // status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 1e-4);      
01095       // status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 1e-5);      
01096       // status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 1e-6);
01097       status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 1.0e-8);
01098       // status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 1e-8);
01099       // status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 1e-9);
01100       
01101       printf("convergence status = %s\n",gsl_strerror(status));
01102       
01103       { 
01104         gsl_matrix * covar = gsl_matrix_alloc(6,6);
01105         gsl_multifit_covar(s->J,0.0,covar);
01106         printf ("more info:\n"
01107                 "a    = %12.6f +/- %12.6f\n"
01108                 "e    = %12.6f +/- %12.6f\n"
01109                 "i    = %12.6f +/- %12.6f\n"
01110                 "\n",
01111                 gsl_vector_get(s->x,0),sqrt(gsl_matrix_get(covar,0,0)),
01112                 gsl_vector_get(s->x,1),sqrt(gsl_matrix_get(covar,1,1)),
01113                 gsl_vector_get(s->x,2)*(180/pi),sqrt(gsl_matrix_get(covar,2,2))*(180/pi));
01114       }
01115       
01116       gsl_multifit_covar(s->J,0.0,par.covar);
01117       par.use_covar = true;
01118       
01119     } while ((status == GSL_CONTINUE) && (iter < 1024));
01120     
01121     gsl_matrix * covar = gsl_matrix_alloc(6,6);
01122     gsl_multifit_covar(s->J,0.0,covar);
01123     // gsl_matrix_fprintf(stdout, covar,"%g");
01124     
01125 #define FIT(i) gsl_vector_get(s->x, i)
01126 #define ERR(i) sqrt(gsl_matrix_get(covar,i,i))
01127     
01128     printf("a    = %12.6f +/- %12.6f\n", FIT(0), ERR(0));
01129     printf("e    = %12.6f +/- %12.6f\n", FIT(1), ERR(1));
01130     printf("i    = %12.6f +/- %12.6f\n", FIT(2)*(180/pi), ERR(2)*(180/pi));
01131     printf("node = %12.6f +/- %12.6f\n", FIT(3)*(180/pi), ERR(3)*(180/pi));
01132     printf("peri = %12.6f +/- %12.6f\n", FIT(4)*(180/pi), ERR(4)*(180/pi));
01133     printf("M    = %12.6f +/- %12.6f\n", FIT(5)*(180/pi), ERR(5)*(180/pi));
01134     
01135     printf ("status = %s\n", gsl_strerror (status));
01136     
01137     // update orbit!
01138     orbit.a                = gsl_vector_get (s->x,0);
01139     orbit.e                = gsl_vector_get (s->x,1);
01140     orbit.i                = gsl_vector_get (s->x,2);
01141     orbit.omega_node       = gsl_vector_get (s->x,3);
01142     orbit.omega_pericenter = gsl_vector_get (s->x,4);
01143     orbit.M                = gsl_vector_get (s->x,5);
01144     
01145     // set covariance matrix
01146     {
01147       double covm[6][6];
01148       unsigned int i,j;
01149       for (i=0;i<6;++i) {
01150         for (j=0;j<6;++j) {
01151           covm[i][j] = gsl_matrix_get(covar,i,j);
01152         }
01153       }
01154       orbit.SetCovarianceMatrix((double**)covm,Osculating);
01155     }
01156     
01157     // print errors and RMS.
01158     {
01159       OptimizedOrbitPositions opt(orbit);
01160       double arcsec, rms=0.0;
01161       for (unsigned int k=0;k<obs.size();++k) {
01162         arcsec = opt.PropagatedSky_J2000(obs[k].date,obs[k].obscode).delta_arcsec(obs[k]);
01163         rms += secure_pow(arcsec,2);
01164         fprintf(stderr,"delta_arcsec obs[%02i]: %g (%+.2f,%+.2f)\n",k,arcsec,
01165                 (opt.PropagatedSky_J2000(obs[k].date,obs[k].obscode).ra().GetRad() -obs[k].ra.GetRad() )*(180/pi)*3600.0,
01166                 (opt.PropagatedSky_J2000(obs[k].date,obs[k].obscode).dec().GetRad()-obs[k].dec.GetRad())*(180/pi)*3600.0);
01167       }
01168       rms = sqrt(rms/obs.size());
01169       fprintf(stderr,"\n RMS: %g\n\n",rms);
01170     }
01171     
01172     // free
01173     gsl_multifit_fdfsolver_free (s);
01174     gsl_vector_free (x);
01175     
01176     cerr << "out of odcls..." << endl;
01177   }
01178   
01179   //
01180   
01181   // gauss tool: search optimal velocity in the central point
01182   
01183   class g_v_class {
01184   public:
01185     vector<Observation> obs; // three observations
01186     Vector r; // position vector vector given at the epoch of the first observation (first in the obs vector...)
01187   };
01188   
01189   int gauss_v_f (const gsl_vector *v, void * params, gsl_vector *f) {
01190     
01191     Vector velocity;
01192     //
01193     velocity.x = gsl_vector_get(v,0);
01194     velocity.y = gsl_vector_get(v,1);
01195     velocity.z = gsl_vector_get(v,2);
01196     
01197     g_v_class *par = (g_v_class*) params;
01198     
01199     vector<Observation> &obs(par->obs);
01200     
01201     // cerr << "Length(r): " << (par->r).Length()   << endl;
01202     // cerr << "Length(v): " << (velocity).Length() << endl;
01203     
01204     OrbitWithEpoch orbit;
01205     orbit.Compute(par->r,velocity,GetG()*GetMSun(),obs[0].date);
01206     // orbit.epoch = obs[1].date;
01207     // orbit.epoch = obs[0].date;
01208     
01209     {
01210       // debug
01211       /* cerr << "gauss_v_f() ---> orbit.a = " << orbit.a << endl;
01212          cerr << "gauss_v_f() ---> orbit.e = " << orbit.e << endl;
01213          cerr << "gauss_v_f() ---> orbit.i = " << orbit.i*(180/pi) << endl;
01214       */
01215       ORSA_DEBUG(
01216               "gauss_v_f():\n"
01217               "orbit.a = %g\n"
01218               "orbit.e = %g\n"
01219               "orbit.i = %g\n",
01220               orbit.a,orbit.e,orbit.i*(180/pi));
01221     }
01222     
01223     OptimizedOrbitPositions opt(orbit);
01224     
01225     size_t i;
01226     double arcsec;
01227     for (i=0;i<obs.size();++i) {
01228       arcsec = opt.PropagatedSky_J2000(obs[i].date,obs[i].obscode).delta_arcsec(obs[i]);
01229       gsl_vector_set(f,i,arcsec);
01230       // cerr << "f[" << i << "] = " << arcsec << endl;
01231     }
01232     
01233     return GSL_SUCCESS;
01234   }
01235   
01236   class gauss_v_diff_par_class {
01237   public:
01238     Vector r;
01239     Vector velocity;
01240     Observation obs;
01241     int var_index;
01242     Date orbit_epoch;
01243   };
01244   
01245   double gauss_v_diff_f(double x, void *params) {
01246     
01247     // cerr << "gauss_v_diff_f()..." << endl;
01248     
01249     gauss_v_diff_par_class *diff_par = (gauss_v_diff_par_class*) params;
01250     
01251     // cerr << "gauss_v_diff_f()... var_index = " << diff_par->var_index << endl;
01252     
01253     Vector velocity(diff_par->velocity);
01254     
01255     switch(diff_par->var_index) {
01256     case 0: velocity.x = x; break;
01257     case 1: velocity.y = x; break;
01258     case 2: velocity.z = x; break;
01259     }
01260     
01261     OrbitWithEpoch orbit;
01262     orbit.Compute(diff_par->r,velocity,GetG()*GetMSun(),diff_par->orbit_epoch);
01263     // orbit.epoch = diff_par->orbit_epoch;
01264     
01265     return (PropagatedSky_J2000(orbit,diff_par->obs.date,diff_par->obs.obscode).delta_arcsec(diff_par->obs));
01266   }
01267   
01268   int gauss_v_df (const gsl_vector *v, void *params, gsl_matrix *J) {
01269     
01270     Vector velocity;
01271     //
01272     velocity.x = gsl_vector_get(v,0);
01273     velocity.y = gsl_vector_get(v,1);
01274     velocity.z = gsl_vector_get(v,2);
01275     
01276     g_v_class *par = (g_v_class*) params;
01277     
01278     vector<Observation> &obs(par->obs);
01279     
01280     OrbitWithEpoch orbit;
01281     orbit.Compute(par->r,velocity,GetG()*GetMSun(),obs[0].date);
01282     // orbit.epoch = obs[1].date;
01283     // orbit.epoch = obs[0].date;
01284     
01285     gauss_v_diff_par_class diff_par;
01286     
01287     diff_par.r           = par->r;
01288     diff_par.velocity    = velocity;
01289     // diff_par.orbit_epoch = obs[1].date;
01290     diff_par.orbit_epoch = orbit.epoch;
01291     
01292     gsl_function diff_f;
01293     double result, abserr;
01294     
01295     diff_f.function = &gauss_v_diff_f;
01296     diff_f.params   = &diff_par;
01297     
01298     size_t i,j;
01299     for (i=0;i<obs.size();++i) {
01300       diff_par.obs = obs[i];
01301       for (j=0;j<3;++j) {
01302         diff_par.var_index = j;
01303         // gsl_diff_central (&diff_f, gsl_vector_get(v,j), &result, &abserr);
01304         gsl_deriv_central(&diff_f, gsl_vector_get(v,j), 1.0e-9, &result, &abserr);
01305         gsl_matrix_set (J, i, j, result);  
01306         // cerr << "diff: " << result << " +/- " << abserr << endl;
01307       }
01308     }
01309     
01310     return GSL_SUCCESS; 
01311   }
01312   
01313   int gauss_v_fdf (const gsl_vector *v, void *params, gsl_vector *f, gsl_matrix *J) {
01314     gauss_v_f (v,params,f);
01315     gauss_v_df(v,params,J);
01316     return GSL_SUCCESS;
01317   }
01318   
01319   // r and v are given at the epoch of the first obs, i.e. obs[0]
01320   void gauss_v(const Vector &r, Vector &v, const vector<Observation> &obs) {
01321     
01322     if (obs.size() < 2) {
01323       ORSA_ERROR("gauss_v(...) needs at least two observations...");
01324       return;
01325     }   
01326     
01327     // cerr << "..gauss_v..." << endl;
01328     
01329     g_v_class g_v;
01330     
01331     g_v.obs = obs;
01332     g_v.r   = r;
01333     
01334     gsl_multifit_fdfsolver *s = gsl_multifit_fdfsolver_alloc(gsl_multifit_fdfsolver_lmsder,obs.size(),3);
01335     
01336     gsl_multifit_function_fdf gauss_v_function;
01337     
01338     gauss_v_function.f      = &gauss_v_f;
01339     gauss_v_function.df     = &gauss_v_df;
01340     gauss_v_function.fdf    = &gauss_v_fdf;
01341     gauss_v_function.n      = obs.size();
01342     gauss_v_function.p      = 3;
01343     gauss_v_function.params = &g_v;
01344     
01345     gsl_vector *x = gsl_vector_alloc(3);
01346     
01347     cerr << "..inside, Length(r): " << (r).Length() << endl;
01348     
01349     gsl_vector_set(x,0,v.x);
01350     gsl_vector_set(x,1,v.y);
01351     gsl_vector_set(x,2,v.z);
01352     
01353     gsl_multifit_fdfsolver_set(s,&gauss_v_function,x);
01354     
01355     size_t iter = 0;
01356     int status;
01357     do {
01358       
01359       ++iter;
01360       
01361       status = gsl_multifit_fdfsolver_iterate (s);
01362       
01363       printf ("itaration status = %s\n", gsl_strerror (status));
01364       
01365       /* 
01366          printf ("iter: %3u  %15.8f  %15.8f  %15.8f   |f(x)| = %g\n",
01367          iter,
01368          gsl_vector_get (s->x, 0), 
01369          gsl_vector_get (s->x, 1),
01370          gsl_vector_get (s->x, 2), 
01371          gsl_blas_dnrm2 (s->f));
01372       */
01373       
01374       // status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 1.0e-3);
01375       status = gsl_multifit_test_delta (s->dx, s->x, 0.0, 5.0e-2);
01376       
01377       printf ("convergence status = %s\n", gsl_strerror (status));
01378       
01379     } while ((status == GSL_CONTINUE) && (iter < 10));
01380     
01381     printf ("status = %s\n", gsl_strerror (status));
01382     
01383     // save results
01384     v.x = gsl_vector_get (s->x,0);
01385     v.y = gsl_vector_get (s->x,1);
01386     v.z = gsl_vector_get (s->x,2);
01387     
01388     // free
01389     gsl_multifit_fdfsolver_free (s);
01390     gsl_vector_free (x);
01391   }
01392   
01393   //
01394   
01395   // OrbitWithCovarianceMatrixGSL Compute(const vector<Observation> &obs_in) {
01396   OrbitWithCovarianceMatrixGSL Compute(const vector<Observation> & obs) {
01397     
01398     // vector<Observation> obs(obs_in);
01399     
01400     if (obs.size() < 3) {
01401       ORSA_ERROR("at least three observations are needed.");
01402       OrbitWithCovarianceMatrixGSL o;
01403       return o;
01404     }
01405     
01406     if (universe->GetUniverseType() != Real) {
01407       ORSA_ERROR("trying to compute an orbit using the wrong universe type: return.");
01408       OrbitWithCovarianceMatrixGSL o;
01409       return o;
01410     }   
01411     
01412     // test 
01413     /* 
01414        {
01415        OrbitWithEpoch orbit;
01416        
01417        orbit.a = 1.75;
01418        orbit.e = 0.39705;
01419        orbit.i = 54.52*(pi/180);
01420        orbit.omega_node       = 47.46   *(pi/180);
01421        orbit.omega_pericenter = 95.47   *(pi/180);
01422        orbit.M                = 322.918 *(pi/180);
01423        //
01424        Date date;
01425        date.SetJulian(52950+2400000.5);
01426        orbit.epoch.SetDate(date);
01427        //
01428        OrbitDifferentialCorrectionsLeastSquares(orbit,obs);
01429        exit(0);
01430        }
01431     */
01432     
01433     // needed?
01434     // sort(obs.begin(),obs.end());
01435     
01436     // cerr << "Orbit::Compute() observations: " << obs.size() << endl;
01437     
01438     // NOTE: check the reference system somewhere near here!!
01439     
01440     /* 
01441        if (0) {
01442        unsigned int k=0;
01443        while (k<obs.size()) {
01444        printf("obs. %i: JD = %f\n",k,obs[k].date.GetJulian());
01445        ++k;
01446        }
01447       }
01448     */
01449     
01450     // vector<OrbitWithEpoch> preliminary_orbits;
01451     vector<PreliminaryOrbit> preliminary_orbits;
01452     
01453     if (0) {
01454       vector<Observation> test_obs;
01455       //
01456       test_obs.push_back(obs[0]);
01457       test_obs.push_back(obs[obs.size()/2]);
01458       test_obs.push_back(obs[obs.size()-1]);
01459       //
01460       Compute_TestMethod(test_obs,preliminary_orbits);
01461     }
01462     
01463     if (0) {
01464       Compute_TestMethod(obs,preliminary_orbits);
01465     }
01466     
01467     // yet another test...
01468     if (0) {
01469       vector<Observation> test_obs;
01470       //
01471       test_obs.push_back(obs[0]);
01472       test_obs.push_back(obs[obs.size()/2]);
01473       test_obs.push_back(obs[obs.size()-1]);
01474       //
01475       Compute_Gauss(test_obs,preliminary_orbits);
01476     }
01477     
01478     if (1) {
01479       vector<ThreeObservations> triplet;
01480       triplet.clear();
01481       // BuildObservationTriplets(obs, triplet, FromUnits(2,HOUR));
01482       // BuildObservationTriplets(obs, triplet, FromUnits(20,DAY));
01483       BuildObservationTriplets(obs, triplet, FromUnits(270,DAY));
01484       //
01485       cerr << "triplets: " << triplet.size() << endl;
01486       
01487       unsigned int trial=0;
01488       while (trial<triplet.size()) {
01489         // cerr << "using triplet with weight = " << triplet[trial].Weight() << endl;
01490         //
01491         cerr << "delay 1->2: " << triplet[trial][1].date.GetJulian()-triplet[trial][0].date.GetJulian() << " days" << endl;
01492         cerr << "delay 2->3: " << triplet[trial][2].date.GetJulian()-triplet[trial][1].date.GetJulian() << " days" << endl;
01493         //
01494         
01495         Compute_Gauss(triplet[trial],preliminary_orbits);
01496         // Compute_TestMethod(triplet[trial],preliminary_orbits);
01497         
01498         ++trial;
01499         //
01500         // if (trial > 1) break;
01501         // if (trial > 1) break;
01502         // if (preliminary_orbits.size() > 7) break;    
01503         if (trial > 500) break;
01504         
01505         cerr << "----trial: " << trial << endl;
01506       }
01507       
01508     }   
01509     
01510     // compute rms for each preliminary orbit
01511     { // don't remove THIS!!
01512       unsigned int r=0;
01513       while (r<preliminary_orbits.size()) {
01514         preliminary_orbits[r].ComputeRMS(obs);
01515         printf("%20.12f %20.12f %20.12f %20.12f\n",preliminary_orbits[r].a,preliminary_orbits[r].e,preliminary_orbits[r].i*(180/pi),preliminary_orbits[r].GetRMS());
01516         ++r;
01517       }
01518     }
01519     //
01520     // sort preliminary orbits
01521     sort(preliminary_orbits.begin(),preliminary_orbits.end());
01522     
01523     {
01524       cerr << "total prelim. orbits: " << preliminary_orbits.size() << endl;
01525       unsigned int r=0;
01526       while (r<preliminary_orbits.size()) {
01527         printf("%20.12f %20.12f %20.12f %20.12f\n",preliminary_orbits[r].a,preliminary_orbits[r].e,preliminary_orbits[r].i*(180/pi),preliminary_orbits[r].GetRMS());
01528         ++r;
01529       }
01530     }
01531     
01532     // only the first
01533     OrbitDifferentialCorrectionsLeastSquares(preliminary_orbits[0],obs);
01534     
01535     if (0) {
01536       unsigned int r=0;
01537       // double rms;
01538       while (r<preliminary_orbits.size()) {
01539         // rms = RMS_residuals(obs,preliminary_orbits[r]);
01540         // printf("%20.12f %20.12f %20.12f %20.12f\n",preliminary_orbits[r].a,preliminary_orbits[r].e,preliminary_orbits[r].i*(180/pi),rms);
01541         // if (rms < 1.0*3600) OrbitSimulatedAnnealing(preliminary_orbits[r],obs);
01542         // if (rms < 500.0) OrbitDifferentialCorrectionsMultimin(preliminary_orbits[r],obs);
01543         // if (rms < 200.0) OrbitDifferentialCorrectionsLeastSquares(preliminary_orbits[r],obs);
01544         
01545         OrbitDifferentialCorrectionsLeastSquares(preliminary_orbits[r],obs);
01546         
01547         // test...
01548         if (0) {
01549           
01550           OptimizedOrbitPositions opt(preliminary_orbits[r]);
01551           
01552           vector<Observation> good_obs;
01553           unsigned int k;
01554           for (k=0;k<obs.size();++k) {
01555             // if (PropagatedSky(preliminary_orbits[r],obs[k].date,obs[k].obscode).delta_arcsec(obs[k]) < 60.0) { // 5.0
01556             // if (PropagatedSky(preliminary_orbits[r],obs[k].date,obs[k].obscode).delta_arcsec(obs[k]) < 60.0) { // 5.0
01557             if (opt.PropagatedSky_J2000(obs[k].date,obs[k].obscode).delta_arcsec(obs[k]) < 300.0) { // 5.0, 60.0
01558               good_obs.push_back(obs[k]);
01559             }
01560             if (good_obs.size()>12) break;
01561           }
01562           
01563           cerr << "good obs.: " << good_obs.size() << endl;
01564           
01565           // improve orbit using 'good observations'
01566           if (good_obs.size() > 3) OrbitDifferentialCorrectionsLeastSquares(preliminary_orbits[r],good_obs);
01567         }  
01568         
01569         ++r;
01570       }
01571       
01572     }
01573     
01574     // WARNING: check on the preliminary_orbits dimension...
01575     // to be refined!
01576     return preliminary_orbits[0];
01577   }
01578   
01579   // gsl stuff
01580   struct poly_8_params {
01581     double coeff_6, coeff_3, coeff_0;
01582   };
01583   
01584   double poly_8     (double x, void *params);
01585   double poly_8_df  (double x, void *params);
01586   void   poly_8_fdf (double x, void *params, double *y, double *dy);
01587   
01588   double poly_8 (double x, void *params) {
01589     struct poly_8_params *p = (struct poly_8_params *) params;
01590     return (secure_pow(x,8) - p->coeff_6*secure_pow(x,6) - p->coeff_3*secure_pow(x,3) - p->coeff_0);
01591   }
01592   
01593   double poly_8_df (double x, void *params) {
01594     struct poly_8_params *p = (struct poly_8_params *) params;
01595     return (8*secure_pow(x,7) - 6*p->coeff_6*secure_pow(x,5) - 3*p->coeff_3*secure_pow(x,2));
01596   }
01597   
01598   void poly_8_fdf (double x, void *params, double *y, double *dy) {
01599     struct poly_8_params *p = (struct poly_8_params *) params;
01600     *y  = (secure_pow(x,8) - p->coeff_6*secure_pow(x,6) - p->coeff_3*secure_pow(x,3) - p->coeff_0);
01601     *dy = (8*secure_pow(x,7) - 6*p->coeff_6*secure_pow(x,5) - 3*p->coeff_3*secure_pow(x,2));
01602   }
01603   
01604   class poly_8_solution {
01605   public:
01606     double value, error;
01607   };
01608   
01609   void poly_8_gsl_solve(poly_8_params &params, vector<poly_8_solution> &solutions) {
01610     
01611     const int    max_iter = 100;
01612     const double x_start  = FromUnits(0.0,AU);
01613     const double x_incr   = FromUnits(0.2,AU);
01614     
01615     const double nominal_relative_accuracy = 1.0e-5;
01616     
01617     solutions.clear();
01618     
01619     poly_8_solution tmp_solution;
01620     
01621     double x, x0; // this value is not used
01622     int    gsl_status;
01623     // const  gsl_root_fdfsolver_type *T;
01624     
01625     gsl_root_fdfsolver *s;
01626     gsl_function_fdf FDF;
01627     
01628     /* 
01629        cerr << " poly_8_gsl_solve(): params.coeff_6 = " << params->coeff_6 << endl;
01630        cerr << " poly_8_gsl_solve(): params.coeff_3 = " << params->coeff_3 << endl;
01631        cerr << " poly_8_gsl_solve(): params.coeff_0 = " << params->coeff_0 << endl;
01632     */
01633     
01634     FDF.f      = &poly_8;
01635     FDF.df     = &poly_8_df;
01636     FDF.fdf    = &poly_8_fdf;
01637     FDF.params = &params;
01638     
01639     // T = gsl_root_fdfsolver_steffenson;
01640     // s = gsl_root_fdfsolver_alloc (T);
01641     s = gsl_root_fdfsolver_alloc(gsl_root_fdfsolver_steffenson);
01642     
01643     int iter = 0;
01644     while (iter<max_iter) {
01645       
01646       ++iter;
01647       x = x_start+iter*x_incr;
01648       gsl_root_fdfsolver_set (s, &FDF, x);
01649       
01650       int iter_gsl=0;
01651       const int max_iter_gsl = 100;
01652       do {
01653         ++iter_gsl;
01654         gsl_status = gsl_root_fdfsolver_iterate(s);
01655         x0 = x;
01656         x = gsl_root_fdfsolver_root(s);
01657         gsl_status = gsl_root_test_delta (x, x0, nominal_relative_accuracy, 0);
01658         // if (gsl_status == GSL_SUCCESS) printf ("Converged:\n");
01659         // printf ("%5d %10.7f %10.7f\n",iter_gsl, x, x - x0);
01660       } while ((gsl_status == GSL_CONTINUE) && (iter_gsl < max_iter_gsl));
01661       
01662       if (gsl_status == GSL_SUCCESS) {
01663         tmp_solution.value = x;
01664         // gsl_root_fdfsolver_iterate(s); tmp_solution.error = fabs(x-gsl_root_fdfsolver_root(s)); // GSL doc: ...the error can be estimated more accurately by taking the difference between the current iterate and next iterate rather than the previous iterate.
01665         tmp_solution.error = nominal_relative_accuracy;
01666         
01667         unsigned int k=0;
01668         bool duplicate=false;
01669         while (k<solutions.size()) {
01670           if (fabs(solutions[k].value-tmp_solution.value) < (solutions[k].error+tmp_solution.error)) {
01671             duplicate= true;
01672             break;
01673           }
01674           ++k;
01675         }
01676         
01677         if (!duplicate) {
01678           solutions.push_back(tmp_solution);
01679         }
01680       }
01681     }
01682     
01683     gsl_root_fdfsolver_free(s);
01684     
01685     if (0) { 
01686       cerr << "solutions found: " << solutions.size() << endl;
01687       unsigned int k=0;
01688       while (k<solutions.size()) {
01689         cerr << k << ": "  << solutions[k].value << " accuracy: " << solutions[k].error << endl;
01690         ++k;
01691       }
01692     }
01693   }
01694   
01695   void Compute_Gauss(const vector<Observation> & obs_in, vector<PreliminaryOrbit> & preliminary_orbits) {
01696     
01697     cerr << "inside Compute_Gauss(...)" << endl;
01698     
01699     // see A. E. Roy, "Orbital Motion", section 13.4
01700     
01701     PreliminaryOrbit orbit;
01702     
01703     vector<Observation> obs(obs_in);
01704     
01705     if (obs.size() != 3) {
01706       ORSA_ERROR("Compute_Gauss(): three observations needed.");
01707       return;
01708     }
01709     
01710     unsigned int j;
01711     
01712     // TODO: the obs. are sorted in time?
01713     
01714     sort(obs.begin(),obs.end());
01715     
01716     // debug
01717     /* {
01718        int l;
01719        for (l=0;l<3;++l) {
01720        printf("observation %i: julian=%14.5f ra=%g dec=%g\n",l,obs[l].date.GetJulian(),obs[l].ra.GetRad(),obs[l].dec.GetRad());
01721        }
01722        }
01723     */
01724     
01725     double tau[3];
01726     const double sqrt_GM = sqrt(GetG()*GetMSun());
01727     //
01728     tau[2] = sqrt_GM*FromUnits(obs[1].date.GetJulian()-obs[0].date.GetJulian(),DAY);
01729     tau[0] = sqrt_GM*FromUnits(obs[2].date.GetJulian()-obs[1].date.GetJulian(),DAY);
01730     tau[1] = sqrt_GM*FromUnits(obs[2].date.GetJulian()-obs[0].date.GetJulian(),DAY);
01731     
01732     /* 
01733        Config conf;
01734        OrsaConfigFile ocf(&conf);
01735        ocf.Read();
01736        ocf.Close();
01737     */
01738     
01739     Vector r;
01740     Vector R[3]; // heliocentric earth position
01741     {
01742       /* 
01743          Vector tmp_v;
01744          JPLFile jf(config->paths[JPL_EPHEM_FILE]->GetValue().c_str());
01745          for (j=0;j<3;++j) { 
01746          jf.GetEph(obs[j].date,EARTH,SUN,r,tmp_v);
01747          R[j] = r;
01748          }
01749       */
01750       //
01751       for (j=0;j<3;++j) { 
01752         R[j] = jpl_cache->GetJPLBody(EARTH,obs[j].date).position() - jpl_cache->GetJPLBody(SUN  ,obs[j].date).position();
01753       }
01754     }
01755     
01756     // add the observer location to the earth position
01757     {
01758       /* 
01759          LocationFile lf;
01760          lf.SetFileName(config->paths[OBSCODE]->GetValue().c_str());
01761          lf.Read();
01762          lf.Close();
01763       */
01764       for (j=0;j<3;++j) { 
01765         R[j] += location_file->ObsPos(obs[j].obscode,obs[j].date);
01766       }
01767     }
01768     
01769     Vector u_rho[3]; // object direction from the observer (unit vectors)
01770     for (j=0;j<3;++j) { 
01771       u_rho[j].x = cos(obs[j].dec) * cos(obs[j].ra);
01772       u_rho[j].y = cos(obs[j].dec) * sin(obs[j].ra);
01773       u_rho[j].z = sin(obs[j].dec);
01774       
01775       /* 
01776          if (universe->GetReferenceSystem() == ECLIPTIC) {
01777          u_rho[j].rotate(0.0,-obleq(obs[j].date).GetRad(),0.0);
01778          }
01779       */
01780       
01781       /* 
01782          if (universe->GetReferenceSystem() == ECLIPTIC) {
01783          EquatorialToEcliptic_J2000(u_rho[j]);
01784          }
01785       */
01786       
01787       if (universe->GetReferenceSystem() == ECLIPTIC) {
01788         // EquatorialToEcliptic_J2000(u_rho[j]);
01789         Angle obl = obleq_J2000();
01790         u_rho[j].rotate(0.0,-obl.GetRad(),0.0);
01791       }
01792       
01793     }
01794     
01795     Vector cross = Cross(u_rho[0],u_rho[2]);
01796     // const Vector f = cross/cross.Length();
01797     const Vector f = cross.Normalized();
01798     
01799     const double rho_1_f = u_rho[1]*f;
01800       
01801     const double R_0_f = R[0]*f;
01802     const double R_1_f = R[1]*f;
01803     const double R_2_f = R[2]*f;
01804     
01805     const double A = (tau[0]/tau[1]*R_0_f + tau[2]/tau[1]*R_2_f - R_1_f)/rho_1_f;
01806     // const double B = (tau[0]/tau[1]*(secure_pow(tau[1],2)-secure_pow(tau[0],2))*R_0_f + tau[2]/tau[1]*(secure_pow(tau[1],2)-secure_pow(tau[2],2))*R_2_f)/rho_1_f/6.0;
01807     const double B = (tau[0]/tau[1]*(tau[1]*tau[1]-tau[0]*tau[0])*R_0_f + tau[2]/tau[1]*(tau[1]*tau[1]-tau[2]*tau[2])*R_2_f)/rho_1_f/6.0;
01808     
01809     const double Xl_Ym_Zn = R[1]*u_rho[1];
01810     
01811     poly_8_params params;
01812     params.coeff_6 = R[1].LengthSquared() + A*A + 2*A*Xl_Ym_Zn;
01813     params.coeff_3 = 2*A*B + 2*B*Xl_Ym_Zn;
01814     params.coeff_0 = B*B;
01815     vector<poly_8_solution> solutions;
01816     poly_8_gsl_solve(params,solutions);
01817     
01818     if (solutions.size() > 0) {
01819       
01820       Vector rho[3];
01821       Vector r[3];
01822       Vector v; // v[0]
01823       double c[3];
01824       
01825       double tmp_length;
01826       double tmp_value;
01827       unsigned int p;
01828       for (p=0;p<solutions.size();++p) {
01829         
01830         // rho[1] = u_rho[1]*(A+(B/secure_pow(solutions[p].value,3)));
01831         // check
01832         // tmp_length = A + (B/secure_pow(solutions[p].value,3));
01833         tmp_value = solutions[p].value;
01834         tmp_length = A + (B/(tmp_value*tmp_value*tmp_value));
01835         // cerr << "tmp_length: " << tmp_length << endl;
01836         if (tmp_length <= 0.0) {
01837           // cerr << "out..." << endl;
01838           continue;
01839         }
01840         //
01841         rho[1] = u_rho[1]*tmp_length;
01842         
01843         r[1] = R[1] + rho[1];
01844         
01845         // standard relation
01846         for (j=0;j<3;++j) { 
01847           // c[j] = tau[j]/tau[1]*(1+(secure_pow(tau[1],2)-secure_pow(tau[j],2))/(6*secure_pow(r[1].Length(),3)));
01848           c[j] = tau[j]/tau[1]*(1+(tau[1]*tau[1]-tau[j]*tau[j])/(6*secure_pow(r[1].Length(),3)));
01849           // printf("OLD c[%i] = %g\n",j,c[j]);
01850         }
01851         
01852         {
01853           
01854           const Vector v_k = rho[1] - (c[0]*R[0] + c[2]*R[2] - R[1]);
01855           const double   k = v_k.Length();
01856           // const Vector u_k = v_k/v_k.Length();
01857           const Vector u_k = v_k.Normalized();
01858           
01859           const double s02 = u_rho[0]*u_rho[2];
01860           const double s0k = u_rho[0]*u_k;
01861           const double s2k = u_rho[2]*u_k;
01862           
01863           // rho[0] = u_rho[0]*(k*(s0k-s02*s2k)/(1-secure_pow(s02,2)))/c[0];
01864           // tmp_length = (k*(s0k-s02*s2k)/(1-secure_pow(s02,2)))/c[0];
01865           tmp_length = (k*(s0k-s02*s2k)/(1-s02*s02))/c[0];
01866           if (tmp_length <= 0.0) {
01867             // cerr << "out..." << endl;
01868             continue;
01869           }
01870           //
01871           rho[0] = u_rho[0]*tmp_length;
01872           
01873           // rho[2] = u_rho[2]*(k*(s2k-s02*s0k)/(1-secure_pow(s02,2)))/c[2];
01874           // tmp_length = (k*(s2k-s02*s0k)/(1-secure_pow(s02,2)))/c[2];
01875           tmp_length = (k*(s2k-s02*s0k)/(1-s02*s02))/c[2];
01876           if (tmp_length <= 0.0) {
01877             // cerr << "out..." << endl;
01878             continue;
01879           }
01880           //
01881           rho[2] = u_rho[2]*tmp_length;
01882           
01883           r[0] = rho[0] + R[0];
01884           r[2] = rho[2] + R[2];
01885           
01886         }
01887         
01888         if (0) { 
01889           // refinements
01890           int iter_gibbs=0;
01891           double old_c[3];
01892           double delta_c = 1.0;
01893           while ((iter_gibbs<10) && (delta_c > 1.0e-6)) {
01894             
01895             // Gibbs relation (see i.e. F. Zagar, "Astronomia Sferica e teorica", chap. XVIII)
01896             double Gibbs_B[3];
01897             Gibbs_B[0] = (tau[1]*tau[2]-secure_pow(tau[0],2))/12;
01898             Gibbs_B[1] = (tau[0]*tau[2]-secure_pow(tau[1],2))/12;
01899             Gibbs_B[2] = (tau[0]*tau[1]-secure_pow(tau[2],2))/12;
01900             double Brm3[3];
01901             for (j=0;j<3;++j) { 
01902               Brm3[j] = Gibbs_B[j]/secure_pow(r[j].Length(),3);
01903             }
01904             delta_c = 0.0;
01905             for (j=0;j<3;++j) { 
01906               old_c[j] = c[j];
01907               c[j] = tau[j]/tau[1]*(1+Brm3[j])/(1-Brm3[1]);
01908               delta_c += secure_pow(c[j]-old_c[j],2);
01909               // printf("NEW c[%i] = %g\n",j,c[j]);
01910             }
01911             delta_c /= 3;
01912             delta_c  = sqrt(delta_c);
01913             
01914             {
01915               const Vector v_k = rho[1] - (c[0]*R[0] + c[2]*R[2] - R[1]);
01916               const double   k = v_k.Length();
01917               // const Vector u_k = v_k/Length(v_k);
01918               const Vector u_k = v_k.Normalized();
01919               
01920               const double s02 = u_rho[0]*u_rho[2];
01921               const double s0k = u_rho[0]*u_k;
01922               const double s2k = u_rho[2]*u_k;
01923               
01924               rho[0] = u_rho[0]*(k*(s0k-s02*s2k)/(1-secure_pow(s02,2)))/c[0];
01925               rho[2] = u_rho[2]*(k*(s2k-s02*s0k)/(1-secure_pow(s02,2)))/c[2];
01926               
01927               r[0] = rho[0] + R[0];
01928               r[2] = rho[2] + R[2];
01929             }
01930             
01931             ++iter_gibbs;
01932           }
01933          
01934         }
01935         
01936         // try a simpler rule
01937         // v = (r[1]-r[0])/(FromUnits(obs[0].date.GetJulian()-obs[1].date.GetJulian(),DAY));
01938         /* v = ( (r[1]-r[0])/(FromUnits(obs[1].date.GetJulian()-obs[0].date.GetJulian(),DAY)) + 
01939            (r[2]-r[1])/(FromUnits(obs[2].date.GetJulian()-obs[1].date.GetJulian(),DAY)) ) / 2.0;
01940         */
01941         // v = velocity at the epoch of the first obs, obs[0]
01942         // Vector v = (r[1]-r[0])/(FromUnits(obs[0].date.GetJulian()-obs[1].date.GetJulian(),DAY));
01943         Vector v = (r[1]-r[0])/(FromUnits(obs[1].date.GetJulian()-obs[0].date.GetJulian(),DAY));
01944         
01945         // light-time correction [to be checked!]
01946         r[0] += v*(r[0]-R[0]).Length()/GetC();
01947         
01948         // orbit.ref_body = Body("Sun",GetMSun(),Vector(0,0,0),Vector(0,0,0));
01949         // orbit.Compute(r[1],v,GetG()*GetMSun());
01950         orbit.Compute(r[0],v,GetG()*GetMSun(),obs[0].date);
01951         
01952         // cerr << "HHH ref body mass : " << ref_body.mass() << endl;
01953         
01954         // orbit.epoch = obs[1].date;
01955         // orbit.epoch = obs[0].date;
01956         
01957         // quick test
01958         if (orbit.e < 1.0) {
01959           
01960           // const Vector old_v = v;
01961           // test!
01962           //
01963           // test... maybe is needed...
01964           // gauss_v(r[0],v,obs);
01965           //
01966           //
01967           // update the orbit with the improved velocity
01968           // orbit.Compute(r[1],v,GetG()*GetMSun());
01969           orbit.Compute(r[0],v,GetG()*GetMSun(),obs[0].date);
01970           
01971           // debug
01972           cerr << " ---> orbit.a = " << orbit.a << endl;
01973           cerr << " ---> orbit.e = " << orbit.e << endl;
01974           cerr << " ---> orbit.i = " << orbit.i*(180/pi) << endl;
01975           
01976           // Sky sky;
01977           // UniverseTypeAwareTime utat; utat.SetDate(obs[1].date);
01978           // sky.Compute(rho[1],utat);
01979           // cerr << obs[1].ra.GetRad() << "  " << obs[1].dec.GetRad() << "  " << sky.ra().GetRad() << "  " << sky.dec().GetRad() << "  " << RMS_residuals(obs) << "  " << obs[1].date.GetJulian()-obs[0].date.GetJulian() << "  " << obs[2].date.GetJulian()-obs[1].date.GetJulian() << endl;
01980           
01981           // cerr << "better..." << endl;
01982           /* 
01983              int j;
01984              Sky sky;
01985              UniverseTypeAwareTime utat;
01986              for (j=0;j<3;++j) {
01987              utat.SetDate(obs[j].date); 
01988              // sky.Compute(rho[j],utat);
01989              sky.Compute(r[j]-R[j],utat);
01990              fprintf(stderr,"SKY     ra = %30.20f   dec = %30.20f\n",obs[j].ra.GetRad(),obs[j].dec.GetRad());
01991              fprintf(stderr,"COMP    ra = %30.20f   dec = %30.20f\n",sky.ra().GetRad(),sky.dec().GetRad());
01992              }
01993           */
01994           
01995           fprintf(stderr,"limited RMS: %g\n",RMS_residuals(obs,orbit));
01996           
01997           if (0) {
01998             UniverseTypeAwareTime utat;
01999             Sky sky;
02000             double hand_RMS=0;
02001             for (j=0;j<3;++j) {
02002               // sky.Compute(r[j]-R[j],utat);
02003               sky.Compute_J2000(r[j]-R[j]);
02004               hand_RMS += ( secure_pow((obs[j].ra.GetRad()-sky.ra().GetRad())*cos(obs[j].dec.GetRad()),2) + 
02005                             secure_pow(obs[j].dec.GetRad()-sky.dec().GetRad(),2) );
02006             }   
02007             hand_RMS /= 3.0;
02008             hand_RMS  = sqrt(hand_RMS);
02009             hand_RMS *= (180/pi)*3600;
02010             fprintf(stderr,"RMS by hands: %g\n",hand_RMS);
02011             
02012             // tests
02013             if (hand_RMS > 1.0) {
02014               cerr << "controlled exit..." << endl;
02015               exit(0);
02016             }
02017             
02018           }
02019           
02020         } else {
02021           // cerr << "orbit eccentricity > 1.0: a=" << orbit.a << "  e=" << orbit.e << "  i=" << orbit.i*180/pi << endl;
02022         }       
02023         
02024         /* 
02025            if (orbit.e > 0.8) {
02026            cerr << "prelim. orbit:   a = " << orbit.a << "   e = " << orbit.e << "  i = " << orbit.i*(180/pi) << endl;
02027            } else {
02028            cerr << "prelim. orbit:   a = " << orbit.a << "   e = " << orbit.e << "  i = " << orbit.i*(180/pi) << " RMS residuals (arcsec): " << RMS_residuals(obs,orbit) << endl;
02029            }
02030         */
02031         
02032         if (orbit.e < 1.0) preliminary_orbits.push_back(orbit);
02033       }
02034     }
02035     
02036     cerr << "out of Compute_Gauss(...)" << endl;
02037   }
02038   
02039   
02040   // 
02041   
02042   void Compute_TestMethod(const vector<Observation> &obs_in, vector<PreliminaryOrbit> &preliminary_orbits) {
02043     
02044     cerr << "inside Compute_TestMethod(...)" << endl;
02045     
02046     PreliminaryOrbit orbit;
02047     
02048     vector<Observation> obs(obs_in);
02049     
02050     unsigned int j;
02051     
02052     sort(obs.begin(),obs.end());
02053     
02054     /* 
02055        Config conf;
02056        OrsaConfigFile ocf(&conf);
02057        ocf.Read();
02058        ocf.Close();
02059     */
02060     
02061     const unsigned int size = obs.size();
02062     
02063     Vector R[size]; // heliocentric earth position
02064     {
02065       Vector tmp_r, tmp_v;
02066       JPLFile jf(config->paths[JPL_EPHEM_FILE]->GetValue().c_str());
02067       for (j=0;j<size;++j) { 
02068         jf.GetEph(obs[j].date,EARTH,SUN,tmp_r,tmp_v);
02069         R[j] = tmp_r;
02070       }
02071     }
02072     
02073     // add the observer location to the earth position
02074     {
02075       
02076       /* 
02077          LocationFile lf;
02078          lf.SetFileName(config->paths[OBSCODE]->GetValue().c_str());
02079          lf.Read();
02080          lf.Close();
02081       */
02082       for (j=0;j<size;++j) { 
02083         R[j] += location_file->ObsPos(obs[j].obscode,obs[j].date);
02084       }
02085     }
02086     
02087     Vector u_rho[size]; // object direction from the observer (unit vectors)
02088     for (j=0;j<size;++j) { 
02089       u_rho[j].x = cos(obs[j].dec) * cos(obs[j].ra);
02090       u_rho[j].y = cos(obs[j].dec) * sin(obs[j].ra);
02091       u_rho[j].z = sin(obs[j].dec);
02092       
02093       /* 
02094          if (universe->GetReferenceSystem() == ECLIPTIC) {
02095          u_rho[j].rotate(0.0,-obleq(obs[j].date).GetRad(),0.0);
02096          } 
02097       */
02098       
02099       /* 
02100          if (universe->GetReferenceSystem() == ECLIPTIC) {
02101          EquatorialToEcliptic_J2000(u_rho[j]);
02102          }
02103       */
02104       
02105       if (universe->GetReferenceSystem() == ECLIPTIC) {
02106         // EquatorialToEcliptic_J2000(u_rho[j]);
02107         Angle obl = obleq_J2000();
02108         u_rho[j].rotate(0.0,-obl.GetRad(),0.0);
02109       }
02110       
02111     }
02112     
02113     Vector rho[size];
02114     Vector r[size];
02115     Vector v; // v[0]
02116     
02117     /* 
02118        double tmp_rho = FromUnits(0.2,EARTHMOON);
02119        while (tmp_rho < FromUnits(50,AU)) {
02120     */
02121     
02122     /* 
02123        double tmp_rho = FromUnits(0.5 ,AU);
02124        while (tmp_rho < FromUnits(0.55,AU)) {
02125     */
02126     
02127     double tmp_rho = FromUnits(0.535,AU);
02128     while (tmp_rho < FromUnits(0.536,AU)) {
02129       
02130       cerr << "[******] tmp_rho: " << tmp_rho << endl;
02131       
02132       for (j=0;j<size;++j) { 
02133         rho[j] = u_rho[j]*tmp_rho;      
02134         r[j] = R[j] + rho[j];
02135       }
02136       
02137       v = (r[1]-r[0])/(FromUnits(obs[0].date.GetJulian()-obs[1].date.GetJulian(),DAY));
02138       
02139       // least sq. v
02140       gauss_v(r[0],v,obs);
02141       
02142       orbit.Compute(r[0],v,GetG()*GetMSun(),obs[0].date);
02143       // orbit.epoch = obs[0].date;
02144       
02145       // debug
02146       cerr << " ---> orbit.a = " << orbit.a << endl;
02147       cerr << " ---> orbit.e = " << orbit.e << endl;
02148       cerr << " ---> orbit.i = " << orbit.i*(180/pi) << endl;
02149       
02150       if (orbit.e < 1.0) preliminary_orbits.push_back(orbit);
02151       
02152       // tmp_rho *= 1.3;
02153       tmp_rho += 0.005;
02154     }
02155     
02156   }
02157   
02158   // OrbitWithCovarianceMatrixGSL
02159   
02160   OrbitWithCovarianceMatrixGSL::OrbitWithCovarianceMatrixGSL() : OrbitWithEpoch(), bool_have_covariance_matrix(false) {
02161     
02162   }
02163   
02164   OrbitWithCovarianceMatrixGSL::OrbitWithCovarianceMatrixGSL(Orbit &nominal_orbit) : OrbitWithEpoch(nominal_orbit), bool_have_covariance_matrix(false) {
02165     
02166   }
02167   
02168   OrbitWithCovarianceMatrixGSL::OrbitWithCovarianceMatrixGSL(Orbit &nominal_orbit, double **covariance_matrix, CovarianceMatrixElements base) : OrbitWithEpoch(nominal_orbit) {
02169     SetCovarianceMatrix(covariance_matrix,base);
02170   }
02171   
02172   void OrbitWithCovarianceMatrixGSL::SetCovarianceMatrix(double **covariance_matrix, CovarianceMatrixElements base) {
02173     memcpy((void*)covm, (const void*)covariance_matrix, 6*6*sizeof(double));
02174     cov_base = base;
02175     bool_have_covariance_matrix = true;
02176   }
02177   
02178   void OrbitWithCovarianceMatrixGSL::GetCovarianceMatrix(double **covariance_matrix, CovarianceMatrixElements &base) const {
02179     if (have_covariance_matrix()) {
02180       memcpy((void*)covariance_matrix, (const void*)covm, 6*6*sizeof(double));
02181       base = cov_base;
02182     } else {
02183       ORSA_ERROR("called method OrbitWithCovarianceMatrixGSL::GetCovarianceMatrix() from an orbit with undefined covariance matrix");
02184     }
02185   }
02186   
02187   // void OrbitWithCovarianceMatrixGSL::generate(vector<OrbitWithEpoch> &list, const int num, const int random_seed) const {
02188   void OrbitWithCovarianceMatrixGSL::GenerateUsingCholeskyDecomposition(vector<OrbitWithEpoch> & list, const int num, const int random_seed) const {
02189     
02190     list.clear();
02191     
02192     if (num < 1) return;
02193     
02194     if (!have_covariance_matrix()) {
02195       ORSA_ERROR("called method OrbitWithCovarianceMatrixGSL::GenerateUsingCholeskyDecomposition() from an orbit with undefined covariance matrix.");
02196       return;
02197     }
02198     
02199     gsl_vector * mu = gsl_vector_alloc(6);
02200     gsl_vector * x  = gsl_vector_alloc(6); // random vector
02201     gsl_vector * y  = gsl_vector_alloc(6);
02202     
02203     switch (cov_base) {
02204     case Osculating: {
02205       
02206       gsl_vector_set(mu,0,a);
02207       gsl_vector_set(mu,1,e);
02208       gsl_vector_set(mu,2,i);
02209       gsl_vector_set(mu,3,omega_node);
02210       gsl_vector_set(mu,4,omega_pericenter);
02211       gsl_vector_set(mu,5,M);
02212       
02213       break;
02214     }
02215     case Equinoctal: {
02216       
02217       gsl_vector_set(mu,0,a);
02218       gsl_vector_set(mu,1,e*sin(omega_node+omega_pericenter));
02219       gsl_vector_set(mu,2,e*cos(omega_node+omega_pericenter));
02220       gsl_vector_set(mu,3,tan(i/2)*sin(omega_node));
02221       gsl_vector_set(mu,4,tan(i/2)*cos(omega_node));
02222       gsl_vector_set(mu,5,fmod(omega_node+omega_pericenter+M,twopi));
02223       
02224       break;
02225     }
02226     }
02227     
02228     gsl_matrix * A = gsl_matrix_alloc(6,6);
02229     
02230     int i,k;
02231     for (i=0;i<6;++i) {
02232       for (k=0;k<6;++k) {
02233         gsl_matrix_set(A,i,k,covm[i][k]);
02234       }
02235     }
02236     
02237     // Cholesky decomposition
02238     int decomp_status = gsl_linalg_cholesky_decomp(A);
02239     
02240     if (decomp_status) {
02241       
02242       ORSA_WARNING("Cholesky decomposition failed.");
02243       
02244       gsl_vector_free(mu);
02245       gsl_vector_free(x);
02246       gsl_vector_free(y);
02247       gsl_matrix_free(A);
02248       
02249       return;
02250     }
02251     
02252     // remove L^T, the upper triangle (keep the diagonal elements!)
02253     for (i=0;i<6;++i) {
02254       for (k=i+1;k<6;++k) {
02255         gsl_matrix_set(A,i,k,0.0);
02256       }
02257     }
02258     
02259     // check
02260     // gsl_matrix_fprintf(stderr,A,"%g");
02261     
02262     OrbitWithEpoch gen_orb = (*this);
02263     
02264     int generated = 0;
02265     
02266     // random number generator
02267     gsl_rng * rnd = gsl_rng_alloc(gsl_rng_gfsr4);
02268     gsl_rng_set(rnd,random_seed);
02269     
02270     switch (cov_base) {
02271     case Osculating: {
02272       while (1) {
02273         
02274         for (i=0;i<6;++i) gsl_vector_set(x,i,gsl_ran_ugaussian(rnd));
02275         
02276         // y = A x + mu
02277         gsl_vector_memcpy(y,mu);
02278         gsl_blas_dgemv(CblasNoTrans,1.0,A,x,1.0,y);
02279         
02280         ++generated;
02281         
02282         gen_orb.a                 = gsl_vector_get (y,0);
02283         gen_orb.e                 = gsl_vector_get (y,1);
02284         gen_orb.i                 = gsl_vector_get (y,2);
02285         gen_orb.omega_node        = gsl_vector_get (y,3);
02286         gen_orb.omega_pericenter  = gsl_vector_get (y,4);
02287         gen_orb.M                 = gsl_vector_get (y,5);
02288         
02289         list.push_back(gen_orb);
02290         
02291         if (generated>=num) break;
02292       }
02293       break;
02294     }
02295     case Equinoctal: {
02296       
02297       while (1) {
02298         
02299         for (i=0;i<6;++i) gsl_vector_set(x,i,gsl_ran_ugaussian(rnd));
02300         
02301         // y = A x + mu
02302         gsl_vector_memcpy(y,mu);
02303         gsl_blas_dgemv(CblasNoTrans,1.0,A,x,1.0,y);
02304         
02305         ++generated;
02306         
02307         const double omega_tilde  = secure_atan2(gsl_vector_get(y,1),gsl_vector_get(y,2));
02308         
02309         gen_orb.a                 = gsl_vector_get(y,0);
02310         gen_orb.e                 = sqrt(gsl_vector_get(y,1)*gsl_vector_get(y,1)+gsl_vector_get(y,2)*gsl_vector_get(y,2));
02311         gen_orb.i                 = 2.0*atan(sqrt(gsl_vector_get(y,3)*gsl_vector_get(y,3)+gsl_vector_get(y,4)*gsl_vector_get(y,4)));
02312         gen_orb.omega_node        = fmod(10.0*twopi+secure_atan2(gsl_vector_get(y,3),gsl_vector_get(y,4)),twopi);
02313         gen_orb.omega_pericenter  = fmod(10.0*twopi+omega_tilde-omega_node,twopi);
02314         gen_orb.M                 = fmod(10.0*twopi+gsl_vector_get(y,5)-omega_tilde,twopi);
02315         
02316         list.push_back(gen_orb);
02317         
02318         if (generated>=num) break;
02319       }
02320       break;
02321     }
02322     }
02323     
02324     gsl_vector_free(mu);
02325     gsl_vector_free(x);
02326     gsl_vector_free(y);
02327     gsl_matrix_free(A);
02328     
02329     // free random number generator
02330     gsl_rng_free(rnd);
02331     
02332   }
02333   
02334   /*******/
02335   
02336   void OrbitWithCovarianceMatrixGSL::GenerateUsingPrincipalAxisTransformation(vector<OrbitWithEpoch> & list, const int num, const int random_seed) const {
02337     
02338     list.clear();
02339     
02340     if (num < 1) return;
02341     
02342     if (!have_covariance_matrix()) {
02343       ORSA_ERROR("called method OrbitWithCovarianceMatrixGSL::GenerateUsingPrincipalAxisTransformation() from an orbit with undefined covariance matrix.");
02344       return;
02345     }
02346     
02347     gsl_vector * mu = gsl_vector_alloc(6);
02348     gsl_vector * x  = gsl_vector_alloc(6); // random vector
02349     gsl_vector * y  = gsl_vector_alloc(6);
02350     
02351     switch (cov_base) {
02352     case Osculating: {
02353       
02354       gsl_vector_set(mu,0,a);
02355       gsl_vector_set(mu,1,e);
02356       gsl_vector_set(mu,2,i);
02357       gsl_vector_set(mu,3,omega_node);
02358       gsl_vector_set(mu,4,omega_pericenter);
02359       gsl_vector_set(mu,5,M);
02360       
02361       break;
02362     }
02363     case Equinoctal: {
02364       
02365       gsl_vector_set(mu,0,a);
02366       gsl_vector_set(mu,1,e*sin(omega_node+omega_pericenter));
02367       gsl_vector_set(mu,2,e*cos(omega_node+omega_pericenter));
02368       gsl_vector_set(mu,3,tan(i/2)*sin(omega_node));
02369       gsl_vector_set(mu,4,tan(i/2)*cos(omega_node));
02370       gsl_vector_set(mu,5,fmod(omega_node+omega_pericenter+M,twopi));
02371       
02372       break;
02373     }
02374     }
02375     
02376     gsl_matrix * A = gsl_matrix_alloc(6,6);
02377     
02378     for (unsigned int i=0;i<6;++i) {
02379       for (unsigned int k=0;k<6;++k) {
02380         gsl_matrix_set(A,i,k,covm[i][k]);
02381       }
02382     }
02383     
02384     // covariance matrix view
02385     // const gsl_matrix_view m = gsl_matrix_view_array(A,6,6);
02386     
02387     gsl_permutation * p = gsl_permutation_alloc(6);
02388     gsl_matrix * LU = gsl_matrix_alloc(6,6);
02389     gsl_matrix_memcpy(LU,A);  
02390     int s;
02391     
02392     // covariance matrix LU decomposition
02393     gsl_linalg_LU_decomp(LU, p, &s);
02394     
02395     // covariance matrix inverse
02396     gsl_matrix * inv = gsl_matrix_alloc(6,6);
02397     gsl_linalg_LU_invert(LU, p, inv);
02398     
02399     // covariance matrix determinant
02400     // const double det = gsl_linalg_LU_det(LU, s);
02401     
02402     // covariance eigenvalues and eigenvectors
02403     gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc(6); // workspace for eigenvectors/values
02404     gsl_vector * eval = gsl_vector_alloc(6);    // eigenvalues
02405     gsl_matrix * evec = gsl_matrix_alloc(6,6);  // eigenvectors
02406     //
02407     gsl_eigen_symmv (A, eval, evec, w); // NOTE: The diagonal and lower triangular part of A are destroyed during the computation
02408     
02409     /********/
02410     
02411     {
02412       // debug
02413       for (unsigned int i=0;i<6;++i) {
02414         cerr << "eval[" << i << "] = " << gsl_vector_get(eval,i) << endl;
02415       }
02416     }
02417     
02418     OrbitWithEpoch gen_orb = (*this);
02419     
02420     int generated = 0;
02421     
02422     // random number generator
02423     gsl_rng * rnd = gsl_rng_alloc(gsl_rng_gfsr4);
02424     gsl_rng_set(rnd,random_seed);
02425     
02426     double sigma[6];
02427     for (unsigned int i=0;i<6;++i) {
02428       if (gsl_vector_get(eval,i) < 0.0) {
02429         
02430         ORSA_ERROR("problems with the covariance matrix: negative eigenvalue found.");
02431         
02432         // free GSL stuff
02433         gsl_vector_free(mu);
02434         gsl_vector_free(x);
02435         gsl_vector_free(y);
02436         gsl_matrix_free(A);
02437         gsl_permutation_free(p);
02438         gsl_matrix_free(LU); 
02439         gsl_matrix_free(inv);
02440         gsl_eigen_symmv_free(w);
02441         gsl_vector_free(eval);
02442         gsl_matrix_free(evec);
02443         gsl_rng_free(rnd);
02444         
02445         return;
02446       }
02447       
02448       sigma[i] = secure_sqrt(gsl_vector_get(eval,i));
02449     }
02450     
02451     switch (cov_base) {
02452     case Osculating: {
02453       while (1) {
02454         
02455         for (unsigned int i=0;i<6;++i) {
02456           gsl_vector_set(x,i,gsl_ran_gaussian(rnd,sigma[i]));
02457         }
02458         
02459         gsl_vector_memcpy(y,mu);
02460         gsl_blas_dgemv(CblasNoTrans,1.0,evec,x,1.0,y);
02461         
02462         ++generated;
02463         
02464         gen_orb.a                 = gsl_vector_get (y,0);
02465         gen_orb.e                 = gsl_vector_get (y,1);
02466         gen_orb.i                 = gsl_vector_get (y,2);
02467         gen_orb.omega_node        = gsl_vector_get (y,3);
02468         gen_orb.omega_pericenter  = gsl_vector_get (y,4);
02469         gen_orb.M                 = gsl_vector_get (y,5);
02470         
02471         list.push_back(gen_orb);
02472         
02473         if (generated>=num) break;
02474       }
02475       break;
02476     }
02477     case Equinoctal: {
02478       while (1) {
02479         
02480         for (unsigned int i=0;i<6;++i) {
02481           gsl_vector_set(x,i,gsl_ran_gaussian(rnd,sigma[i]));
02482         }
02483         
02484         gsl_vector_memcpy(y,mu);
02485         gsl_blas_dgemv(CblasNoTrans,1.0,evec,x,1.0,y);
02486         
02487         ++generated;
02488         
02489         const double omega_tilde  = secure_atan2(gsl_vector_get(y,1),gsl_vector_get(y,2));
02490         
02491         gen_orb.a                 = gsl_vector_get(y,0);
02492         gen_orb.e                 = sqrt(gsl_vector_get(y,1)*gsl_vector_get(y,1)+gsl_vector_get(y,2)*gsl_vector_get(y,2));
02493         gen_orb.i                 = 2.0*atan(sqrt(gsl_vector_get(y,3)*gsl_vector_get(y,3)+gsl_vector_get(y,4)*gsl_vector_get(y,4)));
02494         gen_orb.omega_node        = fmod(10.0*twopi+secure_atan2(gsl_vector_get(y,3),gsl_vector_get(y,4)),twopi);
02495         gen_orb.omega_pericenter  = fmod(10.0*twopi+omega_tilde-omega_node,twopi);
02496         gen_orb.M                 = fmod(10.0*twopi+gsl_vector_get(y,5)-omega_tilde,twopi);
02497         
02498         list.push_back(gen_orb);
02499         
02500         if (generated>=num) break;
02501       }
02502       break;
02503     }
02504     }
02505     
02506     // free GSL stuff
02507     gsl_vector_free(mu);
02508     gsl_vector_free(x);
02509     gsl_vector_free(y);
02510     gsl_matrix_free(A);
02511     gsl_permutation_free(p);
02512     gsl_matrix_free(LU); 
02513     gsl_matrix_free(inv);
02514     gsl_eigen_symmv_free(w);
02515     gsl_vector_free(eval);
02516     gsl_matrix_free(evec);
02517     gsl_rng_free(rnd);
02518     
02519   } 
02520   
02521   // Close Approaches
02522   
02523   class CloseApproaches_gsl_parameters {
02524   public:
02525     Frame f; // reference frame, never changed
02526     unsigned int obj_index, index;
02527     Evolution * e;
02528   };
02529   
02530   double CloseApproaches_gsl_f(const gsl_vector * v, void * params) {
02531     
02532     CloseApproaches_gsl_parameters * p = (CloseApproaches_gsl_parameters *) params;
02533     
02534     Frame f = p->f;
02535     
02536     const UniverseTypeAwareTime gsl_time(gsl_vector_get(v,0));
02537     
02538     p->e->clear();
02539     p->e->push_back(f);
02540     p->e->SetSamplePeriod(f - gsl_time); 
02541     p->e->SetMaxUnsavedSubSteps(10000);
02542     p->e->Integrate(gsl_time,true);
02543     
02544     f = (*(p->e))[p->e->size()-1];
02545     
02546     return (f[p->obj_index].position() - f[p->index].position()).Length();
02547   }
02548   
02549   // void ComputeCloseApproaches(const Frame & f, const unsigned int obj_index, const UniverseTypeAwareTime & utat_start, const UniverseTypeAwareTime & utat_stop, vector<CloseApproach> & clapp, const double distance_threshold, const double sample_period) {
02550   
02551   /* 
02552      void ComputeCloseApproaches(const Frame & f, const unsigned int obj_index, const UniverseTypeAwareTime & utat_start, const UniverseTypeAwareTime & utat_stop, vector<CloseApproach> & clapp, const double distance_threshold, const UniverseTypeAwareTimeStep & sample_period) {
02553      
02554      clapp.clear();
02555      
02556      if (obj_index >= f.size()) return;
02557      
02558      // double t_start, t_stop;
02559      UniverseTypeAwareTime t_start, t_stop;
02560      if (utat_start < utat_stop) {
02561      t_start = utat_start;
02562      t_stop  = utat_stop;
02563      } else if (utat_start > utat_stop) {
02564      t_start = utat_stop;
02565      t_stop  = utat_start;
02566      } else {
02567      // same times
02568      return;
02569      }
02570      
02571      vector<Frame> frames;
02572      
02573      Radau15 itg; 
02574      itg.accuracy = 1.0e-12;
02575      itg.timestep = FromUnits(1,DAY);
02576      
02577      // Newton newton;
02578      Interaction * itr = 0;
02579      
02580      #ifdef HAVE_MPI
02581      int size;
02582      MPI_Comm_size( MPI_COMM_WORLD, &size );
02583      if (size > 1) {
02584      itr = new Newton_MPI();
02585      } else {
02586      itr = new Newton();
02587      }
02588      #else
02589      itr = new Newton();
02590      #endif
02591      
02592      Evolution evol;
02593      evol.SetInteraction(itr);
02594      evol.SetIntegrator(&itg);
02595      evol.SetMaxUnsavedSubSteps(100000);
02596      evol.SetSamplePeriod(sample_period);
02597      
02598      delete itr;
02599      
02600      Frame running_frame;
02601      
02602      cerr << "clapp [1]" << endl;
02603      
02604      if (t_start < f) {
02605      if (t_stop < f) {
02606      evol.push_back(f);
02607      evol.Integrate(t_stop);
02608      running_frame = evol[evol.size()-1];
02609      evol.clear();
02610      evol.push_back(running_frame);
02611      evol.Integrate(t_start);
02612      frames.resize(evol.size());
02613      for (unsigned int k=0;k<evol.size();++k) {
02614      frames[k] = evol[k];
02615      }
02616      } else {
02617      evol.push_back(f);
02618      evol.Integrate(t_start);
02619      // frames = evol;
02620      frames.resize(evol.size());
02621      for (unsigned int k=0;k<evol.size();++k) {
02622      frames[k] = evol[k];
02623      }
02624      evol.clear();
02625      evol.push_back(f);
02626      evol.Integrate(t_stop);
02627      unsigned int k=1; // skip first frame, already saved in frames
02628      while (k<evol.size()) {
02629      frames.push_back(evol[k]);
02630      ++k;
02631      }
02632      }
02633      } else {
02634      evol.push_back(f);
02635      evol.Integrate(t_start);
02636      running_frame = evol[evol.size()-1];
02637      evol.clear();
02638      evol.push_back(running_frame);
02639      evol.Integrate(t_stop);
02640      frames.resize(evol.size());
02641      for (unsigned int k=0;k<evol.size();++k) {
02642      frames[k] = evol[k];
02643      }
02644      }
02645      
02646      cerr << "clapp [2]" << endl;
02647      
02648      evol.clear();
02649      
02650      sort(frames.begin(),frames.end());
02651      
02652      // gsl init
02653      CloseApproaches_gsl_parameters par;
02654      par.e = &evol;
02655      //
02656      gsl_multimin_fminimizer * s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex,1);
02657      //
02658      gsl_multimin_function clapp_function;
02659      clapp_function.f      = &CloseApproaches_gsl_f;
02660      clapp_function.n      = 1;
02661      clapp_function.params = &par;
02662      //
02663      gsl_vector * x         = gsl_vector_alloc(1);
02664      gsl_vector * step_size = gsl_vector_alloc(1);
02665      
02666      cerr << "clapp [3]" << endl;
02667      
02668      vector<double> distance;
02669      distance.resize(frames.size());
02670      CloseApproach ca;
02671      unsigned int index=0, j;
02672      while (index < f.size()) {
02673      if (index != obj_index) {
02674      
02675      j=0;
02676      while (j<frames.size()) {
02677      distance[j] = (frames[j][obj_index].position() - frames[j][index].position()).Length();
02678      ++j;
02679      } 
02680      
02681      // skip first and last
02682      j=1;
02683      while (j<(frames.size()-1)) {
02684      
02685      if ( (distance[j] < 1.5*distance_threshold) &&
02686      (distance[j] < distance[j-1]) &&
02687      (distance[j] < distance[j+1]) ) {
02688      
02689      // gsl stuff
02690      par.f         = frames[j];
02691      par.obj_index = obj_index;
02692      par.index     = index;
02693      
02694      gsl_vector_set(x,0,frames[j].GetTime());
02695      
02696      gsl_vector_set(step_size,0,sample_period.GetDouble());
02697      
02698      gsl_multimin_fminimizer_set(s, &clapp_function, x, step_size);
02699      
02700      cerr << "clapp [4]" << endl;
02701      
02702      size_t iter = 0;
02703      int status;
02704      do {
02705      
02706      ++iter;
02707      
02708      // iter status
02709      status = gsl_multimin_fminimizer_iterate(s);
02710      
02711      // convergence status
02712      // status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),1.0e-6);
02713      status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),FromUnits(1,MINUTE));
02714      
02715      } while (status == GSL_CONTINUE && iter < 200);
02716      
02717      // const double minimum = gsl_multimin_fminimizer_minimum(s);
02718      
02719      if (status == GSL_SUCCESS) {
02720      if (gsl_multimin_fminimizer_minimum(s) < distance_threshold) {
02721      ca.name = frames[j][index].name();
02722      ca.epoch.SetTime(gsl_vector_get(s->x,0));
02723      ca.distance = gsl_multimin_fminimizer_minimum(s);
02724      ca.relative_velocity = (frames[j][obj_index].velocity() - frames[j][index].velocity()).Length(); // should use a better value
02725      clapp.push_back(ca);
02726      }
02727      }
02728      
02729      cerr << "clapp [5]" << endl;
02730      
02731      }
02732      ++j;
02733      }
02734      
02735      }
02736      ++index;
02737      }
02738      
02739      // gsl free
02740      gsl_multimin_fminimizer_free(s);
02741      gsl_vector_free(x);
02742      gsl_vector_free(step_size);
02743      }
02744   */
02745   
02746   void SearchCloseApproaches(const Evolution * evol, const unsigned int obj_index, const unsigned int index, std::vector<CloseApproach> & clapp, const double distance_threshold, const double time_accuracy) {
02747     
02748     if (index == obj_index) return;
02749     
02750     // gsl init
02751     CloseApproaches_gsl_parameters par;
02752     par.e = new Evolution(*evol);
02753     //
02754     gsl_multimin_fminimizer * s = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex,1);
02755     //
02756     gsl_multimin_function clapp_function;
02757     clapp_function.f      = &CloseApproaches_gsl_f;
02758     clapp_function.n      = 1;
02759     clapp_function.params = &par;
02760     //
02761     gsl_vector * x         = gsl_vector_alloc(1);
02762     gsl_vector * step_size = gsl_vector_alloc(1);
02763     
02764     // cerr << "clapp [3]" << endl;
02765     
02766     vector<double> distance;
02767     distance.resize(evol->size());
02768     
02769     CloseApproach ca;
02770     
02771     for (unsigned int j=0;j<evol->size();++j) {
02772       distance[j] = ((*evol)[j][obj_index].position() - (*evol)[j][index].position()).Length();
02773     } 
02774     
02775     // skip first and last
02776     for (unsigned int j=1;j<(evol->size()-1);++j) {
02777       
02778       if ( (distance[j] <= 1.5*distance_threshold) &&
02779            (distance[j] <= distance[j-1]) &&
02780            (distance[j] <= distance[j+1]) ) {
02781         
02782         // gsl stuff
02783         par.f         = (*evol)[j];
02784         par.obj_index = obj_index;
02785         par.index     = index;
02786         
02787         gsl_vector_set(x,0,(*evol)[j].GetTime());
02788         
02789         gsl_vector_set(step_size,0,evol->GetSamplePeriod().GetDouble());
02790         
02791         gsl_multimin_fminimizer_set(s, &clapp_function, x, step_size);
02792         
02793         // cerr << "clapp [4]" << endl;
02794         
02795         size_t iter = 0;
02796         int status;
02797         do {
02798           
02799           ++iter;
02800           
02801           // iter status
02802           status = gsl_multimin_fminimizer_iterate(s);
02803           
02804           // convergence status
02805           // status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),FromUnits(1,SECOND));
02806           // status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),FromUnits(10.0,MINUTE));
02807           // status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),FromUnits(1,HOUR));
02808           status = gsl_multimin_test_size(gsl_multimin_fminimizer_size(s),time_accuracy);
02809           
02810         } while (status == GSL_CONTINUE && iter < 200);
02811         
02812         // const double minimum = gsl_multimin_fminimizer_minimum(s);
02813         
02814         if (status == GSL_SUCCESS) {
02815           if (gsl_multimin_fminimizer_minimum(s) < distance_threshold) {
02816             ca.name = (*evol)[j][index].name();
02817             ca.epoch.SetTime(gsl_vector_get(s->x,0));
02818             ca.distance = gsl_multimin_fminimizer_minimum(s);
02819             
02820             {
02821               Frame f = par.f;
02822               //
02823               const UniverseTypeAwareTime gsl_time(gsl_vector_get(s->x,0));
02824               //
02825               par.e->clear();
02826               par.e->push_back(f);
02827               par.e->SetSamplePeriod(f - gsl_time); 
02828               par.e->SetMaxUnsavedSubSteps(10000);
02829               par.e->Integrate(gsl_time,true);
02830               //
02831               f = (*(par.e))[par.e->size()-1];
02832               //
02833               ca.relative_velocity = (f[obj_index].velocity() - f[index].velocity()).Length(); 
02834               
02835               // target plane
02836               {
02837                 const Vector unit_v = (f[obj_index].velocity() - f[index].velocity()).Normalize();
02838                 const Vector d      =  f[obj_index].position() - f[index].position();
02839                 const Vector unit_d = d.Normalized();
02840                 const Vector unit_q = ExternalProduct(unit_v,unit_d).Normalize();
02841                 
02842                 // d' = \alpha d + \beta q
02843                 // and d'.z = 0.0
02844                 
02845                 if (unit_d.z != 0.0) {
02846                   const double beta = sqrt(+ unit_q.x*unit_q.x 
02847                                            + unit_q.y*unit_q.y
02848                                            + unit_q.z*unit_q.z/(unit_d.z*unit_d.z)*(unit_d.x*unit_d.x+unit_d.y*unit_d.y)
02849                                            - 2*(unit_q.z/unit_d.z)*(unit_q.x*unit_d.y+unit_q.y*unit_d.x));
02850                   const double alpha = - beta * unit_q.z / unit_d.z;
02851                   
02852                   const Vector new_unit_d = (alpha*unit_d+beta*unit_q).Normalize();
02853                   const Vector new_unit_q = ExternalProduct(unit_v,new_unit_d).Normalize();
02854                   
02855                   // this solves the ambiguity in beta, that has 2 solutions: +/- sqrt(...)
02856                   if (new_unit_q.z > 0.0) {
02857                     ca.tp_xy = d*new_unit_d;
02858                     ca.tp_z  = d*new_unit_q;
02859                   } else {
02860                     ca.tp_xy = -d*new_unit_d;
02861                     ca.tp_z  = -d*new_unit_q;
02862                   }
02863                   
02864                   /* 
02865                      {
02866                      // debug 
02867                      cerr << "new_unit_d: " << new_unit_d.x << "  " << new_unit_d.y << "  " << new_unit_d.z << endl;
02868                      cerr << "new_unit_q: " << new_unit_q.x << "  " << new_unit_q.y << "  " << new_unit_q.z << endl;
02869                      cerr << "d*unit_v:   " << d*unit_v << endl;
02870                      cerr << "sqrt((d*new_unit_d)*(d*new_unit_d)+(d*new_unit_q)*(d*new_unit_q)): " << sqrt((d*new_unit_d)*(d*new_unit_d)+(d*new_unit_q)*(d*new_unit_q)) << endl;
02871                      }
02872                   */
02873                   
02874                 } else {
02875                   ca.tp_xy = d.Length();
02876                   ca.tp_z  = 0.0;
02877                 }
02878                 
02879                 // ca.tp_xy = d*unit_xy;
02880                 // ca.tp_z  = d*unit_z;
02881                 
02882               }
02883               
02884             }
02885             //
02886             clapp.push_back(ca);
02887           }
02888           
02889         }
02890         
02891         // cerr << "clapp [5]" << endl;
02892         
02893       }
02894       
02895     }
02896     
02897     // }
02898     
02899     // gsl free
02900     gsl_multimin_fminimizer_free(s);
02901     gsl_vector_free(x);
02902     gsl_vector_free(step_size);
02903     
02904     delete par.e;
02905   }
02906   
02907 } // namespace orsa

Generated on Fri Nov 3 20:37:42 2006 for liborsa by  doxygen 1.4.7