Elaboradar  0.1
 Tutto Classi Namespace File Funzioni Variabili Tipi enumerati (enum) Gruppi
interpola_vpr_gsl.cpp
2 //#include "cum_bac.h"
3 #include <radarelab/logging.h>
4 //#include <vpr_par.h> presente in interpola_vpr.h
5 #include <gsl/gsl_version.h>
6 #include <gsl/gsl_vector.h>
7 #include <gsl/gsl_blas.h>
8 #include <gsl/gsl_multifit_nlin.h>
9 
10 // Funzioni di supporto a interpola_vpr
11 namespace {
12 
13 struct data {
14  const unsigned n;
15  double *t;
16  double *y;
17  double *sigma;
18  data(unsigned n)
19  : n(n), t(new double[n]), y(new double[n]), sigma(new double[n])
20  {
21  }
22  ~data()
23  {
24  delete[] t;
25  delete[] y;
26  delete[] sigma;
27  }
28  void print()
29  {
30  fprintf(stderr, "i t y sigma\n");
31  for (unsigned i = 0; i < n; ++i)
32  fprintf(stderr, "%2d %.2f %.2f %.2f\n", i, t[i], y[i], sigma[i]);
33  }
34 };
35 
36 int
37 expb_f (const gsl_vector * x, void *data,
38  gsl_vector * f)
39 {
40  const struct data& args = *(struct data*)data;
41  double B = gsl_vector_get (x, 0);
42  double E = gsl_vector_get (x, 1);
43  double G = gsl_vector_get (x, 2);
44  double C = gsl_vector_get (x, 3);
45  double F = gsl_vector_get (x, 4);
46  for (unsigned i = 0; i < args.n; i++)
47  {
48  /* Model Yi = A * exp(-lambda * i) + b */
49  //double t = i;
50  double Yi = B * exp (- (args.t[i]-E)/G*(args.t[i]-E)/G ) + C+F*args.t[i];
51  gsl_vector_set (f, i, (Yi - args.y[i])/args.sigma[i]);
52  }
53  return GSL_SUCCESS;
54 }
55 
56 int
57 expb_df (const gsl_vector * x, void *data,
58  gsl_matrix * J)
59 {
60  const struct data& args = *(struct data*)data;
61  double B = gsl_vector_get (x, 0);
62  double E = gsl_vector_get (x, 1);
63  double G= gsl_vector_get (x, 2);
64  for (unsigned i = 0; i < args.n; i++)
65  {
66  /* Jacobian matrix J(i,j) = dfi / dxj, */
67  /* where fi = (Yi - yi)/sigma[i], */
68  /* Yi = A * exp(-lambda * i) + b */
69  /* and the xj are the parameters (A,lambda,b) */
70  // double t = i;
71  // double e = exp(-lambda * t);
72  double arg =((args.t[i]-E)/G);
73  double ex=exp(-arg*arg);
74  double fac=B*ex*2.0*arg;
75 
76  gsl_matrix_set (J, i, 0, ex);
77  gsl_matrix_set (J, i, 1, fac/G);
78  gsl_matrix_set (J, i, 2, fac*arg/G);
79  gsl_matrix_set (J, i, 3, 1.);
80  gsl_matrix_set (J, i, 4, args.t[i]);
81 
82  }
83  return GSL_SUCCESS;
84 }
85 
86 int
87 expb_fdf (const gsl_vector * x, void *data,
88  gsl_vector * f, gsl_matrix * J)
89 {
90  expb_f (x, data, f);
91  expb_df (x, data, J);
92 
93  return GSL_SUCCESS;
94 }
95 
96 void
97 print_state (size_t iter, gsl_multifit_fdfsolver * s)
98 {
99  fprintf(stderr, "iter: %3zu x = % 15.8f % 15.8f % 15.8f % 15.8f % 15.8f "
100  "|f(x)| = %g\n",
101  iter,
102  gsl_vector_get (s->x, 0),
103  gsl_vector_get (s->x, 1),
104  gsl_vector_get (s->x, 2),
105  gsl_vector_get (s->x, 3),
106  gsl_vector_get (s->x, 4),
107  gsl_blas_dnrm2 (s->f));
108 }
109 
110 int testfit(double a[])
111 {
112  if (a[0]<0. || a[0] >15.) return 1;
113  if (a[1] >10.) return 1;
114  if (a[2]<0.2 || a[2] > 0.6 ) return 1; //da analisi set dati
115  if (a[3]<0. ) return 1;
116  if (a[4]>0 ) return 1;
117  return 0;
118 }
119 
120 float lineargauss(double xint , double a[])
121 {
122  return a[0] * exp (- (xint-a[1])/a[2]*(xint-a[1])/a[2] ) + a[3]+a[4]*xint;
123 
124 }
125 
126 }
127 
128 
129 namespace radarelab {
130 
131 int InterpolaVPR_GSL::interpola_VPR(const float* vpr, int hvprmax, int livmin)
132 {
133  LOG_CATEGORY("radar.vpr");
134  static const unsigned N = 10;
135  const gsl_multifit_fdfsolver_type *T;
136  gsl_multifit_fdfsolver *s;
137  int status;
138  unsigned int i;
139  const size_t n = N;
140  const size_t p = 5;
141  char file_vprint[512];
142  gsl_matrix *covar = gsl_matrix_alloc (p, p);
143  double a[5];
144  struct data d(N);
145  gsl_multifit_function_fdf f;
146  double x_init[5] = { 4, 0.2, 3. , 1.4, -0.4 };
147  gsl_vector_view x = gsl_vector_view_array (x_init, p);
148 
150  int ier_int=0;
151  double xint,yint;
152  /* punti interessanti per inizializzare parametri*/
153  int in1=(int)((hvprmax-TCK_VPR/2)/TCK_VPR); //indice del massimo
154  int in2=(int)((hvprmax+HALF_BB)/TCK_VPR); //indice del massimo + 500 m
155  int in3=in2+1;
156  int in4=in2+5; //indice del massimo + 1000 m
157  if (in4 > NMAXLAYER-1) {
158  ier_int=1;
159  return ier_int;
160  }
161 
162  B=vpr[in1]-vpr[in2];
163  E=hvprmax/1000.;
164  G=0.25;
165  C=vpr[in2-1];
166  F=vpr[in4]<vpr[in3]?(vpr[in4]-vpr[in3])/((in4-in3)*TCK_VPR/1000.):0.;
167  // fprintf(stderr, "const unsigned NMAXLAYER=%d;\n", NMAXLAYER);
168  // fprintf(stderr, "float vpr[] = {");
169  // for (unsigned i = 0; i < NMAXLAYER; ++i)
170  // fprintf(stderr, "%s%f", i==0?"":",", (double)vpr[i]);
171  // fprintf(stderr, "};\n");
172 
173  x_init[0]= a[0]=B;
174  x_init[1]= a[1]=E;
175  x_init[2]= a[2]=G;
176  x_init[3]= a[3]=C;
177  x_init[4]= a[4]=F;
178 
179 
181 
182  f.f = &expb_f;
183  f.df = &expb_df;
184  f.fdf = &expb_fdf;
185  f.n = n;
186  f.p = p;
187  f.params = &d;
188 
189  /* This is the data to be fitted */
190 
191  for (i = 0; i < n; i++)
192  {
193  d.t[i]= ((hvprmax-1000.)>livmin)? (i*TCK_VPR+(hvprmax-800)-TCK_VPR)/1000. : (livmin+i*TCK_VPR)/1000.;
194  d.y[i]= ((hvprmax-1000.)>livmin)? vpr[i+(int)(((hvprmax-800)-TCK_VPR)/TCK_VPR)] : vpr[i+(int)(livmin/TCK_VPR)];
195  d.sigma[i] = 0.5;
196  };
197 
198  T = gsl_multifit_fdfsolver_lmsder;
199  s = gsl_multifit_fdfsolver_alloc (T, n, p);
200  gsl_multifit_fdfsolver_set (s, &f, &x.vector);
201 
202  //print_state (0, s);
203  bool found = false;
204  for (unsigned iter = 0; !found && iter < 500; ++iter)
205  {
206  //fprintf(stderr, "Iter %d\n", iter);
207  //d.print();
208  int status = gsl_multifit_fdfsolver_iterate (s);
209  if (status != 0)
210  {
211  LOG_ERROR("gsl_multifit_fdfsolver_iterate: %s", gsl_strerror(status));
212  return 1;
213  }
214 
215  //print_state (iter, s);
216 
217  status = gsl_multifit_test_delta (s->dx, s->x,
218  1e-4, 1e-4);
219  switch (status)
220  {
221  case GSL_SUCCESS: found = true; break;
222  case GSL_CONTINUE: break;
223  default:
224  LOG_ERROR("gsl_multifit_test_delta: %s", gsl_strerror(status));
225  return 1;
226  }
227  }
228 
229 #if GSL_MAJOR_VERSION == 2
230  // Use of GSL 2.0 taken from https://sft.its.cern.ch/jira/browse/ROOT-7776
231  gsl_matrix* J = gsl_matrix_alloc(s->fdf->n, s->fdf->p);
232  gsl_multifit_fdfsolver_jac(s, J);
233  gsl_multifit_covar(J, 0.0, covar);
234 #else
235  gsl_multifit_covar(s->J, 0.0, covar);
236 #endif
237 
238 #define FIT(i) gsl_vector_get(s->x, i)
239 #define ERR(i) sqrt(gsl_matrix_get(covar,i,i))
240 
241  {
242  double chi = gsl_blas_dnrm2(s->f);
243  double dof = n - p;
244  double c = GSL_MAX_DBL(1, chi / sqrt(dof));
245 
246  // printf("chisq/dof = %g\n", pow(chi, 2.0) / dof);
247 
248  // printf ("B = %.5f +/- %.5f\n", FIT(0), c*ERR(0));
249  // printf ("E = %.5f +/- %.5f\n", FIT(1), c*ERR(1));
250  // printf ("G = %.5f +/- %.5f\n", FIT(2), c*ERR(2));
251  // printf ("C = %.5f +/- %.5f\n", FIT(3), c*ERR(3));
252  // printf ("F = %.5f +/- %.5f\n", FIT(4), c*ERR(4));
253  }
254 
255  B = a[0] = FIT(0);
256  E = a[1] = FIT(1);
257  G = a[2] = FIT(2);
258  C = a[3] = FIT(3);
259  F = a[4] = FIT(4);
260 
261  gsl_multifit_fdfsolver_free (s);
262  gsl_matrix_free (covar);
263 
265 
266  if (testfit(a) == 1)
267  return 1;
268 
269  for (i=1; i<=N; i++)
270  {
271  xint=(i*TCK_VPR-TCK_VPR/2)/1000.;
272  yint= lineargauss(xint, a);
273  vpr_int[i-1] = yint;
274  }
275 
276  return 0;
277 }
278 
279 }