GRAVI Pipeline Reference Manual 1.8.0
Loading...
Searching...
No Matches
gravi_demodulate.c
Go to the documentation of this file.
1/*
2 * This file is part of the GRAVI Pipeline
3 * Copyright (C) 2002,2003 European Southern Observatory
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 */
19
32/*-----------------------------------------------------------------------------
33 Includes
34 -----------------------------------------------------------------------------*/
35
36#ifdef HAVE_CONFIG_H
37#include <config.h>
38#endif
39
40#include <cpl.h>
41#include <math.h>
42#include <time.h>
43
44#include <gsl/gsl_matrix.h>
45#include <gsl/gsl_vector.h>
46#include <gsl/gsl_multimin.h>
47#include <gsl/gsl_statistics.h>
48
49#include "gravi_demodulate.h"
50
51#include "gravi_pfits.h"
52#include "gravi_utils.h"
53
54#define FT 0
55#define SC 1
56
57#define STEPS_PER_SECOND 500
58#define MAX_SECONDS_PER_CHUNK 100
59
60#define PI 3.14159265359
61#define TWOPI 6.28318530718
62
63/*-----------------------------------------------------------------------------
64 Private prototypes
65 -----------------------------------------------------------------------------*/
66
76static cpl_size diode_column_index (int tele, int diode, int side)
77{
78 cpl_size i, r;
79 i = (side == FT) ? 0 : 16;
80 r = i + (4 * tele) + diode;
81 return 2 * r;
82}
83
84const cpl_size ntel = 4;
85const cpl_size ndiode = 4;
86const cpl_size nside = 2;
87
94const double diode_zeros[] = {
95 -0.0039217663072871976, -0.005375368693370768, -0.0039996565553508424, -0.0025503151499257103,
96 -0.004993987370698725, -0.0025982105556817733, -0.0033492901476214598, -0.0024666122995294915,
97 -0.004914056163432626, -0.004605706229646769, -0.0024913748841215717, -0.0025406836901383087,
98 -0.0035045471990661765, -0.004864493604480354, -0.0038971489167063875, -0.002645429318536674,
99 -0.0031186035580824016, -0.0030236901965512925, -0.00455348514695007, -0.0027973126198521715,
100 -0.00329832479577174, -0.004717880979213343, -0.0038105012008050844, -0.0034806513678800706,
101 -0.004843769955531035, -0.0038383890934418244, -0.003413267406206906, -0.0032447329693567305,
102 -0.0019067813627941682, -0.003741090151381262, -0.0031293194990934074, -0.002917132698761452,
103 -0.003846563742841961, -0.0016110031116351089, -0.004055586210967794, -0.004665653836460701,
104 -0.0035651245279117107, -0.003703624441150295, -0.0042027507769680774, -0.003685527333212408,
105 -0.0018717967368672794, -0.004309454414641543, -0.004161530517996172, -0.003603457935318008,
106 -0.0031196452749965883, -0.003456638711821881, -0.005237079636108828, -0.0029119219882156274,
107 -0.002883279151359765, -0.0044843120806118035, -0.0033190561101935586, -0.004244083297006716,
108 -0.005334829455674041, -0.004158489195313494, -0.00242824997808483, -0.0018624941393818074,
109 -0.0029737356896519826, -0.004348274099950943, -0.002654502952395225, -0.0030576540586404475,
110 -0.0032698514683164475, -0.003638017106725761, -0.0029787962274195018, -0.002629800558439812,
111 0.0005056591585058203, 0.0006414248583206548, 0.0005120370824436637, 0.0006245251138490539,
112 0.0002781845526483057, -0.00026739218921257777, 0.0005758898707200935, 0.0003888211264503715,
113 0.00036289398311494447, 6.419787480760673e-05, 0.0008198190512220566, 0.0007714407655275396,
114 0.0008010985218159154, 9.667686713004861e-05, 0.0008396378442216361, 0.0005619108431658763
115};
116
117static double model_x (const gsl_vector *X, int step)
118{
119 double fstep = (1.0 * step) / STEPS_PER_SECOND;
120 double a = gsl_vector_get(X, 0);
121 double b = gsl_vector_get(X, 1);
122 double pha1 = gsl_vector_get(X, 2);
123 double pha2 = gsl_vector_get(X, 3);
124
125 return a * sin(b * sin(fstep * TWOPI + pha1) + pha2);
126}
127
128static double model_y (const gsl_vector *X, int step)
129{
130 double fstep = (1.0 * step) / STEPS_PER_SECOND;
131 double a = gsl_vector_get(X, 0);
132 double b = gsl_vector_get(X, 1);
133 double pha1 = gsl_vector_get(X, 2);
134 double pha2 = gsl_vector_get(X, 3);
135
136 return a * cos(b * sin(fstep * TWOPI + pha1) + pha2);
137}
138
139static double modulation_model_chi2 (const gsl_vector *x, void *params);
140
142 const gsl_vector *volts_x, *volts_y;
144
145static cpl_error_code fit_model_modulation (const gsl_vector *vx, const gsl_vector *vy, gsl_vector *Xsolve);
146
147/*-----------------------------------------------------------------------------
148 Function code
149 -----------------------------------------------------------------------------*/
150
151/*----------------------------------------------------------------------------*/
160/*----------------------------------------------------------------------------*/
161cpl_error_code gravi_metrology_demodulate (gravi_data *met_data, cpl_boolean zero_subtracted)
162{
164 cpl_ensure_code(met_data, CPL_ERROR_NULL_INPUT);
165
166 cpl_boolean modulation_active;
167 if (cpl_propertylist_has(gravi_data_get_header(met_data), "ESO INS PMC1 MODULATE")) {
168 modulation_active = cpl_propertylist_get_bool(gravi_data_get_header(met_data), "ESO INS PMC1 MODULATE");
169 } else {
170 modulation_active = 0;
171 cpl_msg_warning (cpl_func, "Can't find modulation keyword, use false");
172 }
173
174 cpl_table *met_data_table = gravi_data_get_table(met_data, GRAVI_METROLOGY_EXT);
175
176 /* Return immediately if demodulation disabled or original metrology data is not modulated */
177 if (modulation_active) {
178 cpl_msg_info (cpl_func, "Demodulate the metrology");
179 if (!zero_subtracted)
180 cpl_msg_info(cpl_func, "Demodulating metrology with no DARK. Using hardcoded diode zero offsets");
181 } else {
182 cpl_msg_info (cpl_func, "Metrology modulation is not enabled");
184 return CPL_ERROR_NONE;
185 }
186
187 /* Duplicate the VOLT column to store a copy of original metrology data */
188 // cpl_table_duplicate_column(met_data_table, "VOLT_RAW", met_data_table, "VOLT");
189
190 cpl_size ndata = cpl_table_get_nrow(met_data_table);
191 cpl_size ncol = cpl_table_get_column_dimension(met_data_table, "VOLT", 0);
192
193 /* Break table into chunks of up to MAX_SECONDS_PER_CHUNK */
194 double exptime = cpl_propertylist_get_double(gravi_data_get_header(met_data), "EXPTIME");
195 cpl_size nchunks = floor(exptime / MAX_SECONDS_PER_CHUNK) + 1;
196 cpl_size seconds_per_chunk = floor(exptime / nchunks);
197 cpl_size chunk_size = STEPS_PER_SECOND * seconds_per_chunk;
198
199 /* Loop over chunks */
200 for (int ichunk = 0; ichunk < nchunks; ichunk++) {
201 /* Start and end indices of chunk, including remainder for last chunk */
202 cpl_size start = ichunk * chunk_size;
203 cpl_size count = (ichunk < nchunks - 1) ? chunk_size : ndata - start;
204 cpl_table * chunk_data = cpl_table_extract(met_data_table, start, count);
205
206 cpl_size nrow = cpl_table_get_nrow(chunk_data);
207 cpl_size nsec = nrow / STEPS_PER_SECOND;
208 cpl_msg_debug(cpl_func, "chunk %d has %lld rows (%lld-%lld)\n", ichunk, nrow, start, start+count);
209 cpl_msg_debug(cpl_func, "seconds in this chunk: %lld\n", nsec);
210 cpl_msg_debug(cpl_func, "leftover portion of a second: %lld\n", nrow % seconds_per_chunk);
211
212 /* Extract all volt data into a GSL matrix */
213 gsl_matrix * volts = gsl_matrix_alloc(nrow, ncol);
214 cpl_table_cast_column(chunk_data, "VOLT", "VOLTf64", CPL_TYPE_DOUBLE);
215 const cpl_array ** volt_arrs = cpl_table_get_data_array_const(chunk_data, "VOLTf64");
216 for (int irow = 0; irow < nrow; irow++) {
217 gsl_vector_const_view row_view = gsl_vector_const_view_array(
218 cpl_array_get_data_double_const(volt_arrs[irow]), ncol
219 );
220 gsl_matrix_set_row(volts, irow, &row_view.vector);
221 }
222 CPLCHECK_INT("Cannot extract the metrology data");
223
224 /* If DARK provided, metrology has already been zeroed by subtracting values from the DARK */
225 /* Otherwise use hardcoded diode_zeros array to subtract offsets */
226 gsl_matrix * volts_zeroed = gsl_matrix_alloc(nrow, ncol);
227 if (zero_subtracted) {
228 gsl_matrix_memcpy(volts_zeroed, volts);
229 } else {
230 /* Shift voltages by zero-offset */
231 gsl_vector_const_view zero_view = gsl_vector_const_view_array(diode_zeros, ncol);
232 for (int i = 0; i < nrow; i++) {
233 gsl_matrix_set_row(volts_zeroed, i, &zero_view.vector);
234 }
235 gsl_matrix_scale(volts_zeroed, -1.0);
236 gsl_matrix_add(volts_zeroed, volts);
237 }
238
239 /* Calculate average over steps of zeroed data */
240 gsl_matrix * volts_zeroed_averaged = gsl_matrix_alloc(STEPS_PER_SECOND, ncol);
241 for (int step = 0; step < STEPS_PER_SECOND; step++) {
242 for (int icol = 0; icol < ncol; icol++) {
243 /* Select the data in the appropriate column for the current step within each second */
244 double *start = volts_zeroed->data + (step * ncol + icol);
245 gsl_vector_const_view step_col_view = gsl_vector_const_view_array_with_stride(
246 start, STEPS_PER_SECOND * ncol, nsec);
247 /* NB: ignores fractional second at the end of last chunk */
248 double mean = gsl_stats_mean(
249 step_col_view.vector.data, step_col_view.vector.stride, step_col_view.vector.size);
250 gsl_matrix_set(volts_zeroed_averaged, step, icol, mean);
251 }
252 }
253
254 /* Fit demodulation model to averaged voltages */
255 gsl_matrix *Xsolve = gsl_matrix_alloc(ntel * ndiode * nside, 4);
256 int isolve = 0;
257 for (int tel = 0; tel < ntel; tel++) {
258 for (int diode = 0; diode < ndiode; diode++) {
259 for (int side = FT; side <= SC; side++) {
260 int ix = diode_column_index(tel, diode, side);
261 int iy = ix + 1;
262 gsl_vector_const_view vx = gsl_matrix_const_column(volts_zeroed_averaged, ix);
263 gsl_vector_const_view vy = gsl_matrix_const_column(volts_zeroed_averaged, iy);
264
265 gsl_vector_view Xsolve_row = gsl_matrix_row(Xsolve, isolve);
266 cpl_error_code status = fit_model_modulation(&vx.vector, &vy.vector, &Xsolve_row.vector);
267 if (status)
268 cpl_error_set(cpl_func, status);
269 isolve++;
270 }
271 }
272 }
273 CPLCHECK_INT("Cannot fit modulation model");
274
275 /* Calculate phase from average */
276 gsl_matrix * volts_phase = gsl_matrix_alloc(STEPS_PER_SECOND, ntel * ndiode * nside);
277 int icol = 0;
278 for (int tel = 0; tel < ntel; tel++) {
279 for (int diode = 0; diode < ndiode; diode++) {
280 for (int side = FT; side <= SC; side++) {
281 double pha2 = gsl_matrix_get(Xsolve, icol, 3);
282 int ix = diode_column_index(tel, diode, side);
283 int iy = ix + 1;
284 gsl_vector_const_view vx = gsl_matrix_const_column(volts_zeroed_averaged, ix);
285 gsl_vector_const_view vy = gsl_matrix_const_column(volts_zeroed_averaged, iy);
286 for (int step = 0; step < STEPS_PER_SECOND; step++) {
287 double phase = atan2(
288 gsl_vector_get(&vy.vector, step), gsl_vector_get(&vx.vector, step));
289 gsl_matrix_set(volts_phase, step, icol, -phase - pha2);
290 }
291 icol++;
292 }
293 }
294 }
295
296 /* Remove modulation component from phase */
297 icol = 0;
298 for (int tel = 0; tel < ntel; tel++) {
299 for (int diode = 0; diode < ndiode; diode++) {
300 for (int side = FT; side <= SC; side++) {
301 int ix = diode_column_index(tel, diode, side);
302 int iy = ix + 1;
303 gsl_vector_view vx = gsl_matrix_column(volts_zeroed, ix);
304 gsl_vector_view vy = gsl_matrix_column(volts_zeroed, iy);
305 for (int step = 0; step < STEPS_PER_SECOND; step++) {
306 double phase = gsl_matrix_get(volts_phase, step, icol);
307 double s = sin(phase), c = cos(phase);
308 int idx = step;
309 /* weird loop structure to correctly handle fractional second at end of chunk */
310 while (idx < nrow) {
311 double vx_orig = gsl_vector_get(&vx.vector, idx);
312 double vy_orig = gsl_vector_get(&vy.vector, idx);
313 double vx_demod = c * vx_orig - s * vy_orig;
314 double vy_demod = s * vx_orig + c * vy_orig;
315 gsl_vector_set(&vx.vector, idx, vx_demod);
316 gsl_vector_set(&vy.vector, idx, vy_demod);
317 idx += STEPS_PER_SECOND;
318 }
319 }
320 icol++;
321 }
322 }
323 }
324
325 /* Write back the demodulated data */
326 cpl_array * cpl_row = cpl_array_new(ncol, CPL_TYPE_FLOAT);
327 for (int irow = 0; irow < nrow; irow++) {
328 gsl_vector_const_view row_view = gsl_matrix_const_row(volts_zeroed, irow);
329 for (cpl_size i = 0; i < ncol; i++) {
330 cpl_array_set_float(cpl_row, i, (float) gsl_vector_get(&row_view.vector, i));
331 }
332 cpl_table_set_array(met_data_table, "VOLT", start + irow, cpl_row);
333 }
334 cpl_array_delete(cpl_row);
335 CPLCHECK_INT("Cannot store demodulated metrology data");
336
337 /* Clean up intermediate values */
338 FREE(gsl_matrix_free, volts_phase);
339 FREE(gsl_matrix_free, volts_zeroed_averaged);
340 FREE(gsl_matrix_free, volts_zeroed);
341 FREE(gsl_matrix_free, Xsolve);
342 FREE(gsl_matrix_free, volts);
343 FREE(cpl_table_delete, chunk_data);
344 } /* End loop over chunks */
345
347 return CPL_ERROR_NONE;
348}
349
358double modulation_model_chi2 (const gsl_vector *X, void *params)
359{
360 model_params *p = params;
361
362 /* model */
363 gsl_vector *model_vx = gsl_vector_alloc(STEPS_PER_SECOND);
364 gsl_vector *model_vy = gsl_vector_alloc(STEPS_PER_SECOND);
365 for (int i = 0; i < STEPS_PER_SECOND; i++) {
366 gsl_vector_set(model_vx, i, model_x(X, i));
367 gsl_vector_set(model_vy, i, model_y(X, i));
368 }
369
370 /* form chi2 */
371 gsl_vector *chi2_x = gsl_vector_alloc(STEPS_PER_SECOND);
372 gsl_vector_memcpy(chi2_x, p->volts_x);
373 gsl_vector_sub(chi2_x, model_vx);
374 gsl_vector_mul(chi2_x, chi2_x);
375
376 gsl_vector *chi2_y = gsl_vector_alloc(STEPS_PER_SECOND);
377 gsl_vector_memcpy(chi2_y, p->volts_y);
378 gsl_vector_sub(chi2_y, model_vy);
379 gsl_vector_mul(chi2_y, chi2_y);
380
381 double chi2 = 0.0;
382 for (int i = 0; i < STEPS_PER_SECOND; i++)
383 chi2 += gsl_vector_get(chi2_x, i) + gsl_vector_get(chi2_y, i);
384
385 gsl_vector_free(model_vx);
386 gsl_vector_free(model_vy);
387 gsl_vector_free(chi2_x);
388 gsl_vector_free(chi2_y);
389
390 return chi2;
391}
392
404cpl_error_code fit_model_modulation (const gsl_vector *vx, const gsl_vector *vy, gsl_vector *Xsolve)
405{
406 cpl_ensure_code(vx, CPL_ERROR_NULL_INPUT);
407 cpl_ensure_code(vy, CPL_ERROR_NULL_INPUT);
408 cpl_ensure_code(Xsolve, CPL_ERROR_NULL_INPUT);
409
410 /* The minimisation has 4 free parameters: a, b, pha1, pha2 */
411 const int dim = 4;
412
413 const int MAX_ITERATIONS = 1000;
414 const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2rand;
415 gsl_multimin_fminimizer *mini = gsl_multimin_fminimizer_alloc(T, dim);
416 int status;
417 double size;
418
419 model_params params = {
420 .volts_x = vx,
421 .volts_y = vy
422 };
423
424 gsl_multimin_function func = {
426 .n = dim,
427 .params = &params
428 };
429
430 double best_chi2 = INFINITY;
431 double a_guess = gsl_stats_sd(vx->data, vx->stride, vx->size);
432 double b_guess = 0.25;
433
434 double phase_range = PI;
435 int nphase = 1;
436 double phase_step = phase_range / (2 * nphase);
437
438 /* Try multiple starting points for minimisation to ensure global minimum */
439 for (int iph1 = -nphase; iph1 < nphase ; iph1++) {
440 for (int iph2 = -nphase; iph2 < nphase; iph2++) {
441 double pha1_guess = iph1 * phase_step;
442 double pha2_guess = iph2 * phase_step;
443
444 double initial_guess[] = {a_guess, b_guess, pha1_guess, pha2_guess};
445 gsl_vector_view guess_view = gsl_vector_view_array(initial_guess, dim);
446 double step_size[] = {0.1, 0.1, 1, 1};
447 gsl_vector_view step_view = gsl_vector_view_array(step_size, dim);
448 gsl_multimin_fminimizer_set(mini, &func, &guess_view.vector, &step_view.vector);
449
450 int iterations = 0;
451 do {
452 iterations++;
453 status = gsl_multimin_fminimizer_iterate(mini);
454
455 /* Error taking optimisation step */
456 if (status)
457 break;
458
459 size = gsl_multimin_fminimizer_size(mini);
460 status = gsl_multimin_test_size(size, 1e-3);
461 } while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);
462
463 if (status == GSL_SUCCESS && mini->fval < best_chi2) {
464 best_chi2 = mini->fval;
465 gsl_vector_memcpy(Xsolve, mini->x);
466 }
467 }
468 }
469
470 /* Normalise fit parameters: a positive, adjust pha2 as required */
471 double a, pha1, pha2;
472 if ((a = gsl_vector_get(Xsolve, 0)) < 0) {
473 gsl_vector_set(Xsolve, 0, -a);
474 pha2 = gsl_vector_get(Xsolve, 3);
475 gsl_vector_set(Xsolve, 3, pha2 + PI);
476 }
477
478 /* wrap phases */
479 if (fabs(pha1 = gsl_vector_get(Xsolve, 2)) > PI)
480 gsl_vector_set(Xsolve, 2, pha1 - SIGN(TWOPI, pha1));
481 if (fabs(pha2 = gsl_vector_get(Xsolve, 3)) > PI)
482 gsl_vector_set(Xsolve, 3, pha2 - SIGN(TWOPI, pha2));
483
484 if (status == GSL_CONTINUE)
485 cpl_msg_warning(cpl_func, "model-fitting did not converge");
486 else if (status != GSL_SUCCESS)
487 cpl_msg_error(cpl_func, "model-fitting failed with error code %d (%s)", status, gsl_strerror(status));
488 else
489 cpl_msg_debug(cpl_func, "converged with best-fit:\na\tb\tpha1\tpha2\n%.4f\t%.4f\t%.4f\t%.4f\n", mini->x->data[0], mini->x->data[1], mini->x->data[2], mini->x->data[3]);
490
491 FREE(gsl_multimin_fminimizer_free, mini);
492 return status ? CPL_ERROR_ILLEGAL_OUTPUT : CPL_ERROR_NONE;
493}
typedefCPL_BEGIN_DECLS struct _gravi_data_ gravi_data
Definition: gravi_data.h:38
#define gravi_data_get_header(data)
Definition: gravi_data.h:74
static cpl_error_code fit_model_modulation(const gsl_vector *vx, const gsl_vector *vy, gsl_vector *Xsolve)
Fit the modulation model to voltage data for a single diode.
static cpl_size diode_column_index(int tele, int diode, int side)
Return column index in VOLT table for specific diode.
#define MAX_SECONDS_PER_CHUNK
const cpl_size ntel
const cpl_size nside
#define TWOPI
static double modulation_model_chi2(const gsl_vector *x, void *params)
Calculate chi^2 statistic for modulation model.
#define STEPS_PER_SECOND
#define PI
const cpl_size ndiode
#define SC
cpl_error_code gravi_metrology_demodulate(gravi_data *met_data, cpl_boolean zero_subtracted)
Demodulate the metrology.
static double model_x(const gsl_vector *X, int step)
const double diode_zeros[]
struct _demodulation_model_params_ model_params
static double model_y(const gsl_vector *X, int step)
#define FT
cpl_msg_debug(cpl_func, "Spectra has <50 pixels -> don't flat")
cpl_msg_info(cpl_func, "Compute WAVE_SCAN for %s", GRAVI_TYPE(type_data))
#define GRAVI_METROLOGY_EXT
Definition: gravi_pfits.h:60
#define CPLCHECK_INT(msg)
Definition: gravi_utils.h:51
#define gravi_msg_function_exit(flag)
Definition: gravi_utils.h:85
#define FREE(function, variable)
Definition: gravi_utils.h:69
#define gravi_msg_function_start(flag)
Definition: gravi_utils.h:84
#define SIGN(a, b)
Definition: gravi_utils.h:153
cpl_table * gravi_data_get_table(gravi_data *self, const char *extname)
Return a pointer on a table extension by its EXTNAME.
Definition: gravi_data.c:2096