GRAVI Pipeline Reference Manual 1.7.2
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
93const double diode_zeros[] = {
94 -0.0039217663072871976, -0.005375368693370768, -0.0039996565553508424, -0.0025503151499257103,
95 -0.004993987370698725, -0.0025982105556817733, -0.0033492901476214598, -0.0024666122995294915,
96 -0.004914056163432626, -0.004605706229646769, -0.0024913748841215717, -0.0025406836901383087,
97 -0.0035045471990661765, -0.004864493604480354, -0.0038971489167063875, -0.002645429318536674,
98 -0.0031186035580824016, -0.0030236901965512925, -0.00455348514695007, -0.0027973126198521715,
99 -0.00329832479577174, -0.004717880979213343, -0.0038105012008050844, -0.0034806513678800706,
100 -0.004843769955531035, -0.0038383890934418244, -0.003413267406206906, -0.0032447329693567305,
101 -0.0019067813627941682, -0.003741090151381262, -0.0031293194990934074, -0.002917132698761452,
102 -0.003846563742841961, -0.0016110031116351089, -0.004055586210967794, -0.004665653836460701,
103 -0.0035651245279117107, -0.003703624441150295, -0.0042027507769680774, -0.003685527333212408,
104 -0.0018717967368672794, -0.004309454414641543, -0.004161530517996172, -0.003603457935318008,
105 -0.0031196452749965883, -0.003456638711821881, -0.005237079636108828, -0.0029119219882156274,
106 -0.002883279151359765, -0.0044843120806118035, -0.0033190561101935586, -0.004244083297006716,
107 -0.005334829455674041, -0.004158489195313494, -0.00242824997808483, -0.0018624941393818074,
108 -0.0029737356896519826, -0.004348274099950943, -0.002654502952395225, -0.0030576540586404475,
109 -0.0032698514683164475, -0.003638017106725761, -0.0029787962274195018, -0.002629800558439812,
110 0.0005056591585058203, 0.0006414248583206548, 0.0005120370824436637, 0.0006245251138490539,
111 0.0002781845526483057, -0.00026739218921257777, 0.0005758898707200935, 0.0003888211264503715,
112 0.00036289398311494447, 6.419787480760673e-05, 0.0008198190512220566, 0.0007714407655275396,
113 0.0008010985218159154, 9.667686713004861e-05, 0.0008396378442216361, 0.0005619108431658763
114};
115
116static double model_x (const gsl_vector *X, int step)
117{
118 double fstep = (1.0 * step) / STEPS_PER_SECOND;
119 double a = gsl_vector_get(X, 0);
120 double b = gsl_vector_get(X, 1);
121 double pha1 = gsl_vector_get(X, 2);
122 double pha2 = gsl_vector_get(X, 3);
123
124 return a * sin(b * sin(fstep * TWOPI + pha1) + pha2);
125}
126
127static double model_y (const gsl_vector *X, int step)
128{
129 double fstep = (1.0 * step) / STEPS_PER_SECOND;
130 double a = gsl_vector_get(X, 0);
131 double b = gsl_vector_get(X, 1);
132 double pha1 = gsl_vector_get(X, 2);
133 double pha2 = gsl_vector_get(X, 3);
134
135 return a * cos(b * sin(fstep * TWOPI + pha1) + pha2);
136}
137
138static double modulation_model_chi2 (const gsl_vector *x, void *params);
139
141 const gsl_vector *volts_x, *volts_y;
143
144static cpl_error_code fit_model_modulation (const gsl_vector *vx, const gsl_vector *vy, gsl_vector *Xsolve);
145
146/*-----------------------------------------------------------------------------
147 Function code
148 -----------------------------------------------------------------------------*/
149
150/*----------------------------------------------------------------------------*/
159/*----------------------------------------------------------------------------*/
160cpl_error_code gravi_metrology_demodulate (gravi_data *met_data, cpl_boolean zero_subtracted)
161{
163 cpl_ensure_code(met_data, CPL_ERROR_NULL_INPUT);
164
165 cpl_boolean modulation_active;
166 if (cpl_propertylist_has(gravi_data_get_header(met_data), "ESO INS PMC1 MODULATE")) {
167 modulation_active = cpl_propertylist_get_bool(gravi_data_get_header(met_data), "ESO INS PMC1 MODULATE");
168 } else {
169 modulation_active = 0;
170 cpl_msg_warning (cpl_func, "Can't find modulation keyword, use false");
171 }
172
173 cpl_table *met_data_table = gravi_data_get_table(met_data, GRAVI_METROLOGY_EXT);
174
175 /* Return immediately if demodulation disabled or original metrology data is not modulated */
176 if (modulation_active) {
177 cpl_msg_info (cpl_func, "Demodulate the metrology");
178 if (!zero_subtracted)
179 cpl_msg_info(cpl_func, "Demodulating metrology with no DARK. Using hardcoded diode zero offsets");
180 } else {
181 cpl_msg_info (cpl_func, "Metrology modulation is not enabled");
183 return CPL_ERROR_NONE;
184 }
185
186 /* Duplicate the VOLT column to store a copy of original metrology data */
187 // cpl_table_duplicate_column(met_data_table, "VOLT_RAW", met_data_table, "VOLT");
188
189 cpl_size ndata = cpl_table_get_nrow(met_data_table);
190 cpl_size ncol = cpl_table_get_column_dimension(met_data_table, "VOLT", 0);
191
192 /* Break table into chunks of up to MAX_SECONDS_PER_CHUNK */
193 double exptime = cpl_propertylist_get_double(gravi_data_get_header(met_data), "EXPTIME");
194 cpl_size nchunks = floor(exptime / MAX_SECONDS_PER_CHUNK) + 1;
195 cpl_size seconds_per_chunk = floor(exptime / nchunks);
196 cpl_size chunk_size = STEPS_PER_SECOND * seconds_per_chunk;
197
198 /* Loop over chunks */
199 for (int ichunk = 0; ichunk < nchunks; ichunk++) {
200 /* Start and end indices of chunk, including remainder for last chunk */
201 cpl_size start = ichunk * chunk_size;
202 cpl_size count = (ichunk < nchunks - 1) ? chunk_size : ndata - start;
203 cpl_table * chunk_data = cpl_table_extract(met_data_table, start, count);
204
205 cpl_size nrow = cpl_table_get_nrow(chunk_data);
206 cpl_size nsec = nrow / STEPS_PER_SECOND;
207 cpl_msg_debug(cpl_func, "chunk %d has %lld rows (%lld-%lld)\n", ichunk, nrow, start, start+count);
208 cpl_msg_debug(cpl_func, "seconds in this chunk: %lld\n", nsec);
209 cpl_msg_debug(cpl_func, "leftover portion of a second: %lld\n", nrow % seconds_per_chunk);
210
211 /* Extract all volt data into a GSL matrix */
212 gsl_matrix * volts = gsl_matrix_alloc(nrow, ncol);
213 cpl_table_cast_column(chunk_data, "VOLT", "VOLTf64", CPL_TYPE_DOUBLE);
214 const cpl_array ** volt_arrs = cpl_table_get_data_array_const(chunk_data, "VOLTf64");
215 for (int irow = 0; irow < nrow; irow++) {
216 gsl_vector_const_view row_view = gsl_vector_const_view_array(
217 cpl_array_get_data_double_const(volt_arrs[irow]), ncol
218 );
219 gsl_matrix_set_row(volts, irow, &row_view.vector);
220 }
221 CPLCHECK_INT("Cannot extract the metrology data");
222
223 /* If DARK provided, metrology has already been zeroed by subtracting values from the DARK */
224 /* Otherwise use hardcoded diode_zeros array to subtract offsets */
225 gsl_matrix * volts_zeroed = gsl_matrix_alloc(nrow, ncol);
226 if (zero_subtracted) {
227 gsl_matrix_memcpy(volts_zeroed, volts);
228 } else {
229 /* Shift voltages by zero-offset */
230 gsl_vector_const_view zero_view = gsl_vector_const_view_array(diode_zeros, ncol);
231 for (int i = 0; i < nrow; i++) {
232 gsl_matrix_set_row(volts_zeroed, i, &zero_view.vector);
233 }
234 gsl_matrix_scale(volts_zeroed, -1.0);
235 gsl_matrix_add(volts_zeroed, volts);
236 }
237
238 /* Calculate average over steps of zeroed data */
239 gsl_matrix * volts_zeroed_averaged = gsl_matrix_alloc(STEPS_PER_SECOND, ncol);
240 for (int step = 0; step < STEPS_PER_SECOND; step++) {
241 for (int icol = 0; icol < ncol; icol++) {
242 /* Select the data in the appropriate column for the current step within each second */
243 double *start = volts_zeroed->data + (step * ncol + icol);
244 gsl_vector_const_view step_col_view = gsl_vector_const_view_array_with_stride(
245 start, STEPS_PER_SECOND * ncol, nsec);
246 /* NB: ignores fractional second at the end of last chunk */
247 double mean = gsl_stats_mean(
248 step_col_view.vector.data, step_col_view.vector.stride, step_col_view.vector.size);
249 gsl_matrix_set(volts_zeroed_averaged, step, icol, mean);
250 }
251 }
252
253 /* Fit demodulation model to averaged voltages */
254 gsl_matrix *Xsolve = gsl_matrix_alloc(ntel * ndiode * nside, 4);
255 int isolve = 0;
256 for (int tel = 0; tel < ntel; tel++) {
257 for (int diode = 0; diode < ndiode; diode++) {
258 for (int side = FT; side <= SC; side++) {
259 int ix = diode_column_index(tel, diode, side);
260 int iy = ix + 1;
261 gsl_vector_const_view vx = gsl_matrix_const_column(volts_zeroed_averaged, ix);
262 gsl_vector_const_view vy = gsl_matrix_const_column(volts_zeroed_averaged, iy);
263
264 gsl_vector_view Xsolve_row = gsl_matrix_row(Xsolve, isolve);
265 cpl_error_code status = fit_model_modulation(&vx.vector, &vy.vector, &Xsolve_row.vector);
266 if (status)
267 cpl_error_set(cpl_func, status);
268 isolve++;
269 }
270 }
271 }
272 CPLCHECK_INT("Cannot fit modulation model");
273
274 /* Calculate phase from average */
275 gsl_matrix * volts_phase = gsl_matrix_alloc(STEPS_PER_SECOND, ntel * ndiode * nside);
276 int icol = 0;
277 for (int tel = 0; tel < ntel; tel++) {
278 for (int diode = 0; diode < ndiode; diode++) {
279 for (int side = FT; side <= SC; side++) {
280 double pha2 = gsl_matrix_get(Xsolve, icol, 3);
281 int ix = diode_column_index(tel, diode, side);
282 int iy = ix + 1;
283 gsl_vector_const_view vx = gsl_matrix_const_column(volts_zeroed_averaged, ix);
284 gsl_vector_const_view vy = gsl_matrix_const_column(volts_zeroed_averaged, iy);
285 for (int step = 0; step < STEPS_PER_SECOND; step++) {
286 double phase = atan2(
287 gsl_vector_get(&vy.vector, step), gsl_vector_get(&vx.vector, step));
288 gsl_matrix_set(volts_phase, step, icol, -phase - pha2);
289 }
290 icol++;
291 }
292 }
293 }
294
295 /* Remove modulation component from phase */
296 icol = 0;
297 for (int tel = 0; tel < ntel; tel++) {
298 for (int diode = 0; diode < ndiode; diode++) {
299 for (int side = FT; side <= SC; side++) {
300 int ix = diode_column_index(tel, diode, side);
301 int iy = ix + 1;
302 gsl_vector_view vx = gsl_matrix_column(volts_zeroed, ix);
303 gsl_vector_view vy = gsl_matrix_column(volts_zeroed, iy);
304 for (int step = 0; step < STEPS_PER_SECOND; step++) {
305 double phase = gsl_matrix_get(volts_phase, step, icol);
306 double s = sin(phase), c = cos(phase);
307 int idx = step;
308 /* weird loop structure to correctly handle fractional second at end of chunk */
309 while (idx < nrow) {
310 double vx_orig = gsl_vector_get(&vx.vector, idx);
311 double vy_orig = gsl_vector_get(&vy.vector, idx);
312 double vx_demod = c * vx_orig - s * vy_orig;
313 double vy_demod = s * vx_orig + c * vy_orig;
314 gsl_vector_set(&vx.vector, idx, vx_demod);
315 gsl_vector_set(&vy.vector, idx, vy_demod);
316 idx += STEPS_PER_SECOND;
317 }
318 }
319 icol++;
320 }
321 }
322 }
323
324 /* Write back the demodulated data */
325 cpl_array * cpl_row = cpl_array_new(ncol, CPL_TYPE_FLOAT);
326 for (int irow = 0; irow < nrow; irow++) {
327 gsl_vector_const_view row_view = gsl_matrix_const_row(volts_zeroed, irow);
328 for (cpl_size i = 0; i < ncol; i++) {
329 cpl_array_set_float(cpl_row, i, (float) gsl_vector_get(&row_view.vector, i));
330 }
331 cpl_table_set_array(met_data_table, "VOLT", start + irow, cpl_row);
332 }
333 cpl_array_delete(cpl_row);
334 CPLCHECK_INT("Cannot store demodulated metrology data");
335
336 /* Clean up intermediate values */
337 FREE(gsl_matrix_free, volts_phase);
338 FREE(gsl_matrix_free, volts_zeroed_averaged);
339 FREE(gsl_matrix_free, volts_zeroed);
340 FREE(gsl_matrix_free, Xsolve);
341 FREE(gsl_matrix_free, volts);
342 FREE(cpl_table_delete, chunk_data);
343 } /* End loop over chunks */
344
346 return CPL_ERROR_NONE;
347}
348
357double modulation_model_chi2 (const gsl_vector *X, void *params)
358{
359 model_params *p = params;
360
361 /* model */
362 gsl_vector *model_vx = gsl_vector_alloc(STEPS_PER_SECOND);
363 gsl_vector *model_vy = gsl_vector_alloc(STEPS_PER_SECOND);
364 for (int i = 0; i < STEPS_PER_SECOND; i++) {
365 gsl_vector_set(model_vx, i, model_x(X, i));
366 gsl_vector_set(model_vy, i, model_y(X, i));
367 }
368
369 /* form chi2 */
370 gsl_vector *chi2_x = gsl_vector_alloc(STEPS_PER_SECOND);
371 gsl_vector_memcpy(chi2_x, p->volts_x);
372 gsl_vector_sub(chi2_x, model_vx);
373 gsl_vector_mul(chi2_x, chi2_x);
374
375 gsl_vector *chi2_y = gsl_vector_alloc(STEPS_PER_SECOND);
376 gsl_vector_memcpy(chi2_y, p->volts_y);
377 gsl_vector_sub(chi2_y, model_vy);
378 gsl_vector_mul(chi2_y, chi2_y);
379
380 double chi2 = 0.0;
381 for (int i = 0; i < STEPS_PER_SECOND; i++)
382 chi2 += gsl_vector_get(chi2_x, i) + gsl_vector_get(chi2_y, i);
383
384 gsl_vector_free(model_vx);
385 gsl_vector_free(model_vy);
386 gsl_vector_free(chi2_x);
387 gsl_vector_free(chi2_y);
388
389 return chi2;
390}
391
401cpl_error_code fit_model_modulation (const gsl_vector *vx, const gsl_vector *vy, gsl_vector *Xsolve)
402{
403 cpl_ensure_code(vx, CPL_ERROR_NULL_INPUT);
404 cpl_ensure_code(vy, CPL_ERROR_NULL_INPUT);
405 cpl_ensure_code(Xsolve, CPL_ERROR_NULL_INPUT);
406
407 /* The minimisation has 4 free parameters: a, b, pha1, pha2 */
408 const int dim = 4;
409
410 const int MAX_ITERATIONS = 1000;
411 const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2rand;
412 gsl_multimin_fminimizer *mini = gsl_multimin_fminimizer_alloc(T, dim);
413 int status, iterations;
414 double size;
415
416 model_params params = {
417 .volts_x = vx,
418 .volts_y = vy
419 };
420
421 gsl_multimin_function func = {
423 .n = dim,
424 .params = &params
425 };
426
427 double best_chi2 = INFINITY;
428 double a_guess = gsl_stats_sd(vx->data, vx->stride, vx->size);
429 double b_guess = 0.25;
430
431 double phase_range = PI;
432 int nphase = 1;
433 double phase_step = phase_range / (2 * nphase);
434
435 /* Try multiple starting points for minimisation to ensure global minimum */
436 for (int iph1 = -nphase; iph1 < nphase; iph1++) {
437 for (int iph2 = -nphase; iph2 < nphase; iph2++) {
438 double pha1_guess = iph1 * phase_step;
439 double pha2_guess = iph2 * phase_step;
440
441 double initial_guess[] = {a_guess, b_guess, pha1_guess, pha2_guess};
442 gsl_vector_view guess_view = gsl_vector_view_array(initial_guess, dim);
443 double step_size[] = {0.1, 0.1, 1, 1};
444 gsl_vector_view step_view = gsl_vector_view_array(step_size, dim);
445 gsl_multimin_fminimizer_set(mini, &func, &guess_view.vector, &step_view.vector);
446
447 iterations = 0;
448 do {
449 iterations++;
450 status = gsl_multimin_fminimizer_iterate(mini);
451
452 /* Error taking optimisation step */
453 if (status)
454 break;
455
456 size = gsl_multimin_fminimizer_size(mini);
457 status = gsl_multimin_test_size(size, 1e-3);
458 } while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);
459
460 if (status == GSL_SUCCESS && mini->fval < best_chi2) {
461 best_chi2 = mini->fval;
462 gsl_vector_memcpy(Xsolve, mini->x);
463 }
464 }
465 }
466
467 /* Normalise fit parameters: a positive, adjust pha2 as required */
468 double a, pha1, pha2;
469 if ((a = gsl_vector_get(Xsolve, 0)) < 0) {
470 gsl_vector_set(Xsolve, 0, -a);
471 pha2 = gsl_vector_get(Xsolve, 3);
472 gsl_vector_set(Xsolve, 3, pha2 + PI);
473 }
474
475 /* wrap phases */
476 if (fabs(pha1 = gsl_vector_get(Xsolve, 2)) > PI)
477 gsl_vector_set(Xsolve, 2, pha1 - SIGN(TWOPI, pha1));
478 if (fabs(pha2 = gsl_vector_get(Xsolve, 3)) > PI)
479 gsl_vector_set(Xsolve, 3, pha2 - SIGN(TWOPI, pha2));
480
481 if (status == GSL_CONTINUE)
482 cpl_msg_warning(cpl_func, "model-fitting did not converge");
483 else if (status != GSL_SUCCESS)
484 cpl_msg_error(cpl_func, "model-fitting failed with error code %d (%s)", status, gsl_strerror(status));
485 else
486 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]);
487
488 FREE(gsl_multimin_fminimizer_free, mini);
489 return status ? CPL_ERROR_ILLEGAL_OUTPUT : CPL_ERROR_NONE;
490}
typedefCPL_BEGIN_DECLS struct _gravi_data_ gravi_data
Definition: gravi_data.h:39
#define gravi_data_get_header(data)
Definition: gravi_data.h:75
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:154
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